--- /srv/reproducible-results/rbuild-debian/r-b-build.XPAjlJvg/b1/gtsam_4.2.0+dfsg-1_amd64.changes +++ /srv/reproducible-results/rbuild-debian/r-b-build.XPAjlJvg/b2/gtsam_4.2.0+dfsg-1_amd64.changes ├── Files │ @@ -1,7 +1,7 @@ │ │ 1bd507037c4dc3d69d06f84524205802 362400 libdevel optional libgtsam-dev_4.2.0+dfsg-1_amd64.deb │ - e81a300b13e483893074b764c8505572 14608736 doc optional libgtsam-doc_4.2.0+dfsg-1_all.deb │ + fc06ad5f680d310f7b779cccf91a81a8 14607420 doc optional libgtsam-doc_4.2.0+dfsg-1_all.deb │ 0b1478212f077d679bef12907f107ec4 155193888 debug optional libgtsam4-dbgsym_4.2.0+dfsg-1_amd64.deb │ 6d1750e8cbdb95cf319b061aaad1178b 1577408 libs optional libgtsam4_4.2.0+dfsg-1_amd64.deb │ 7c73eceebd3192de6e867c5da21d0ec3 6319352 debug optional python3-gtsam-dbgsym_4.2.0+dfsg-1_amd64.deb │ 2a067e2ab7d1a4cda588f8876073151c 5910816 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 75932 2024-01-09 05:51:35.000000 control.tar.xz │ │ --rw-r--r-- 0 0 0 14532612 2024-01-09 05:51:35.000000 data.tar.xz │ │ +-rw-r--r-- 0 0 0 76020 2024-01-09 05:51:35.000000 control.tar.xz │ │ +-rw-r--r-- 0 0 0 14531208 2024-01-09 05:51:35.000000 data.tar.xz │ ├── control.tar.xz │ │ ├── control.tar │ │ │ ├── ./md5sums │ │ │ │ ├── ./md5sums │ │ │ │ │┄ Files differ │ │ │ │ ├── line order │ │ │ │ │ @@ -125,121 +125,121 @@ │ │ │ │ │ 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.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00002_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00005.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00005.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/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.js │ │ │ │ │ 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/a00059.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00059_source.html │ │ │ │ │ +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.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00065.js │ │ │ │ │ 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.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00077.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00077_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00080.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00080.js │ │ │ │ │ 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_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_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_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_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00125.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.js │ │ │ │ │ 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 │ │ │ │ │ @@ -249,833 +249,833 @@ │ │ │ │ │ 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/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/a00179.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00179_source.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/a00191.js │ │ │ │ │ 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/a00200.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00200.js │ │ │ │ │ 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/a00209.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ │ 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/a00218_source.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/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_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/a00254_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00260.html │ │ │ │ │ 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/a00266.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00266_source.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/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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00281.js │ │ │ │ │ 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/a00320.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00320_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00323.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00323_source.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/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/a00347_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00350.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00350.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00350_source.html │ │ │ │ │ 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/a00362_source.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/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.html │ │ │ │ │ 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/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/a00413.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00413.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00413_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00416.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00416.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00416_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00419.js │ │ │ │ │ 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/a00428.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00428.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00428_source.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/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/a00440.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00440.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00443.js │ │ │ │ │ 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/a00452.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00452.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00452_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/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/a00473.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00476.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00476.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00473.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00473_source.html │ │ │ │ │ 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_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00491.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00491_source.html │ │ │ │ │ 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/a00506_source.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/a00515_source.html │ │ │ │ │ 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/a00521_source.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/a00530.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00530_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00533.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/a00542.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00545.js │ │ │ │ │ +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/a00569.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00572.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00572.js │ │ │ │ │ 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_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.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00593_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00596.html │ │ │ │ │ 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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00608.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00608.js │ │ │ │ │ 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/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_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/a00629.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00629_source.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.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00647_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00650.html │ │ │ │ │ +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_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00659.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00659.js │ │ │ │ │ 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_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/a00680.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00680.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00677.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00677_source.html │ │ │ │ │ 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/a00692.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00689.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00695.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00695.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00698.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00698_source.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.js │ │ │ │ │ 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/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_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_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00734.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00737.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00734_source.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/a00740.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00740_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00746.js │ │ │ │ │ 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/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/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/a00779.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00785.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00785_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/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/a00800.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00800_source.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/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/a00809.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00809_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/a00818.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00818_source.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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00824.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00827_source.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/a00839.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/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.js │ │ │ │ │ +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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00878.js │ │ │ │ │ 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/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/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/a00914.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00911.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00911_source.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/a00917.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00917_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/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/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/a00950.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00953.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00953_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00956.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00956.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00956_source.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/a00962.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00965.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00968.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00968_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00971.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_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/a01001.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01001_source.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/a01007.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01007.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01007_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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01016.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/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/a01046.html │ │ │ │ │ +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/a01049_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01052.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01052_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.js │ │ │ │ │ +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/a01085.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01085.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01082.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │ │ 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/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.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01097.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01097_source.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/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/a01109.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01112.js │ │ │ │ │ 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/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/a01133.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01133_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01136.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/a01148.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01148.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01151.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/a01151.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01151_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/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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01190.js │ │ │ │ │ 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.js │ │ │ │ │ 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/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/a01235.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01235.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01232.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01232.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01238.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01238_source.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/a01247_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01250.js │ │ │ │ │ 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/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_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01268.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01268.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ │ 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.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01277.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01277_source.html │ │ │ │ │ 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.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01286.js │ │ │ │ │ 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/a01289.js │ │ │ │ │ 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/a01307.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01307.js │ │ │ │ │ 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/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_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_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01328.js │ │ │ │ │ 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.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.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01340.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01340.js │ │ │ │ │ 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/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/a01358_source.html │ │ │ │ │ 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/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/a01376.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/a01382_source.html │ │ │ │ │ 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/a01394.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01394_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/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) 75566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/Hybrid.pdf │ │ │ │ --rw-r--r-- 0 root (0) root (0) 148548 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/ImuFactor.pdf │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 148558 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/ImuFactor.pdf │ │ │ │ -rw-r--r-- 0 root (0) root (0) 39726 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) 6757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17409 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00005.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00005.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66871 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00005_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2086 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 57397 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7079 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 91 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00017.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56603 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2508 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 192234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9190 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 250 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12942 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 52828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00032.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16112 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00035_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00038.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00038_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) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 125 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00053.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1345 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00053.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8418 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 343 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10629 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00059.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00059.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00059_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17818 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1016 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15649 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 556 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6657 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27843 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6884 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9315 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13624 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00077.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00077.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39982 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) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00086_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6608 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82782 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18897 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6209 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25066 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24215 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1480 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 51312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7913 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9399 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12509 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) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8401 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 3446 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77745 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00122_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7344 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 52200 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00128_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6548 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00131.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00131.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20753 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00131_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5941 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00137.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9727 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00143.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35570 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00143_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7983 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 48110 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54641 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) 13635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 803 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13159 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) 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) 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) 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) 7285 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22085 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8041 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24383 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) 10021 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00179.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00179.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00179_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32573 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8783 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 289 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185_source.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) 7720 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00191.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 184 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00191.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6594 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00197.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 50616 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5540 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00203.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5592 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13686 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ +-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) 9286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 309 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6642 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00218.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18784 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00218_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6434 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19109 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00227.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6998 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6738 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44827 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6278 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239.html │ │ │ │ +-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) 6320 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15576 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5555 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7580 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29980 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00254.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00254.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13987 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00254_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 206584 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5552 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00263.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00266.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00266.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25959 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00266_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00269.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12074 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 444 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 69958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6302 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00275.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6858 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19855 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7283 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7321 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) 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) 21198 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 68835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7039 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32402 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21252 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5655 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6953 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00320.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 174 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00320.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103910 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00320_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11986 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00323.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00323_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6957 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34458 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 392 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5538 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00335.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00338.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7269 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 211 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9756 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 352 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26504 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7340 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32342 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00350.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00350.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80368 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00350_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42494 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6350 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00356.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 127405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 51201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00362_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7206 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 189 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42743 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71861 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23382 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4906 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00374.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6379 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00377.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5688 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00389.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31329 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00389_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 457 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 61746 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) 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) 7319 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33928 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6253 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00410.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7527 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00413.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00413.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00413_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6883 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00416.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00416.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00416_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00419.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00425.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00428.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00428.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32907 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00428_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00431.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) 29089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00437_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11287 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00440.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00440.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 327 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6284 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8992 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24792 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00452.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00452.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00452_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5656 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00455.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00461.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00473.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 539 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00473.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22880 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00473_source.html │ │ │ │ -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) 5521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6903 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00485.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00485.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6769 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6737 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18804 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6587 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8586 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 270 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41431 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6506 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00506.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00506_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5524 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00509.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) 6908 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 58194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00521.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00521_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8025 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5713 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00530.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00530.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00530_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5531 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00533.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42027 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00542.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00542.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6690 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00545.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 57767 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00545_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00548.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9935 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7797 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36848 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00569.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 240 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13267 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5671 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 229 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 221 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6879 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) 38328 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8617 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00593.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 96 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00593.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19774 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) 5606 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66585 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00608.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00608.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00608_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00614.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00614_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5529 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00617.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00620.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) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00629.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00629.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00629_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 90312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00632_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46729 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) 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) 7717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00644.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7205 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00647.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 166 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00647.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00647_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00650_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6476 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00659.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00659.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38682 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00659_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15849 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30059 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5593 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17977 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 113625 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10019 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31790 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6436 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6367 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 70 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00689.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18421 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23512 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6363 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00698.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00698_source.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) 7445 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35988 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00710.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12032 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7999 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 276 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 313 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73410 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13381 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5627 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28869 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00734_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) 6141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00740.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00740.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15664 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00740_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9091 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 185 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7323 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 116 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16423 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00752.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17930 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00752_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00755.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37272 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00758_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 160 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00767.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 670 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5557 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7195 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776_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) 7433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15960 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 891 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 125878 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00791.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6366 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 67 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13881 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10979 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 453 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6428 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00800.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00800.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00800_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7058 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38060 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35496 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6387 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13406 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00818.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00818.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00818_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00821.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00824.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00827_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00830.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7995 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 216 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73746 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5725 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5691 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00839.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00842_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12972 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8332 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 192 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00851.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) 6663 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6901 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 69400 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7443 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33932 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00875.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00875.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8297 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26183 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8957 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 60246 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5643 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00890.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6842 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 40634 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6359 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24129 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49274 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7763 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00917.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00917.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38413 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00917_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8826 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 55825 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6963 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00929.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00929.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41393 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5559 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00935.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7069 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34621 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32900 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00944_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9760 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 59601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11831 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00950.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6921 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00953.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37965 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00953_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6414 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00956.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00956.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00956_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5612 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6864 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00962.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5660 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00965.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7321 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 197 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00968.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33342 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00968_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5669 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00971.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00977.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9996 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6390 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00983.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24874 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00983_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00989.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 47156 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) 6611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01001.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01001.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13022 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01001_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10339 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 493 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 50078 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01007.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01007.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32517 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01007_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25127 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11271 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 275 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88722 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01016.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8813 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 60117 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 155 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35912 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 137 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17991 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01028.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7249 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6866 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28417 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 693 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99817 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6735 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24143 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) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94502 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6511 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01052.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17610 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01052_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6497 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01058.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01058.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01058_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37468 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 919 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 61984 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5544 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10777 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01076.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 161 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 74370 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01088.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5537 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01091.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8050 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37487 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01097.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01097.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01097_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7515 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5765 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24596 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01109.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 150 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01112.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01112_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103846 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8787 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 68014 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5778 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30718 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23599 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5542 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01130.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83353 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01133_source.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) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01142.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 43820 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 75095 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7005 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54857 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5672 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 118 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172.js │ │ │ │ -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) 9294 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 108 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6896 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193_source.html │ │ │ │ -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) 10834 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15064 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01202.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) 14404 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37621 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) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01217.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 266 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 75097 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226_source.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) 12714 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01232.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01232.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6847 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01238.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 47234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01238_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42102 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01244_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27308 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44752 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 260 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 70927 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) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01256.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24742 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01256_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27374 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19122 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01271_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18772 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6829 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 123331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6954 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56560 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5751 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20551 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01286.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01286.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01286_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30917 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01289.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01289.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6936 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 106 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36516 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6450 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99463 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18044 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01298_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17845 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13824 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5540 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01304.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7033 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01307.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01307.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 118926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01307_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6459 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 53045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01316_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19703 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22253 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01322_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77347 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01325_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7064 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) 8061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01331.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01331.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01331_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34893 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01334_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01340.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01340.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37474 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01340_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6870 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28867 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01349.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37514 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01361.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370_source.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) 5554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01376.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8301 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) 18569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01382_source.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) 7075 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 232 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 45377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388_source.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) 4933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01394.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7579 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01394_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01400.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18761 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01400_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01406.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) 204471 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) 13750 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex0.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11013 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) 9826 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) 9394 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 │ │ │ │ │ -June 4, 2025 │ │ │ │ │ +May 2, 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/ProductLieGroup.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Functions
│ │ │ │ -
VectorSerialization.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
ProductLieGroup.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

serialization for Vectors │ │ │ │ +

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

│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ -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)
 

│ │ │ │ +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::traits< ProductLieGroup< G, H > >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Namespaces

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

Detailed Description

│ │ │ │ -

serialization for Vectors

│ │ │ │ +

Group product of two Lie Groups.

│ │ │ │ +
Date
May, 2015
│ │ │ │
Author
Frank Dellaert
│ │ │ │ -
Date
February 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,30 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_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) │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, gtsam::Vector &v, unsigned int) │ │ │ │ │ +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_ _>_ _> │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const Eigen::Matrix< double, D, │ │ │ │ │ - 1 > &v, unsigned int) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< double, D, 1 > │ │ │ │ │ - &v, unsigned int) │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -serialization for Vectors │ │ │ │ │ +Group product of two Lie Groups. │ │ │ │ │ + Date │ │ │ │ │ + May, 2015 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - February 2022 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00002_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSerialization.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ProductLieGroup.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,81 +98,193 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VectorSerialization.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/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
│ │ │ │ -
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}
│ │ │ │ +
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
│ │ │ │ -
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}
│ │ │ │ +
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
│ │ │ │ -
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)
│ │ │ │ -
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
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
│ │ │ │ +
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
│ │ │ │ +
Base class and basic functions for Lie types.
│ │ │ │ +
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ +
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition ProductLieGroup.h:29
│ │ │ │ +
ProductLieGroup()
Default constructor yields identity.
Definition ProductLieGroup.h:40
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,77 +1,213 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VectorSerialization.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_/_V_e_c_t_o_r_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27namespace boost { │ │ │ │ │ -28namespace serialization { │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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 │ │ │ │ │ - * _V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._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/a00005.html │ │ │ │ @@ -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/base/TestableAssertions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,55 +95,104 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
debug.cpp File Reference
│ │ │ │ +Functions
│ │ │ │ +
TestableAssertions.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Global debugging flags. │ │ │ │ +

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

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

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

│ │ │ │ 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
 
│ │ │ │ +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

│ │ │ │ -

Global debugging flags.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 1, 2011
│ │ │ │ +

Provides additional testing facilities for common data structures.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,90 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const _K_e_y &expected, const _K_e_y &actual, double │ │ │ │ │ + tol=0.0) │ │ │ │ │ +  Equals testing for basic types. │ │ │ │ │   │ │ │ │ │ -void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const boost::optional< V > │ │ │ │ │ + &actual, double tol=1e-9) │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Global debugging flags. │ │ │ │ │ +Provides additional testing facilities for common data structures. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 1, 2011 │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._c_p_p │ │ │ │ │ + * _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/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/serialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,166 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
cholesky.h 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
│ │ │ │ -
Date
Nov 5, 2010
│ │ │ │ +

Convenience functions for serializing data structures via boost.serialization.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │ +
│ │ │ │ +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.h 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 │ │ │ │ │ Date │ │ │ │ │ - Nov 5, 2010 │ │ │ │ │ + Feb 7, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ + * _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/a00008.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,26 @@ │ │ │ │ │ var a00008 = [ │ │ │ │ │ - ["choleskyCareful", "a00008.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ - ["choleskyPartial", "a00008.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ + ["deserialize", "a00008.html#aa4d516d0d98946341ad2766521011d1b", null], │ │ │ │ │ + ["deserializeBinary", "a00008.html#a56c41958e57a4bc7361ab593011462fd", null], │ │ │ │ │ + ["deserializeFromBinaryFile", "a00008.html#aefeb77f2324e4bbfca2c8152c49a4565", null], │ │ │ │ │ + ["deserializeFromBinaryStream", "a00008.html#a8eb16192d8ebe2532d8a69c9f2bf15b2", null], │ │ │ │ │ + ["deserializeFromBinaryString", "a00008.html#a349b7ed4a67a8c3d80737e5383507bc4", null], │ │ │ │ │ + ["deserializeFromFile", "a00008.html#adbb4a71187706918faca3a11519b62f6", null], │ │ │ │ │ + ["deserializeFromStream", "a00008.html#ab181d2a3de466fc0c87bd29501bbc7ab", null], │ │ │ │ │ + ["deserializeFromString", "a00008.html#a262487af458d1f48dd7d3bc549caae00", null], │ │ │ │ │ + ["deserializeFromXMLFile", "a00008.html#a0d8faf2bb0a908ff484677bb2fcf707c", null], │ │ │ │ │ + ["deserializeFromXMLStream", "a00008.html#a8d7ea177ef3e93af15a4a77c2bd7fa8b", null], │ │ │ │ │ + ["deserializeFromXMLString", "a00008.html#a5f2fe2bfc17737ff9ae1fb8b210604f0", null], │ │ │ │ │ + ["deserializeXML", "a00008.html#a85ab51b2e8f4cea69f72e06a5c136e85", null], │ │ │ │ │ + ["serialize", "a00008.html#a332bad7d1e70d3c04ceca35adce7a134", null], │ │ │ │ │ + ["serializeBinary", "a00008.html#abb476c3e77878f7e0f70dd51871f48e7", null], │ │ │ │ │ + ["serializeToBinaryFile", "a00008.html#abdff0a4b4fdf0e609aa62fb8d8b1dbba", null], │ │ │ │ │ + ["serializeToBinaryStream", "a00008.html#aa5d177a3bf16f9f5acf45ee62c816479", null], │ │ │ │ │ + ["serializeToBinaryString", "a00008.html#a7fad959f63f2eac1478c5ede7b9ae4db", null], │ │ │ │ │ + ["serializeToFile", "a00008.html#aa3261c2d455f892dae1921f8c3a39267", null], │ │ │ │ │ + ["serializeToStream", "a00008.html#a007080ca4741cc3d3d6a209b0ae3fe61", null], │ │ │ │ │ + ["serializeToString", "a00008.html#aa12c78301a9d2968afe5a1a14baa7e90", null], │ │ │ │ │ + ["serializeToXMLFile", "a00008.html#a89449123a746480a1d668cecb5576708", null], │ │ │ │ │ + ["serializeToXMLStream", "a00008.html#ae2e939177cf58b42d7ed4c9856d536c5", null], │ │ │ │ │ + ["serializeToXMLString", "a00008.html#a16c154fe5e35242e26af8286e77eea11", null], │ │ │ │ │ + ["serializeXML", "a00008.html#a976ff7999fcc13ef602b6ece1b078bca", 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/serialization.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,48 +98,319 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
cholesky.h
│ │ │ │ +
serialization.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>
│ │ │ │ +
20#pragma once
│ │ │ │
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
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
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 {
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ +
void serializeToStream(const T &input, std::ostream &out_archive_stream)
Definition serialization.h:64
│ │ │ │ +
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ +
std::string serializeToXMLString(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:146
│ │ │ │ +
void deserializeFromString(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:86
│ │ │ │ +
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
void deserializeFromBinaryString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:229
│ │ │ │ +
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ +
void deserializeFromXMLString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:155
│ │ │ │ +
std::string serializeToBinaryString(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:220
│ │ │ │ +
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ +
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ +
void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in XML
Definition serialization.h:138
│ │ │ │ +
void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in binary
Definition serialization.h:212
│ │ │ │ +
std::string serializeXML(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:185
│ │ │ │ +
std::string serializeToString(const T &input)
serializes to a string
Definition serialization.h:78
│ │ │ │ +
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ +
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ +
void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:204
│ │ │ │ +
void deserializeFromStream(std::istream &in_archive_stream, T &output)
deserializes from a stream
Definition serialization.h:71
│ │ │ │ +
std::string serializeBinary(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:259
│ │ │ │ +
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ +
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ +
void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:130
│ │ │ │ +
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,53 +1,349 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -cholesky.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ 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#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 { │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ + * _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/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/DSFVector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,137 +95,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
Lie.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
DSFVector.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ 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...
class  gtsam::DSFBase
 A fast implementation of disjoint set forests that uses vector as underly data structure. 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...
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.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -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 >
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 >
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 >
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.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class and basic functions for Lie types.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Alex Cunningham
│ │ │ │ -
│ │ │ │ -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

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

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

│ │ │ │ +
Date
Jun 25, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,101 +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 | _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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_o_ _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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ -  │ │ │ │ │ -#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; │ │ │ │ │ -  │ │ │ │ │ -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. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ +A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ + Date │ │ │ │ │ + Jun 25, 2010 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - 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 │ │ │ │ │ + Kai Ni │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _L_i_e_._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/a00011_source.html │ │ │ │ @@ -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/base/DSFVector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,382 +98,96 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Lie.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)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
24#pragma once
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/dllexport.h>
│ │ │ │ + │ │ │ │ +
23
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │
25
│ │ │ │ -
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 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 }
│ │ │ │ +
26#include <vector>
│ │ │ │ +
27#include <set>
│ │ │ │ +
28#include <map>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT DSFBase {
│ │ │ │ +
39
│ │ │ │ +
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
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ -
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 }
│ │ │ │ +
57 void merge(const size_t& i1, const size_t& i2);
│ │ │ │ +
58};
│ │ │ │
│ │ │ │ -
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> {
│ │ │ │ - │ │ │ │ -
176
│ │ │ │ - │ │ │ │ -
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(
│ │ │ │ - │ │ │ │ -
345}
│ │ │ │ -
│ │ │ │ -
346
│ │ │ │ -
351template<class T>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>;
│ │ │ │ -
Concept check class for variable types with Group properties.
│ │ │ │ -
Base class and basic functions for Manifold types.
│ │ │ │ +
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}
│ │ │ │ +
Included from all GTSAM files.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
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
│ │ │ │ -
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition Lie.h:246
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition Lie.h:252
│ │ │ │ -
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ -
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ -
Group Concept.
Definition Group.h:46
│ │ │ │ -
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition Lie.h:121
│ │ │ │ -
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ -
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition Lie.h:89
│ │ │ │ -
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition Lie.h:152
│ │ │ │ -
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
│ │ │ │ -
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition Lie.h:100
│ │ │ │ -
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition Lie.h:116
│ │ │ │ -
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition Lie.h:141
│ │ │ │ -
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition Lie.h:126
│ │ │ │ -
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition Lie.h:131
│ │ │ │ -
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
│ │ │ │ -
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ -
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ -
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ -
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ -
Functor for transforming covariance of T.
Definition Lie.h:353
│ │ │ │ -
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ -
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ -
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
A fast implementation of disjoint set forests that uses vector as underly data structure.
Definition DSFVector.h:38
│ │ │ │ +
std::vector< size_t > V
Vector of ints.
Definition DSFVector.h:41
│ │ │ │ +
DSFVector additionally keeps a vector of keys to support more expensive operations.
Definition DSFVector.h:64
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,440 +1,97 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Lie.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) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -24#pragma once │ │ │ │ │ +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 <_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 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 } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -_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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _L_i_e_._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/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/types.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,103 +96,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
TestableAssertions.h File Reference
│ │ │ │ +
types.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Functions for handling type information. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

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

│ │ │ │ Namespaces

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

│ │ │ │ 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)
 
│ │ │ │ -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.
 
std::string gtsam::demangle (const char *name)
 Pretty print Value type name.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Provides additional testing facilities for common data structures.

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

Functions for handling type information.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
Date
May 18, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,90 +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 │ │ │ │ │ -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_. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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) │ │ │ │ │ -  │ │ │ │ │ -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. │ │ │ │ │ +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 ********** │ │ │ │ │ -Provides additional testing facilities for common data structures. │ │ │ │ │ +Functions for handling type information. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + May 18, 2020 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00014.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,14 +1,3 @@ │ │ │ │ │ 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] │ │ │ │ │ + ["demangle", "a00014.html#a0b97fc96e5f69236e81489c66d5b92ba", null] │ │ │ │ │ ]; │ │ │ ├── ./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/debug.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
timing.cpp File Reference
│ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
debug.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Timing utilities. │ │ │ │ +

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

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

│ │ │ │ Namespaces

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

│ │ │ │ Functions

│ │ │ │ -GTSAM_EXPORT boost::shared_ptr< TimingOutlinegtsam::internal::gTimingRoot (new TimingOutline("Total", getTicTocID("Total")))
 
│ │ │ │ -GTSAM_EXPORT boost::weak_ptr< TimingOutlinegtsam::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)
 
│ │ │ │ +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

│ │ │ │ -

Timing utilities.

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

Global debugging flags.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 1, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -timing.cpp 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 | _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 │ │ │ │ │ -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"))) │ │ │ │ │ +bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │   │ │ │ │ │ - GTSAM_EXPORT boost::weak_ptr< ggttssaamm::::iinntteerrnnaall::::ggCCuurrrreennttTTiimmeerr │ │ │ │ │ - _T_i_m_i_n_g_O_u_t_l_i_n_e >  (gTimingRoot) │ │ │ │ │ +void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │   │ │ │ │ │ - size_t  ggttssaamm::::iinntteerrnnaall::::ggeettTTiiccTTooccIIDD (const char │ │ │ │ │ - *descriptionC) │ │ │ │ │ +bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │   │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttiicc (size_t id, const char │ │ │ │ │ - *labelC) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttoocc (size_t id, const char │ │ │ │ │ - *label) │ │ │ │ │ +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 │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Timing utilities. │ │ │ │ │ +Global debugging flags. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts, Michael Kaess │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 5, 2010 │ │ │ │ │ + Feb 1, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_i_m_i_n_g_._c_p_p │ │ │ │ │ + * _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/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/timing.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,211 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
OptionalJacobian.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
timing.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

Timing utilities. │ │ │ │ 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::internal::TimingOutline
 Timing Entry, arranged in a tree. 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...
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)
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +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< TimingOutlinegtsam::internal::gTimingRoot
 
│ │ │ │ +GTSAM_EXTERN_EXPORT boost::weak_ptr< TimingOutlinegtsam::internal::gCurrentTimer
 
│ │ │ │

Detailed Description

│ │ │ │ -

Special class for optional Jacobian arguments.

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

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)
│ │ │ │ +
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);
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +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 │ │ │ │ │ -OptionalJacobian.h File Reference │ │ │ │ │ -Special class for optional Jacobian arguments. _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 │ │ │ │ │ - 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_._._. │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ +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 │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ +Timing utilities. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Natesh Srinivasan │ │ │ │ │ + Richard Roberts, Michael Kaess │ │ │ │ │ Date │ │ │ │ │ - Nov 28, 2014 │ │ │ │ │ + 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); │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00020.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,4 @@ │ │ │ │ │ var a00020 = [ │ │ │ │ │ - ["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] │ │ │ │ │ + ["gtsam::internal::TimingOutline", "a02516.html", "a02516"], │ │ │ │ │ + ["gtsam::internal::AutoTicToc", "a02520.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/timing.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,292 +98,328 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
OptionalJacobian.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#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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
47 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
│ │ │ │ -
48
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
71 map_(nullptr) {
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
76 map_(nullptr) {
│ │ │ │ -
77 usurp(fixed.data());
│ │ │ │ -
78 }
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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) { ?? }
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
172 if (*this)
│ │ │ │ -
173 return OptionalJacobian<Rows, N>(&map_(0,startCol));
│ │ │ │ -
174 else
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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_;
│ │ │ │ + │ │ │ │ + │ │ │ │ +
163
│ │ │ │ +
164#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ +
165 boost::timer::cpu_timer timer_;
│ │ │ │ +
166#else
│ │ │ │ +
167 boost::timer timer_;
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
251 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
│ │ │ │ -
252};
│ │ │ │ -
│ │ │ │ -
253
│ │ │ │ -
260template<class T, class A>
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
264};
│ │ │ │ -
│ │ │ │ +
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} // namespace gtsam
│ │ │ │ -
267
│ │ │ │ +
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}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
Typedefs for easier changing of types.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:127
│ │ │ │ -
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:122
│ │ │ │ -
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:88
│ │ │ │ -
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:148
│ │ │ │ -
OptionalJacobian()
Default constructor acts like boost::none.
Definition OptionalJacobian.h:70
│ │ │ │ -
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition OptionalJacobian.h:75
│ │ │ │ -
OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
Constructor from an Eigen::Ref value.
Definition OptionalJacobian.h:106
│ │ │ │ -
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:95
│ │ │ │ -
Eigen::Map< Jacobian > * operator->()
operator->()
Definition OptionalJacobian.h:153
│ │ │ │ -
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
│ │ │ │ -
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition OptionalJacobian.h:81
│ │ │ │ -
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
│ │ │ │ -
OptionalJacobian()
View on constructor argument, if given.
Definition OptionalJacobian.h:202
│ │ │ │ -
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:220
│ │ │ │ -
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition OptionalJacobian.h:193
│ │ │ │ -
Jacobian & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:233
│ │ │ │ -
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:215
│ │ │ │ -
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition OptionalJacobian.h:207
│ │ │ │ -
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition OptionalJacobian.h:210
│ │ │ │ -
Jacobian * operator->()
TODO: operator->()
Definition OptionalJacobian.h:238
│ │ │ │ -
: meta-function to generate Jacobian
Definition OptionalJacobian.h:250
│ │ │ │ -
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ +
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
Timing Entry, arranged in a tree.
Definition timing.h:145
│ │ │ │ +
double t2_
cache the
Definition timing.h:150
│ │ │ │ +
ChildMap children_
subtrees
Definition timing.h:162
│ │ │ │ +
double max() const
max time, in seconds
Definition timing.h:183
│ │ │ │ +
double wall() const
wall time, in seconds
Definition timing.h:181
│ │ │ │ +
double secs() const
time taken, in seconds, including children
Definition timing.h:179
│ │ │ │ +
double self() const
self time only, in seconds
Definition timing.h:180
│ │ │ │ +
boost::weak_ptr< TimingOutline > parent_
parent pointer
Definition timing.h:160
│ │ │ │ +
double min() const
min time, in seconds
Definition timing.h:182
│ │ │ │ +
double mean() const
mean self time, in seconds
Definition timing.h:184
│ │ │ │ +
GTSAM_EXPORT size_t time() const
time taken, including children
Definition timing.cpp:65
│ │ │ │ +
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │ +
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,321 +1,394 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -OptionalJacobian.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#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 │ │ │ │ │ -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 │ │ │ │ │ -_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) { ?? } │ │ │ │ │ +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 │ │ │ │ │ +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_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 } │ │ │ │ │ +_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 │ │ │ │ │ -_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}; │ │ │ │ │ +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} // namespace gtsam │ │ │ │ │ -267 │ │ │ │ │ +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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._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/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/numericalDerivative.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,306 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
cholesky.cpp File Reference
│ │ │ │ +
numericalDerivative.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

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

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].
 
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

│ │ │ │ -

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

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Nov 5, 2010
│ │ │ │ +

Some functions to compute numerical derivatives.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 │ │ │ │ │ -cholesky.cpp 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 | _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 │ │ │ │ │ -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]. │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ +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 ********** │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. │ │ │ │ │ +Some functions to compute numerical derivatives. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Nov 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._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/a00023.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,31 @@ │ │ │ │ │ var a00023 = [ │ │ │ │ │ - ["choleskyCareful", "a00023.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ - ["choleskyPartial", "a00023.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ + ["gtsam::internal::FixedSizeMatrix< Y, X >", "a02440.html", null], │ │ │ │ │ + ["gtsam::G_x1< X1, X2 >", "a02444.html", null], │ │ │ │ │ + ["numericalDerivative11", "a00023.html#a581a9f8db043590096fbac682b8f6a93", null], │ │ │ │ │ + ["numericalDerivative11", "a00023.html#a4c4c5744f0b348d4ec6c047f796e7924", null], │ │ │ │ │ + ["numericalDerivative21", "a00023.html#a157eb3b44dd4e38bbd66f3cc68a20e43", null], │ │ │ │ │ + ["numericalDerivative21", "a00023.html#afcc94e3b49906e824c01a3d56df62d33", null], │ │ │ │ │ + ["numericalDerivative22", "a00023.html#a5b0db4ac5f279b12fc91fab4cb40b1bf", null], │ │ │ │ │ + ["numericalDerivative22", "a00023.html#a46d002a0529f1172aebb7e1d85d5c08c", null], │ │ │ │ │ + ["numericalDerivative31", "a00023.html#aa8f0264edaf78d503809c1e3152340ee", null], │ │ │ │ │ + ["numericalDerivative32", "a00023.html#acfbadc0ab0cd9386205b2737c5d0b727", null], │ │ │ │ │ + ["numericalDerivative33", "a00023.html#a526a422ea611724345affed8b71d9bac", null], │ │ │ │ │ + ["numericalDerivative41", "a00023.html#a3b0937e08e79604ee6ab10fca6ffe65e", null], │ │ │ │ │ + ["numericalDerivative42", "a00023.html#af40f61e8eff01acdb028ad33b8f3529b", null], │ │ │ │ │ + ["numericalDerivative43", "a00023.html#a6846b5cc84c1d5a54a436121b27057e0", null], │ │ │ │ │ + ["numericalDerivative44", "a00023.html#a8061b20199f839b115512d95d058fcbf", null], │ │ │ │ │ + ["numericalDerivative51", "a00023.html#a272af6f7139b76d085279060646e32d1", null], │ │ │ │ │ + ["numericalDerivative52", "a00023.html#a722e98babba795ce480f89527b572251", null], │ │ │ │ │ + ["numericalDerivative53", "a00023.html#acc2f741132522a6ce57d544a27a95855", null], │ │ │ │ │ + ["numericalDerivative54", "a00023.html#a2cf5181fc7bca1ef6a1070959de7b975", null], │ │ │ │ │ + ["numericalDerivative55", "a00023.html#a5ca42a2fc87373c005299f3087995a97", null], │ │ │ │ │ + ["numericalDerivative61", "a00023.html#a0ca8a22de666bc39015da34f150df748", null], │ │ │ │ │ + ["numericalDerivative62", "a00023.html#ac224e2384e41171181b2dbc0f700cdc1", null], │ │ │ │ │ + ["numericalDerivative63", "a00023.html#a15c46aa999886d4ccd06ab73fc02db31", null], │ │ │ │ │ + ["numericalDerivative64", "a00023.html#a3eb3fef9c77920dfd9496e71a0bcf669", null], │ │ │ │ │ + ["numericalDerivative65", "a00023.html#a799c83517a20a8e0b0b014f26b50f660", null], │ │ │ │ │ + ["numericalDerivative66", "a00023.html#a4e010b7f436124b9b421ba7dc438d987", null], │ │ │ │ │ + ["numericalGradient", "a00023.html#afd8862271510196ec4e2d894f623942d", null], │ │ │ │ │ + ["numericalHessian", "a00023.html#a11011d8e9bd69e486c9752875f710c18", null], │ │ │ │ │ + ["numericalHessian311", "a00023.html#a10295660c0177e08d45812fbb7e4c554", 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/make_shared.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,94 +94,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
MatrixSerialization.h File Reference
│ │ │ │ +
make_shared.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Serialization for matrices. │ │ │ │ +

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

│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ +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<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)
 
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

│ │ │ │ -

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

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

make_shared trampoline function to ensure proper alignment

│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,59 +1,47 @@ │ │ │ │ │ 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 | _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_. │ │ │ │ │ -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) │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, Eigen::Matrix< Scalar_, │ │ │ │ │ - Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version) │ │ │ │ │ +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 │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, gtsam::Matrix &m, const │ │ │ │ │ - unsigned int version) │ │ │ │ │ +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 ********** │ │ │ │ │ -Serialization for matrices. │ │ │ │ │ +make_shared trampoline function to ensure proper alignment │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - 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 │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00026.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00026 = [ │ │ │ │ │ - ["save", "a00026.html#a068f1f8df2afd609744bc82386259e71", null] │ │ │ │ │ + ["enable_if_t", "a00026.html#a47e06cd29f4e8a20a8a7842e4045845f", null], │ │ │ │ │ + ["make_shared", "a00026.html#abcb9ece2bd206193ac40f118f1815c0e", null], │ │ │ │ │ + ["make_shared", "a00026.html#a6f805b32c5544e5552d702d5e2b4e801", null] │ │ │ │ │ ]; │ │ │ ├── ./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/make_shared.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,68 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MatrixSerialization.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// \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
│ │ │ │ -
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
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
58 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
│ │ │ │ +
59 }
│ │ │ │ +
│ │ │ │ +
60
│ │ │ │ +
62 template<typename T, typename ... Args>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
64 return boost::make_shared<T>(std::forward<Args>(args)...);
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
67}
│ │ │ │ +
Typedefs for easier changing of types.
│ │ │ │ +
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
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::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,82 +1,71 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MatrixSerialization.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// \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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ +_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._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/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/VerticalBlockMatrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,73 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
DSFMap.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
VerticalBlockMatrix.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Allow for arbitrary type in DSF. │ │ │ │ +

A matrix with column blocks of pre-defined sizes. │ │ │ │ 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...
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.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

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

│ │ │ │ -Functions

│ │ │ │ -IndexPairVector gtsam::IndexPairSetAsArray (IndexPairSet &set)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Allow for arbitrary type in DSF.

│ │ │ │ -
Date
Oct 26, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

A matrix with column blocks of pre-defined sizes.

│ │ │ │ +

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +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 │ │ │ │ │ -DSFMap.h File Reference │ │ │ │ │ -Allow for arbitrary type in DSF. _M_o_r_e_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_o_ _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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ -IndexPairVector  ggttssaamm::::IInnddeexxPPaaiirrSSeettAAssAArrrraayy (IndexPairSet &set) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Allow for arbitrary type in DSF. │ │ │ │ │ - Date │ │ │ │ │ - Oct 26, 2013 │ │ │ │ │ +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 │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_M_a_p_._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/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/VerticalBlockMatrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,161 +98,252 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DSFMap.h
│ │ │ │ +
VerticalBlockMatrix.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>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Matrix.h>
│ │ │ │ + │ │ │ │ + │ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ -
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 }
│ │ │ │ +
26 // Forward declarations
│ │ │ │ +
27 class SymmetricBlockMatrix;
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
42 class GTSAM_EXPORT VerticalBlockMatrix
│ │ │ │ +
43 {
│ │ │ │ +
44 public:
│ │ │ │ + │ │ │ │ +
46 typedef Eigen::Block<Matrix> Block;
│ │ │ │ +
47 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │ +
48
│ │ │ │ +
49 protected:
│ │ │ │ +
50 Matrix matrix_;
│ │ │ │ + │ │ │ │ +
52
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
61 rowStart_(0), rowEnd_(0), blockStart_(0)
│ │ │ │ +
62 {
│ │ │ │ +
63 variableColOffsets_.push_back(0);
│ │ │ │ +
64 assertInvariants();
│ │ │ │ +
65 }
│ │ │ │
│ │ │ │ -
67
│ │ │ │ +
66
│ │ │ │ +
68 template<typename CONTAINER>
│ │ │ │
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ -
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
│ │ │ │ +
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}
│ │ │ │ +
Serialization for matrices.
│ │ │ │ +
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Disjoint set forest using an STL map data structure underneath Uses rank compression and union by ran...
Definition DSFMap.h:34
│ │ │ │ -
iterator find__(const KEY &key) const
Given key, find iterator to initial entry.
Definition DSFMap.h:48
│ │ │ │ -
iterator find_(const iterator &it) const
Given iterator to initial entry, find the root Entry.
Definition DSFMap.h:61
│ │ │ │ -
iterator find_(const KEY &key) const
Given key, find the root Entry.
Definition DSFMap.h:69
│ │ │ │ -
std::map< KEY, Set > sets() const
return all sets, i.e. a partition of all elements
Definition DSFMap.h:105
│ │ │ │ -
DSFMap()
constructor
Definition DSFMap.h:78
│ │ │ │ -
KEY find(const KEY &key) const
Given key, find the representative key for the set in which it lives.
Definition DSFMap.h:81
│ │ │ │ -
void merge(const KEY &x, const KEY &y)
Merge two sets.
Definition DSFMap.h:87
│ │ │ │ -
We store the forest in an STL map, but parents are done with pointers.
Definition DSFMap.h:37
│ │ │ │ -
Small utility class for representing a wrappable pairs of ints.
Definition DSFMap.h:117
│ │ │ │ +
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
VerticalBlockMatrix()
Construct an empty VerticalBlockMatrix.
Definition VerticalBlockMatrix.h:60
│ │ │ │ +
Block full()
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:157
│ │ │ │ +
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
│ │ │ │ +
const DenseIndex & firstBlock() const
Get the apparent first block for all operations.
Definition VerticalBlockMatrix.h:182
│ │ │ │ +
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ +
VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block.
Definition VerticalBlockMatrix.h:69
│ │ │ │ +
DenseIndex rowEnd_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:54
│ │ │ │ +
const DenseIndex & rowEnd() const
Get the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:176
│ │ │ │ +
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ +
Block operator()(DenseIndex block)
Access a single block in the underlying matrix with read/write access.
Definition VerticalBlockMatrix.h:124
│ │ │ │ +
DenseIndex & firstBlock()
Get or set the apparent first block for all operations.
Definition VerticalBlockMatrix.h:185
│ │ │ │ +
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ +
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition VerticalBlockMatrix.h:93
│ │ │ │ +
const constBlock operator()(DenseIndex block) const
Access a const block view.
Definition VerticalBlockMatrix.h:127
│ │ │ │ +
const DenseIndex & rowStart() const
Get the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:170
│ │ │ │ +
Matrix matrix_
The full matrix.
Definition VerticalBlockMatrix.h:50
│ │ │ │ +
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
│ │ │ │ +
const constBlock full() const
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:160
│ │ │ │ +
DenseIndex & rowStart()
Get or set the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:173
│ │ │ │ +
Matrix & matrix()
Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:191
│ │ │ │ +
DenseIndex & rowEnd()
Get or set the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:179
│ │ │ │ +
DenseIndex rowStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:53
│ │ │ │ +
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
│ │ │ │ +
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:55
│ │ │ │ +
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition VerticalBlockMatrix.h:51
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,166 +1,346 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DSFMap.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 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include // Provides size_t │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ +26 // Forward declarations │ │ │ │ │ +27 class SymmetricBlockMatrix; │ │ │ │ │ +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. │ │ │ │ │ +_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_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_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 │ │ │ │ │ - * _D_S_F_M_a_p_._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/a00032.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/numericalDerivative.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,306 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
numericalDerivative.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
DSFVector.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ 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

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

│ │ │ │ -

Some functions to compute numerical derivatives.

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

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

│ │ │ │ +
Date
Jun 25, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,516 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -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 ********** │ │ │ │ │ -Some functions to compute numerical derivatives. │ │ │ │ │ +a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ + Date │ │ │ │ │ + Jun 25, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Kai Ni │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00035_source.html │ │ │ │ @@ -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/base/chartTesting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,160 +98,95 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ThreadsafeException.h
│ │ │ │ +
chartTesting.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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
53
│ │ │ │ -
54protected:
│ │ │ │ -
55 bool dynamic_;
│ │ │ │ -
56 mutable boost::optional<String> description_;
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file chartTesting.h
│ │ │ │ +
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>
│ │ │ │ +
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().
│ │ │ │ +
30template<typename T>
│ │ │ │ +
31void testDefaultChart(TestResult& result_,
│ │ │ │ +
32 const std::string& name_,
│ │ │ │ +
33 const T& value) {
│ │ │ │ +
34
│ │ │ │ +
35 GTSAM_CONCEPT_TESTABLE_TYPE(T)
│ │ │ │ +
36
│ │ │ │ +
37 typedef typename gtsam::DefaultChart<T> Chart;
│ │ │ │ +
38 typedef typename Chart::vector Vector;
│ │ │ │ +
39
│ │ │ │ +
40 // First, check the basic chart concept. This checks that the interface is satisfied.
│ │ │ │ +
41 // The rest of the function is even more detailed, checking the correctness of the chart.
│ │ │ │ +
42 BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
│ │ │ │ +
43
│ │ │ │ +
44 T other = value;
│ │ │ │ +
45
│ │ │ │ +
46 // Check that the dimension of the local value matches the chart dimension.
│ │ │ │ +
47 Vector dx = Chart::local(value, other);
│ │ │ │ +
48 EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size());
│ │ │ │ +
49 // And that the "local" of a value vs. itself is zero.
│ │ │ │ +
50 EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size()))));
│ │ │ │ +
51
│ │ │ │ +
52 // Test the invertibility of retract/local
│ │ │ │ +
53 dx.setRandom();
│ │ │ │ +
54 T updated = Chart::retract(value, dx);
│ │ │ │ +
55 Vector invdx = Chart::local(value, updated);
│ │ │ │ +
56 EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
│ │ │ │
57
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
60 dynamic_(false) {
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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) :
│ │ │ │ - │ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91};
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
94class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
│ │ │ │ -
95public:
│ │ │ │ -
│ │ │ │ -
97 OutOfRangeThreadsafe(const std::string& description) :
│ │ │ │ - │ │ │ │ -
99 }
│ │ │ │ -
│ │ │ │ -
100};
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
104 InvalidArgumentThreadsafe> {
│ │ │ │ -
105public:
│ │ │ │ -
│ │ │ │ -
107 InvalidArgumentThreadsafe(const std::string& description) :
│ │ │ │ - │ │ │ │ -
109 }
│ │ │ │ -
│ │ │ │ -
110};
│ │ │ │ -
│ │ │ │ -
111
│ │ │ │ -
│ │ │ │ -
113class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
│ │ │ │ -
114{
│ │ │ │ -
115public:
│ │ │ │ -
116 CholeskyFailed() noexcept {}
│ │ │ │ -
117 ~CholeskyFailed() noexcept override {}
│ │ │ │ -
118};
│ │ │ │ -
│ │ │ │ -
119
│ │ │ │ -
120} // namespace gtsam
│ │ │ │ +
58 // And test that negative steps work as well.
│ │ │ │ +
59 dx = -dx;
│ │ │ │ +
60 updated = Chart::retract(value, dx);
│ │ │ │ +
61 invdx = Chart::local(value, updated);
│ │ │ │ +
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); }
│ │ │ │ +
Concept check for values that can be used in unit tests.
│ │ │ │ +
Base class and basic functions for Manifold types.
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │ -
~ThreadsafeException() noexcept override
Default destructor doesn't have the noexcept.
Definition ThreadsafeException.h:75
│ │ │ │ -
bool dynamic_
Whether this object was moved.
Definition ThreadsafeException.h:55
│ │ │ │ -
ThreadsafeException()
Default constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:59
│ │ │ │ -
ThreadsafeException(const ThreadsafeException &other)
Copy constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:64
│ │ │ │ -
ThreadsafeException(const std::string &description)
Construct with description string.
Definition ThreadsafeException.h:69
│ │ │ │ -
boost::optional< String > description_
Optional description.
Definition ThreadsafeException.h:56
│ │ │ │ -
Thread-safe runtime error exception.
Definition ThreadsafeException.h:85
│ │ │ │ -
RuntimeErrorThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:88
│ │ │ │ -
Thread-safe out of range exception.
Definition ThreadsafeException.h:94
│ │ │ │ -
OutOfRangeThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:97
│ │ │ │ -
Thread-safe invalid argument exception.
Definition ThreadsafeException.h:104
│ │ │ │ -
InvalidArgumentThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:107
│ │ │ │ -
Indicate Cholesky factorization failure.
Definition ThreadsafeException.h:114
│ │ │ │ +
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,169 +1,95 @@ │ │ │ │ │ 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_. │ │ │ │ │ +chartTesting.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 │ │ │ │ │ -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 │ │ │ │ │ -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__; │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file chartTesting.h │ │ │ │ │ +14 * @brief │ │ │ │ │ +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_/_M_a_t_r_i_x_._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_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +_2_8namespace _g_t_s_a_m { │ │ │ │ │ +29// Do a full concept check and test the invertibility of local() vs. retract │ │ │ │ │ +(). │ │ │ │ │ +30template │ │ │ │ │ +31void testDefaultChart(TestResult& result_, │ │ │ │ │ +32 const std::string& name_, │ │ │ │ │ +33 const T& value) { │ │ │ │ │ +34 │ │ │ │ │ +35 GTSAM_CONCEPT_TESTABLE_TYPE(T) │ │ │ │ │ +36 │ │ │ │ │ +37 typedef typename gtsam::DefaultChart Chart; │ │ │ │ │ +38 typedef typename Chart::vector Vector; │ │ │ │ │ +39 │ │ │ │ │ +40 // First, check the basic chart concept. This checks that the interface is │ │ │ │ │ +satisfied. │ │ │ │ │ +41 // The rest of the function is even more detailed, checking the correctness │ │ │ │ │ +of the chart. │ │ │ │ │ +42 BOOST_CONCEPT_ASSERT((ChartConcept)); │ │ │ │ │ +43 │ │ │ │ │ +44 T other = value; │ │ │ │ │ +45 │ │ │ │ │ +46 // Check that the dimension of the local value matches the chart dimension. │ │ │ │ │ +47 Vector dx = Chart::local(value, other); │ │ │ │ │ +48 EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); │ │ │ │ │ +49 // And that the "local" of a value vs. itself is zero. │ │ │ │ │ +50 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); │ │ │ │ │ +51 │ │ │ │ │ +52 // Test the invertibility of retract/local │ │ │ │ │ +53 dx.setRandom(); │ │ │ │ │ +54 T updated = Chart::retract(value, dx); │ │ │ │ │ +55 Vector invdx = Chart::local(value, updated); │ │ │ │ │ +56 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(invdx), 1e-9)); │ │ │ │ │ 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}; │ │ │ │ │ -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 │ │ │ │ │ +58 // And test that negative steps work as well. │ │ │ │ │ +59 dx = -dx; │ │ │ │ │ +60 updated = Chart::retract(value, dx); │ │ │ │ │ +61 invdx = Chart::local(value, updated); │ │ │ │ │ +62 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(invdx), 1e-9)); │ │ │ │ │ +63} │ │ │ │ │ +64} // namespace gtsam │ │ │ │ │ +65 │ │ │ │ │ +68#define CHECK_CHART_CONCEPT(value) \ │ │ │ │ │ +69 { gtsam::testDefaultChart(result_, name_, value); } │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ - * _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ + * cchhaarrttTTeessttiinngg..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/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/debug.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,86 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
testLie.h
│ │ │ │ +
debug.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>
│ │ │ │ - │ │ │ │ +
19#include <gtsam/base/FastMap.h>
│ │ │ │ + │ │ │ │ +
21#include <string>
│ │ │ │ +
22
│ │ │ │
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));
│ │ │ │ +
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
│ │ │ │ -
44 EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
│ │ │ │ -
45 EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
│ │ │ │ +
44namespace gtsam {
│ │ │ │ +
45 GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
│ │ │ │
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 // 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}
│ │ │ │ +
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// 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) {
│ │ │ │ +
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 Matrix H1, H2;
│ │ │ │ -
64 typedef traits<G> T;
│ │ │ │ -
65 typedef typename T::TangentVector V;
│ │ │ │ -
66 typedef OptionalJacobian<T::dimension,T::dimension> OJ;
│ │ │ │ +
63#else
│ │ │ │ +
64
│ │ │ │ +
65#define ISDEBUG(S) (false)
│ │ │ │ +
66#define SETDEBUG(S,V) ((void)false)
│ │ │ │
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); }
│ │ │ │ -
Base class and basic functions for Lie types.
│ │ │ │ -
Some functions to compute numerical derivatives.
│ │ │ │ +
68#endif
│ │ │ │ +
69
│ │ │ │ +
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
Included from all GTSAM files.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
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,119 +1,83 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -testLie.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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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)); │ │ │ │ │ +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 │ │ │ │ │ -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)); │ │ │ │ │ +44namespace _g_t_s_a_m { │ │ │ │ │ +45 GTSAM_EXTERN_EXPORT FastMap > │ │ │ │ │ +debugFlags; │ │ │ │ │ 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 // 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} │ │ │ │ │ +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// 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) { │ │ │ │ │ +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 Matrix H1, H2; │ │ │ │ │ -64 typedef traits T; │ │ │ │ │ -65 typedef typename T::TangentVector V; │ │ │ │ │ -66 typedef OptionalJacobian OJ; │ │ │ │ │ +63#else │ │ │ │ │ +64 │ │ │ │ │ +65#define ISDEBUG(S) (false) │ │ │ │ │ +66#define SETDEBUG(S,V) ((void)false) │ │ │ │ │ 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. │ │ │ │ │ +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_:_:_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 │ │ │ │ │ - * tteessttLLiiee..hh │ │ │ │ │ + * _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/a00044_source.html │ │ │ │ @@ -117,17 +117,17 @@ │ │ │ │
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>
│ │ │ │ - │ │ │ │ -
24#include <gtsam/inference/Key.h>
│ │ │ │ +
22#include <gtsam/base/FastList.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>
│ │ │ │ @@ -285,17 +285,17 @@ │ │ │ │
222 DepthFirstForest(forest, str, visitor);
│ │ │ │
223}
│ │ │ │ │ │ │ │
224} // namespace treeTraversal
│ │ │ │ │ │ │ │
225
│ │ │ │
226} // namespace gtsam
│ │ │ │ -
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ +
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ + │ │ │ │
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition Matrix.cpp:397
│ │ │ │
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
│ │ │ │
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
│ │ │ │
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
│ │ │ │
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
│ │ │ ├── ./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/FastMap.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,59 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
make_shared.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
FastMap.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ 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...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ 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::map that uses boost's fast_pool_allocator.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 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 │ │ │ │ │ +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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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::map that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 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_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/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/FastMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,68 +98,91 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
make_shared.h
│ │ │ │ +
FastMap.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>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ + │ │ │ │ +
22#include <boost/serialization/nvp.hpp>
│ │ │ │ +
23#include <boost/serialization/map.hpp>
│ │ │ │ +
24#include <map>
│ │ │ │
25
│ │ │ │ -
26#include <type_traits>
│ │ │ │ +
26namespace gtsam {
│ │ │ │
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
58 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
│ │ │ │ -
59 }
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
62 template<typename T, typename ... Args>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
64 return boost::make_shared<T>(std::forward<Args>(args)...);
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
67}
│ │ │ │ -
Typedefs for easier changing of types.
│ │ │ │ +
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>,
│ │ │ │ + │ │ │ │ +
44
│ │ │ │ + │ │ │ │ +
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 }
│ │ │ │ +
│ │ │ │ +
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:
│ │ │ │ + │ │ │ │ +
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}
│ │ │ │ +
An easy way to control which allocator is used for Fast* collections.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
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::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
│ │ │ │ +
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ +
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
FastMap(const Base &x)
Copy constructor from the base map class.
Definition FastMap.h:56
│ │ │ │ +
FastMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastMap.h:50
│ │ │ │ +
bool insert2(const KEY &key, const VALUE &val)
Handy 'insert' function for Matlab wrapper.
Definition FastMap.h:64
│ │ │ │ +
FastMap(const FastMap< KEY, VALUE > &x)
Copy constructor from another FastMap.
Definition FastMap.h:53
│ │ │ │ +
FastMap()
Default constructor.
Definition FastMap.h:46
│ │ │ │ +
bool exists(const KEY &e) const
Handy 'exists' function.
Definition FastMap.h:67
│ │ │ │ +
friend class boost::serialization::access
Serialization function.
Definition FastMap.h:71
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,71 +1,112 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -make_shared.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 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 │ │ │ │ │ +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 │ │ │ │ │ -26#include │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 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. │ │ │ │ │ +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 │ │ │ │ │ +_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 } │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ + * _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/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/DSFMap.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,114 +94,74 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
serializationTestHelpers.h File Reference
│ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
DSFMap.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< IndexPairgtsam::IndexPairVector
 
│ │ │ │ +typedef std::set< IndexPairgtsam::IndexPairSet
 
│ │ │ │ +typedef std::map< IndexPair, IndexPairSet > gtsam::IndexPairSetMap
 
│ │ │ │ +typedef DSFMap< IndexPairgtsam::DSFMapIndexPair
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Functions

│ │ │ │ -template<class 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
 
│ │ │ │ +IndexPairVector gtsam::IndexPairSetAsArray (IndexPairSet &set)
 
│ │ │ │

Detailed Description

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

Allow for arbitrary type in DSF.

│ │ │ │ +
Date
Oct 26, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,92 +1,48 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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_:_:_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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - T  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::ccrreeaattee () │ │ │ │ │ +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 │ │ │ │ │   │ │ │ │ │ -boost::filesystem::path  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rreesseettFFiilleessyysstteemm │ │ │ │ │ - (boost::filesystem::path folder="actual") │ │ │ │ │ +typedef std::map< _I_n_d_e_x_P_a_i_r, IndexPairSet >  ggttssaamm::::IInnddeexxPPaaiirrSSeettMMaapp │ │ │ │ │   │ │ │ │ │ -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()) │ │ │ │ │ + typedef _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -const bool  vveerrbboossee = false │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +IndexPairVector  ggttssaamm::::IInnddeexxPPaaiirrSSeettAAssAArrrraayy (IndexPairSet &set) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Allow for arbitrary type in DSF. │ │ │ │ │ Date │ │ │ │ │ - Feb 7, 2012 │ │ │ │ │ + Oct 26, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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_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/a00050_source.html │ │ │ │ @@ -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/base/DSFMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,202 +98,161 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
serializationTestHelpers.h
│ │ │ │ +
DSFMap.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 <iostream>
│ │ │ │ -
23#include <sstream>
│ │ │ │ -
24#include <string>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <cstdlib> // Provides size_t
│ │ │ │ +
22#include <map>
│ │ │ │ +
23#include <set>
│ │ │ │ +
24#include <vector>
│ │ │ │
25
│ │ │ │ - │ │ │ │ +
26namespace gtsam {
│ │ │ │
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;
│ │ │ │ -
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}
│ │ │ │ +
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 }
│ │ │ │ +
│ │ │ │
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}
│ │ │ │ +
│ │ │ │ +
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
│ │ │ │ -
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}
│ │ │ │ +
│ │ │ │ +
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
│ │ │ │ -
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}
│ │ │ │ +
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// 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
│ │ │ │ -
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ -
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ -
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ -
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ -
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ -
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ -
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ -
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ -
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ -
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │ +
Disjoint set forest using an STL map data structure underneath Uses rank compression and union by ran...
Definition DSFMap.h:34
│ │ │ │ +
iterator find__(const KEY &key) const
Given key, find iterator to initial entry.
Definition DSFMap.h:48
│ │ │ │ +
iterator find_(const iterator &it) const
Given iterator to initial entry, find the root Entry.
Definition DSFMap.h:61
│ │ │ │ +
iterator find_(const KEY &key) const
Given key, find the root Entry.
Definition DSFMap.h:69
│ │ │ │ +
std::map< KEY, Set > sets() const
return all sets, i.e. a partition of all elements
Definition DSFMap.h:105
│ │ │ │ +
DSFMap()
constructor
Definition DSFMap.h:78
│ │ │ │ +
KEY find(const KEY &key) const
Given key, find the representative key for the set in which it lives.
Definition DSFMap.h:81
│ │ │ │ +
void merge(const KEY &x, const KEY &y)
Merge two sets.
Definition DSFMap.h:87
│ │ │ │ +
We store the forest in an STL map, but parents are done with pointers.
Definition DSFMap.h:37
│ │ │ │ +
Small utility class for representing a wrappable pairs of ints.
Definition DSFMap.h:117
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,239 +1,166 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -serializationTestHelpers.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 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include // Provides size_t │ │ │ │ │ +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> │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -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} │ │ │ │ │ +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 } │ │ │ │ │ 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} │ │ │ │ │ +_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 │ │ │ │ │ -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} │ │ │ │ │ +_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 │ │ │ │ │ -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} │ │ │ │ │ +_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// 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. │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ * _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_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/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/ThreadsafeException.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,48 +95,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros
│ │ │ │ -
FastDefaultAllocator.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ThreadsafeException.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

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

│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ Classes

struct  gtsam::internal::FastDefaultAllocator< T >
 Default allocator for list, map, and set types. More...
class  gtsam::ThreadsafeException< DERIVED >
 Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. More...
 
struct  gtsam::internal::FastDefaultVectorAllocator< T >
 Default allocator for vector types (we never use boost pool for vectors) 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

│ │ │ │ -

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

│ │ │ │ +

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

│ │ │ │
Author
Richard Roberts
│ │ │ │ -
Date
Aug 15, 2013
│ │ │ │ +
Date
Aug 21, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 | _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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_o_ _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_._._. │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Aug 15, 2013 │ │ │ │ │ + Aug 21, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00056.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,7 @@ │ │ │ │ │ var a00056 = [ │ │ │ │ │ - ["gtsam::internal::FastDefaultAllocator< T >", "a02304.html", null], │ │ │ │ │ - ["gtsam::internal::FastDefaultVectorAllocator< T >", "a02308.html", null] │ │ │ │ │ + ["gtsam::ThreadsafeException< DERIVED >", "a02496.html", "a02496"], │ │ │ │ │ + ["gtsam::RuntimeErrorThreadsafe", "a02500.html", "a02500"], │ │ │ │ │ + ["gtsam::OutOfRangeThreadsafe", "a02504.html", "a02504"], │ │ │ │ │ + ["gtsam::InvalidArgumentThreadsafe", "a02508.html", "a02508"], │ │ │ │ │ + ["gtsam::CholeskyFailed", "a02512.html", 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/ThreadsafeException.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,106 +98,160 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastDefaultAllocator.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#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
20#pragma once
│ │ │ │
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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 <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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
53
│ │ │ │ +
54protected:
│ │ │ │ +
55 bool dynamic_;
│ │ │ │ +
56 mutable boost::optional<String> description_;
│ │ │ │ +
57
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
60 dynamic_(false) {
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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) :
│ │ │ │ + │ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91};
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
94class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
│ │ │ │ +
95public:
│ │ │ │ +
│ │ │ │ +
97 OutOfRangeThreadsafe(const std::string& description) :
│ │ │ │ + │ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100};
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
104 InvalidArgumentThreadsafe> {
│ │ │ │ +
105public:
│ │ │ │ +
│ │ │ │ +
107 InvalidArgumentThreadsafe(const std::string& description) :
│ │ │ │ + │ │ │ │ +
109 }
│ │ │ │ +
│ │ │ │ +
110};
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
│ │ │ │ +
113class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
│ │ │ │ +
114{
│ │ │ │ +
115public:
│ │ │ │ +
116 CholeskyFailed() noexcept {}
│ │ │ │ +
117 ~CholeskyFailed() noexcept override {}
│ │ │ │ +
118};
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
120} // namespace gtsam
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ -
Default allocator for vector types (we never use boost pool for vectors)
Definition FastDefaultAllocator.h:72
│ │ │ │ +
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │ +
~ThreadsafeException() noexcept override
Default destructor doesn't have the noexcept.
Definition ThreadsafeException.h:75
│ │ │ │ +
bool dynamic_
Whether this object was moved.
Definition ThreadsafeException.h:55
│ │ │ │ +
ThreadsafeException()
Default constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:59
│ │ │ │ +
ThreadsafeException(const ThreadsafeException &other)
Copy constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:64
│ │ │ │ +
ThreadsafeException(const std::string &description)
Construct with description string.
Definition ThreadsafeException.h:69
│ │ │ │ +
boost::optional< String > description_
Optional description.
Definition ThreadsafeException.h:56
│ │ │ │ +
Thread-safe runtime error exception.
Definition ThreadsafeException.h:85
│ │ │ │ +
RuntimeErrorThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:88
│ │ │ │ +
Thread-safe out of range exception.
Definition ThreadsafeException.h:94
│ │ │ │ +
OutOfRangeThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:97
│ │ │ │ +
Thread-safe invalid argument exception.
Definition ThreadsafeException.h:104
│ │ │ │ +
InvalidArgumentThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:107
│ │ │ │ +
Indicate Cholesky factorization failure.
Definition ThreadsafeException.h:114
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,102 +1,169 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastDefaultAllocator.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#include // Configuration from CMake │ │ │ │ │ +20#pragma once │ │ │ │ │ 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 // 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 │ │ │ │ │ +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}; │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_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/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/MatrixSerialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,94 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
VerticalBlockMatrix.cpp 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.

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

│ │ │ │ -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,23 +1,59 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +_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_. │ │ │ │ │ +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) │ │ │ │ │   │ │ │ │ │ ********** 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_._c_p_p │ │ │ │ │ + * _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/a00065.html │ │ │ │ @@ -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/base/Testable.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,186 +94,123 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
Vector.h File Reference
│ │ │ │ +
Testable.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Concept check for values that can be used in unit tests. │ │ │ │ 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...
 
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 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
 
│ │ │ │ +#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

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
 
│ │ │ │ +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

│ │ │ │ -

typedef and functions to augment Eigen's VectorXd

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

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_MAKE_VECTOR_DEFS

│ │ │ │ + │ │ │ │ +

◆ GTSAM_CONCEPT_TESTABLE_INST

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │
#define GTSAM_MAKE_VECTOR_DEFS#define GTSAM_CONCEPT_TESTABLE_INST( N)T)   template class gtsam::IsTestable<T>;
│ │ │ │
│ │ │ │ -Value:
using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ -
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │ -
│ │ │ │ + │ │ │ │ +

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,147 +1,84 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_. │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  MMKKLL__BBLLAASS   MKL_DOMAIN_BLAS │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -#define  _G_T_S_A_M___M_A_K_E___V_E_C_T_O_R___D_E_F_S(N) │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef Eigen::VectorXd  ggttssaamm::::VVeeccttoorr │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ - typedef Eigen::Matrix< double, 1, 1 >  ggttssaamm::::VVeeccttoorr11 │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ - typedef Eigen::Vector2d  ggttssaamm::::VVeeccttoorr22 │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ - typedef Eigen::Vector3d  ggttssaamm::::VVeeccttoorr33 │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  GGTTSSAAMM__PPRRIINNTT(x)   ((x).print(#x)) │ │ │ │ │   │ │ │ │ │ - typedef Eigen::VectorBlock< Vector >  ggttssaamm::::SSuubbVVeeccttoorr │ │ │ │ │ +#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. │ │ │ │ │   │ │ │ │ │ -typedef Eigen::VectorBlock< const Vector >  ggttssaamm::::CCoonnssttSSuubbVVeeccttoorr │ │ │ │ │ +#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 │ │ │ │ │ - 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. │ │ │ │ │ +void  ggttssaamm::::pprriinntt (float v, const std::string &s="") │ │ │ │ │ +  │ │ │ │ │ +void  ggttssaamm::::pprriinntt (double v, const std::string &s="") │ │ │ │ │   │ │ │ │ │ - double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ - Vector &weights, Vector &pseudo) │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2, double tol) │ │ │ │ │ +  Call equal on the object. │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2) │ │ │ │ │ +  Call equal without tolerance (use default tolerance) │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ +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 ********** │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alex Hagiopol │ │ │ │ │ - Varun Agrawal │ │ │ │ │ +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__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(); │ │ │ │ │ +********** _?◆_? 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 │ │ │ │ │ - * _V_e_c_t_o_r_._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/a00065.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,23 +1,10 @@ │ │ │ │ │ var a00065 = [ │ │ │ │ │ - ["assert_equal", "a00065.html#ac3cf8e8e70cd696d9c789c31bc1e89a7", null], │ │ │ │ │ - ["assert_equal", "a00065.html#a9f3ee662d25ffb8c04b4e35c4b02e90b", null], │ │ │ │ │ - ["assert_inequal", "a00065.html#a2ca3db47b15350977c1f03c5560ab332", null], │ │ │ │ │ - ["concatVectors", "a00065.html#ae1c88acfa9575bcea3f2e738cb637a85", null], │ │ │ │ │ - ["concatVectors", "a00065.html#a09610ec69a8b3f3a00399bb378eda9cf", null], │ │ │ │ │ - ["dot", "a00065.html#ad80249acf12bbea741e755cd8fc73042", null], │ │ │ │ │ - ["ediv_", "a00065.html#a14844c96706dc362917e0d5e76d65fbe", null], │ │ │ │ │ - ["equal", "a00065.html#a3d1cad2313f69f9fa5008fdc348d1526", null], │ │ │ │ │ - ["equal", "a00065.html#aa7ed1d4fe2dd6828cfeb2d8f488b818d", null], │ │ │ │ │ - ["equal_with_abs_tol", "a00065.html#a8bdced1844ffd6a53af0396d82eaa6da", null], │ │ │ │ │ - ["fpEqual", "a00065.html#ae19c359190a379cba44bf818a2293f7c", null], │ │ │ │ │ - ["greaterThanOrEqual", "a00065.html#a368ee40bd9c0124d572d2e49bcb077bf", null], │ │ │ │ │ - ["house", "a00065.html#afb77eefc25dea0fb3e0fcc8f246b617a", null], │ │ │ │ │ - ["houseInPlace", "a00065.html#ac9c0c05b0198ec0fb6e4115edb755910", null], │ │ │ │ │ - ["inner_prod", "a00065.html#a4ca41ba9ec9d6d21a2b9f5e00f9f25ed", null], │ │ │ │ │ - ["linear_dependent", "a00065.html#a2a0cfd7908b06491df49b6a9c9186775", null], │ │ │ │ │ - ["operator==", "a00065.html#a64988014ab746343803620dc42513646", null], │ │ │ │ │ - ["print", "a00065.html#a760cc36e9009b23cf564c436294b3504", null], │ │ │ │ │ - ["print", "a00065.html#a5e5d8018a0f44a6a299406aeda27d44b", null], │ │ │ │ │ - ["save", "a00065.html#a09f2bbdb9f9d633542362dbe8d79f9ab", null], │ │ │ │ │ - ["weightedPseudoinverse", "a00065.html#aca1c56d11a05464a7a5458dc32ccc777", null] │ │ │ │ │ + ["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", "a00065.html#abe0a238cdd07d484ff0be56ae945b182", null], │ │ │ │ │ + ["assert_equal", "a00065.html#a659619cca082d1c10b07c033d48c54da", null], │ │ │ │ │ + ["equal", "a00065.html#a1f3dc67ad88b799c469088e428c583b1", null], │ │ │ │ │ + ["equal", "a00065.html#ad2dbee4a72127938c79162cc8b6d5152", null] │ │ │ │ │ ]; │ │ │ ├── ./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/Testable.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,192 +98,171 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Vector.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
│ │ │ │ -
21// \callgraph
│ │ │ │ -
22
│ │ │ │ -
23#pragma once
│ │ │ │ -
24#ifndef MKL_BLAS
│ │ │ │ -
25#define MKL_BLAS MKL_DOMAIN_BLAS
│ │ │ │ -
26#endif
│ │ │ │ -
27
│ │ │ │ - │ │ │ │ -
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;
│ │ │ │ +
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;
│ │ │ │ +
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());
│ │ │ │ + │ │ │ │
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);
│ │ │ │ +
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
│ │ │ │ -
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);
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
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);
│ │ │ │ +
133 template<typename T>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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
│ │ │ │ -
Included from all GTSAM files.
│ │ │ │ +
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>;
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition Vector.cpp:199
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
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
│ │ │ │ -
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition Vector.h:202
│ │ │ │
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ -
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
│ │ │ │ -
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition Vector.cpp:212
│ │ │ │
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ -
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
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
│ │ │ │ -
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition Vector.cpp:302
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ +
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
Binary predicate on shared pointers.
Definition Testable.h:123
│ │ │ │ +
Requirements on type to pass it to Testable template below.
Definition Testable.h:134
│ │ │ │ +
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,247 +1,175 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Vector.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 │ │ │ │ │ -21// \callgraph │ │ │ │ │ -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; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -_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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _V_e_c_t_o_r_._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/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/FastSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
types.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
FastSet.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Functions for handling type information. │ │ │ │ +

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.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -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
│ │ │ │ +

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

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - May 18, 2020 │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_y_p_e_s_._c_p_p │ │ │ │ │ + * _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/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/FastVector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
FastMap.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
FastVector.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ 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...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ 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::map that uses boost's fast_pool_allocator.

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

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

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Feb 9, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -FastMap.h File Reference │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. _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_. │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ 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::map that uses boost's fast_pool_allocator. │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ + Feb 9, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_M_a_p_._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/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/FastVector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,47 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastMap.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
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ -
41
│ │ │ │ -
42 typedef std::map<KEY, VALUE, std::less<KEY>,
│ │ │ │ - │ │ │ │ -
44
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ -
│ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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}
│ │ │ │ -
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ + │ │ │ │ +
23#include <vector>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
32template <typename T>
│ │ │ │ + │ │ │ │ +
34 std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
│ │ │ │ +
35
│ │ │ │ +
36} // namespace gtsam
│ │ │ │ +
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ -
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
FastMap(const Base &x)
Copy constructor from the base map class.
Definition FastMap.h:56
│ │ │ │ -
FastMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastMap.h:50
│ │ │ │ -
bool insert2(const KEY &key, const VALUE &val)
Handy 'insert' function for Matlab wrapper.
Definition FastMap.h:64
│ │ │ │ -
FastMap(const FastMap< KEY, VALUE > &x)
Copy constructor from another FastMap.
Definition FastMap.h:53
│ │ │ │ -
FastMap()
Default constructor.
Definition FastMap.h:46
│ │ │ │ -
bool exists(const KEY &e) const
Handy 'exists' function.
Definition FastMap.h:67
│ │ │ │ -
friend class boost::serialization::access
Serialization function.
Definition FastMap.h:71
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,112 +1,46 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastMap.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 │ │ │ │ │ -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: │ │ │ │ │ -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 } │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ - * _F_a_s_t_M_a_p_._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_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/testLie.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,63 +98,111 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
lieProxies.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
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ - │ │ │ │ -
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); }
│ │ │ │ +
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>
│ │ │ │ + │ │ │ │ +
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} // \namespace testing
│ │ │ │ -
53} // \namespace gtsam
│ │ │ │ -
54
│ │ │ │ -
55
│ │ │ │ -
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ -
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ -
T inverse(const T &t)
unary functions
Definition lieProxies.h:43
│ │ │ │ -
Included from all GTSAM files.
│ │ │ │ +
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 // 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); }
│ │ │ │ +
Some functions to compute numerical derivatives.
│ │ │ │ +
Base class and basic functions for Lie types.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
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,67 +1,119 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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); } │ │ │ │ │ +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} // \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. │ │ │ │ │ +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 // 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_:_:_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 │ │ │ │ │ - * _l_i_e_P_r_o_x_i_e_s_._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/a00077_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/chartTesting.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,95 +98,220 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
chartTesting.h
│ │ │ │ +
Group.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 chartTesting.h
│ │ │ │ -
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>
│ │ │ │ -
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().
│ │ │ │ -
30template<typename T>
│ │ │ │ -
31void testDefaultChart(TestResult& result_,
│ │ │ │ -
32 const std::string& name_,
│ │ │ │ -
33 const T& value) {
│ │ │ │ -
34
│ │ │ │ -
35 GTSAM_CONCEPT_TESTABLE_TYPE(T)
│ │ │ │ -
36
│ │ │ │ -
37 typedef typename gtsam::DefaultChart<T> Chart;
│ │ │ │ -
38 typedef typename Chart::vector Vector;
│ │ │ │ +
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
│ │ │ │ + │ │ │ │ + │ │ │ │
39
│ │ │ │ -
40 // First, check the basic chart concept. This checks that the interface is satisfied.
│ │ │ │ -
41 // The rest of the function is even more detailed, checking the correctness of the chart.
│ │ │ │ -
42 BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
│ │ │ │ -
43
│ │ │ │ -
44 T other = value;
│ │ │ │ -
45
│ │ │ │ -
46 // Check that the dimension of the local value matches the chart dimension.
│ │ │ │ -
47 Vector dx = Chart::local(value, other);
│ │ │ │ -
48 EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size());
│ │ │ │ -
49 // And that the "local" of a value vs. itself is zero.
│ │ │ │ -
50 EXPECT(assert_equal(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size()))));
│ │ │ │ +
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 // Test the invertibility of retract/local
│ │ │ │ -
53 dx.setRandom();
│ │ │ │ -
54 T updated = Chart::retract(value, dx);
│ │ │ │ -
55 Vector invdx = Chart::local(value, updated);
│ │ │ │ -
56 EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
│ │ │ │ -
57
│ │ │ │ -
58 // And test that negative steps work as well.
│ │ │ │ -
59 dx = -dx;
│ │ │ │ -
60 updated = Chart::retract(value, dx);
│ │ │ │ -
61 invdx = Chart::local(value, updated);
│ │ │ │ -
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); }
│ │ │ │ -
Concept check for values that can be used in unit tests.
│ │ │ │ -
Base class and basic functions for Manifold types.
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
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)");
│ │ │ │ + │ │ │ │ +
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 flavor_tag flavor;
│ │ │ │ +
76 G e, g, h;
│ │ │ │ +
77 bool b;
│ │ │ │ +
78};
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
81template<typename G>
│ │ │ │ + │ │ │ │ +
83check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
│ │ │ │ +
84 G e = traits<G>::Identity();
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
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;
│ │ │ │ +
Concept check for values that can be used in unit tests.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ +
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ +
Definition Group.h:38
│ │ │ │ +
Group Concept.
Definition Group.h:46
│ │ │ │ +
Definition Group.h:138
│ │ │ │ +
DirectProduct()
Default constructor yields identity.
Definition Group.h:144
│ │ │ │ +
Template to construct the direct sum of two additive groups Assumes existence of three additive opera...
Definition Group.h:169
│ │ │ │ +
DirectSum()
Default constructor yields identity.
Definition Group.h:178
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,95 +1,237 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -chartTesting.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 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file chartTesting.h │ │ │ │ │ -14 * @brief │ │ │ │ │ -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_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +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#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -_2_8namespace _g_t_s_a_m { │ │ │ │ │ -29// Do a full concept check and test the invertibility of local() vs. retract │ │ │ │ │ -(). │ │ │ │ │ -30template │ │ │ │ │ -31void testDefaultChart(TestResult& result_, │ │ │ │ │ -32 const std::string& name_, │ │ │ │ │ -33 const T& value) { │ │ │ │ │ -34 │ │ │ │ │ -35 GTSAM_CONCEPT_TESTABLE_TYPE(T) │ │ │ │ │ -36 │ │ │ │ │ -37 typedef typename gtsam::DefaultChart Chart; │ │ │ │ │ -38 typedef typename Chart::vector Vector; │ │ │ │ │ +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 │ │ │ │ │ -40 // First, check the basic chart concept. This checks that the interface is │ │ │ │ │ -satisfied. │ │ │ │ │ -41 // The rest of the function is even more detailed, checking the correctness │ │ │ │ │ -of the chart. │ │ │ │ │ -42 BOOST_CONCEPT_ASSERT((ChartConcept)); │ │ │ │ │ -43 │ │ │ │ │ -44 T other = value; │ │ │ │ │ -45 │ │ │ │ │ -46 // Check that the dimension of the local value matches the chart dimension. │ │ │ │ │ -47 Vector dx = Chart::local(value, other); │ │ │ │ │ -48 EXPECT_LONGS_EQUAL(Chart::getDimension(value), dx.size()); │ │ │ │ │ -49 // And that the "local" of a value vs. itself is zero. │ │ │ │ │ -50 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(Eigen::VectorXd::Zero(dx.size())))); │ │ │ │ │ +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 // Test the invertibility of retract/local │ │ │ │ │ -53 dx.setRandom(); │ │ │ │ │ -54 T updated = Chart::retract(value, dx); │ │ │ │ │ -55 Vector invdx = Chart::local(value, updated); │ │ │ │ │ -56 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(invdx), 1e-9)); │ │ │ │ │ -57 │ │ │ │ │ -58 // And test that negative steps work as well. │ │ │ │ │ -59 dx = -dx; │ │ │ │ │ -60 updated = Chart::retract(value, dx); │ │ │ │ │ -61 invdx = Chart::local(value, updated); │ │ │ │ │ -62 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(Matrix(dx), Matrix(invdx), 1e-9)); │ │ │ │ │ -63} │ │ │ │ │ -64} // namespace gtsam │ │ │ │ │ -65 │ │ │ │ │ -68#define CHECK_CHART_CONCEPT(value) \ │ │ │ │ │ -69 { gtsam::testDefaultChart(result_, name_, value); } │ │ │ │ │ +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 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. │ │ │ │ │ -_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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * cchhaarrttTTeessttiinngg..hh │ │ │ │ │ + * _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/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/SymmetricBlockMatrix.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,123 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
Testable.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SymmetricBlockMatrix.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

Access to matrices via blocks of pre-defined sizes. │ │ │ │ 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...
 
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

│ │ │ │ -

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
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Access to matrices via blocks of pre-defined sizes.

│ │ │ │ +

Used in GaussianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,84 +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 | _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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymmetricBlockMatrix.cpp File Reference │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. _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. │ │ │ │ │ +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 │ │ │ │ │ - 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 │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ + * _S_y_m_m_e_t_r_i_c_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/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/SymmetricBlockMatrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
DSFVector.h File Reference
│ │ │ │ +
SymmetricBlockMatrix.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Access to matrices via blocks of pre-defined sizes. │ │ │ │ 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::DSFVector
 DSFVector additionally keeps a vector of keys to support more expensive operations. 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 faster implementation for DSF, which uses vector rather than btree.

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

Access to matrices via blocks of pre-defined sizes.

│ │ │ │ +

Used in GaussianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -DSFVector.h File Reference │ │ │ │ │ -A faster implementation for DSF, which uses vector rather than btree. _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_:_:_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_:_:_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 faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ - Date │ │ │ │ │ - Jun 25, 2010 │ │ │ │ │ +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 │ │ │ │ │ - Kai Ni │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._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/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/SymmetricBlockMatrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,96 +98,439 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DSFVector.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 <gtsam/dllexport.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 {
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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}
│ │ │ │ -
Included from all GTSAM files.
│ │ │ │ +
│ │ │ │ +
51 class GTSAM_EXPORT SymmetricBlockMatrix
│ │ │ │ +
52 {
│ │ │ │ +
53 public:
│ │ │ │ + │ │ │ │ +
55 typedef Eigen::Block<Matrix> Block;
│ │ │ │ +
56 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │ +
57
│ │ │ │ +
58 protected:
│ │ │ │ +
59 Matrix matrix_;
│ │ │ │ + │ │ │ │ +
61
│ │ │ │ + │ │ │ │ +
63
│ │ │ │ +
64 public:
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ + │ │ │ │ +
297
│ │ │ │ +
298 protected:
│ │ │ │ +
299
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
302 return variableColOffsets_.size();
│ │ │ │ +
303 }
│ │ │ │ +
│ │ │ │ +
304
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
307 return nOffsets() - 1;
│ │ │ │ +
308 }
│ │ │ │ +
│ │ │ │ +
309
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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}
│ │ │ │ +
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Typedefs for easier changing of types.
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
A fast implementation of disjoint set forests that uses vector as underly data structure.
Definition DSFVector.h:38
│ │ │ │ -
std::vector< size_t > V
Vector of ints.
Definition DSFVector.h:41
│ │ │ │ -
DSFVector additionally keeps a vector of keys to support more expensive operations.
Definition DSFVector.h:64
│ │ │ │ +
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
Block full()
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:340
│ │ │ │ +
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition SymmetricBlockMatrix.h:62
│ │ │ │ +
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
│ │ │ │ +
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition SymmetricBlockMatrix.h:306
│ │ │ │ +
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
│ │ │ │ +
Matrix matrix_
The full matrix.
Definition SymmetricBlockMatrix.h:59
│ │ │ │ +
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
│ │ │ │ +
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition SymmetricBlockMatrix.h:123
│ │ │ │ +
void setZero()
Set the entire active matrix zero.
Definition SymmetricBlockMatrix.h:258
│ │ │ │ +
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
│ │ │ │ +
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:253
│ │ │ │ +
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition SymmetricBlockMatrix.h:242
│ │ │ │ +
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ +
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
│ │ │ │ +
constBlock full() const
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:335
│ │ │ │ +
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:140
│ │ │ │ +
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
│ │ │ │ +
DenseIndex blockStart() const
Retrieve the first logical block, i.e.
Definition SymmetricBlockMatrix.h:277
│ │ │ │ +
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition SymmetricBlockMatrix.h:247
│ │ │ │ +
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ +
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition SymmetricBlockMatrix.h:70
│ │ │ │ +
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition SymmetricBlockMatrix.h:80
│ │ │ │ +
DenseIndex & blockStart()
Retrieve or modify the first logical block, i.e.
Definition SymmetricBlockMatrix.h:273
│ │ │ │ +
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
│ │ │ │ +
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition SymmetricBlockMatrix.h:301
│ │ │ │ +
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
│ │ │ │ +
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ +
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition SymmetricBlockMatrix.h:60
│ │ │ │ +
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
│ │ │ │ +
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition SymmetricBlockMatrix.h:311
│ │ │ │ +
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,97 +1,546 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DSFVector.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#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 { │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._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/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/FastDefaultAllocator.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,210 +96,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
timing.h File Reference
│ │ │ │ +Macros
│ │ │ │ +
FastDefaultAllocator.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Timing utilities. │ │ │ │ +

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

│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ Classes

class  gtsam::internal::TimingOutline
 Timing Entry, arranged in a tree. More...
struct  gtsam::internal::FastDefaultAllocator< T >
 Default allocator for list, map, and set types. More...
 
class  gtsam::internal::AutoTicToc
 Small class that calls internal::tic at construction, and internol::toc when destroyed. 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.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -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)
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -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< TimingOutlinegtsam::internal::gTimingRoot
 
│ │ │ │ -GTSAM_EXTERN_EXPORT boost::weak_ptr< TimingOutlinegtsam::internal::gCurrentTimer
 
│ │ │ │

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)
│ │ │ │ -
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);
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

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

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 15, 2013
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,108 +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 | _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_._._. │ │ │ │ │ +_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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ -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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -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 │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Timing utilities. │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ 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); │ │ │ │ │ + Aug 15, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_i_m_i_n_g_._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/a00092.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00092 = [ │ │ │ │ │ - ["gtsam::internal::TimingOutline", "a02516.html", "a02516"], │ │ │ │ │ - ["gtsam::internal::AutoTicToc", "a02520.html", null] │ │ │ │ │ + ["gtsam::internal::FastDefaultAllocator< T >", "a02304.html", null], │ │ │ │ │ + ["gtsam::internal::FastDefaultVectorAllocator< T >", "a02308.html", 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/FastDefaultAllocator.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,328 +98,106 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
timing.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/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>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
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// 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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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_;
│ │ │ │ - │ │ │ │ - │ │ │ │ -
163
│ │ │ │ -
164#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ -
165 boost::timer::cpu_timer timer_;
│ │ │ │ -
166#else
│ │ │ │ -
167 boost::timer timer_;
│ │ │ │ - │ │ │ │ -
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}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
Typedefs for easier changing of types.
│ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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}
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
Timing Entry, arranged in a tree.
Definition timing.h:145
│ │ │ │ -
double t2_
cache the
Definition timing.h:150
│ │ │ │ -
ChildMap children_
subtrees
Definition timing.h:162
│ │ │ │ -
double max() const
max time, in seconds
Definition timing.h:183
│ │ │ │ -
double wall() const
wall time, in seconds
Definition timing.h:181
│ │ │ │ -
double secs() const
time taken, in seconds, including children
Definition timing.h:179
│ │ │ │ -
double self() const
self time only, in seconds
Definition timing.h:180
│ │ │ │ -
boost::weak_ptr< TimingOutline > parent_
parent pointer
Definition timing.h:160
│ │ │ │ -
double min() const
min time, in seconds
Definition timing.h:182
│ │ │ │ -
double mean() const
mean self time, in seconds
Definition timing.h:184
│ │ │ │ -
GTSAM_EXPORT size_t time() const
time taken, including children
Definition timing.cpp:65
│ │ │ │ -
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │ -
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │ +
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ +
Default allocator for vector types (we never use boost pool for vectors)
Definition FastDefaultAllocator.h:72
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,394 +1,102 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -timing.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_/_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 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20#include // Configuration from CMake │ │ │ │ │ +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// 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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _t_i_m_i_n_g_._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/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/WeightedSampler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,45 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ProductLieGroup.h File Reference
│ │ │ │ +
WeightedSampler.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Fast sampling without replacement. │ │ │ │ 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::traits< ProductLieGroup< G, H > >
class  gtsam::WeightedSampler< Engine >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

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

Detailed Description

│ │ │ │ -

Group product of two Lie Groups.

│ │ │ │ -
Date
May, 2015
│ │ │ │ +

Fast sampling without replacement.

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
May 2019
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -ProductLieGroup.h File Reference │ │ │ │ │ -Group product of two Lie Groups. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_ _>_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Group product of two Lie Groups. │ │ │ │ │ - Date │ │ │ │ │ - May, 2015 │ │ │ │ │ +Fast sampling without replacement. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + May 2019 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00095.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00095 = [ │ │ │ │ │ - ["gtsam::ProductLieGroup< G, H >", "a02464.html", "a02464"], │ │ │ │ │ - ["gtsam::traits< ProductLieGroup< G, H > >", "a02468.html", null] │ │ │ │ │ + ["gtsam::WeightedSampler< Engine >", "a02664.html", "a02664"] │ │ │ │ │ ]; │ │ │ ├── ./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/WeightedSampler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,193 +98,152 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ProductLieGroup.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 * -------------------------------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) {}
│ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
38 private:
│ │ │ │ +
39 Engine* engine_; // random number generation engine
│ │ │ │ +
40
│ │ │ │ +
41 public:
│ │ │ │ +
46 explicit WeightedSampler(Engine* engine) : engine_(engine) {}
│ │ │ │
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;
│ │ │ │ -
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;
│ │ │ │ +
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;
│ │ │ │ +
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;
│ │ │ │ +
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
│ │ │ │ -
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};
│ │ │ │ +
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
│ │ │ │
│ │ │ │ -
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
│ │ │ │ -
Base class and basic functions for Lie types.
│ │ │ │ +
137} // namespace gtsam
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ -
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ -
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition ProductLieGroup.h:29
│ │ │ │ -
ProductLieGroup()
Default constructor yields identity.
Definition ProductLieGroup.h:40
│ │ │ │ +
Definition WeightedSampler.h:37
│ │ │ │ +
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,213 +1,150 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ProductLieGroup.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 * -------------------------------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) {} │ │ │ │ │ +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 │ │ │ │ │ -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)); │ │ │ │ │ +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 _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; │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 _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. │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._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/a00098.html │ │ │ │ @@ -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/types.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,116 +97,205 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
Group.h File Reference
│ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
types.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

Typedefs for easier changing of types. │ │ │ │ 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::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::multiplicative_group_tag
 Group operator syntax flavors. More...
struct  gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the non-const version. More...
 
struct  gtsam::additive_group_tag
struct  gtsam::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the const version. More...
 
class  gtsam::IsGroup< G >
 Group Concept. 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::DirectProduct
class  gtsam::ListOfOneContainer< T >
 A helper class that behaves as a container with one element, and works with boost::range. More...
 
struct  gtsam::traits< DirectProduct< G, H > >
class  gtsam::TbbOpenMPMixedScope
 An object whose scope defines a block where TBB and OpenMP parallelism are mixed. 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::needs_eigen_aligned_allocator< typename, typename >
 A SFINAE trait to mark classes that need special alignment. More...
 
struct  gtsam::traits< DirectSum< G, H > >
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_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_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

│ │ │ │ -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
 
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

│ │ │ │ -

Concept check class for variable types with Group properties.

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

Typedefs for easier changing of types.

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

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_CONCEPT_GROUP_INST

│ │ │ │ + │ │ │ │ +

◆ assert_throw

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_CONCEPT_GROUP_INST#define assert_throw( T)   template class gtsam::IsGroup<T>;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

│ │ │ │ │ │ │ │ -

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

│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#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,72 +1,138 @@ │ │ │ │ │ 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 | _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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +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_s_G_r_o_u_p_<_ _G_ _> │ │ │ │ │ -  Group Concept. _M_o_r_e_._._. │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  GGTTSSAAMM__DDEEPPRREECCAATTEEDD │ │ │ │ │   │ │ │ │ │ -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_ _>_ _> │ │ │ │ │ +#define  DDOO__PPRRAAGGMMAA(x)   _Pragma (#x) │ │ │ │ │   │ │ │ │ │ - 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_._._. │ │ │ │ │ +#define  CCLLAANNGG__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_r_e_c_t_S_u_m_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +#define  GGCCCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +#define  MMSSVVCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(code) │ │ │ │ │   │ │ │ │ │ -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  DDIIAAGGNNOOSSTTIICC__PPOOPP() │ │ │ │ │   │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__GGRROOUUPP__TTYYPPEE(T)   typedef _g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ - _gtsam_IsGroup_##T; │ │ │ │ │ +#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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -  ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__RREEQQUUIIRREESS (((_I_s_G_r_o_u_p< G >)),(bool)) │ │ │ │ │ - check_group_invariants(const G &a │ │ │ │ │ -  Check invariants. │ │ │ │ │ +#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. │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ - const G &  ggttssaamm::::bb │ │ │ │ │ +#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. │ │ │ │ │   │ │ │ │ │ -const G double  ggttssaamm::::ttooll │ │ │ │ │ +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. │ │ │ │ │ +  │ │ │ │ │ +   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 ********** │ │ │ │ │ -Concept check class for variable types with Group properties. │ │ │ │ │ - Date │ │ │ │ │ - November, 2011 │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Aug 21, 2010 │ │ │ │ │ ********** 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 │ │ │ │ │ +********** _?◆_? 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 │ │ │ │ │ - * _G_r_o_u_p_._h │ │ │ │ │ + * _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/a00098.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,12 +1,19 @@ │ │ │ │ │ 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] │ │ │ │ │ + ["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", "a00098.html#a30d3af2da1ea940197addce4f3c223c8", null], │ │ │ │ │ + ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW", "a00098.html#aa123dd61687e08ae328154a1890b7b86", null], │ │ │ │ │ + ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF", "a00098.html#a352ab46cee3a766e30b049690802fb0a", null], │ │ │ │ │ + ["DenseIndex", "a00098.html#a21006f326f57cdf7d3f35fec40f43e40", null], │ │ │ │ │ + ["FactorIndex", "a00098.html#a19c53f2faabb0f4b4b78ce2f7168cc14", null], │ │ │ │ │ + ["Key", "a00098.html#adad029f5f6ffce610428b5fe768b0df2", null], │ │ │ │ │ + ["void_t", "a00098.html#a6525d741529c7a2cd91fa913431f5fb7", null], │ │ │ │ │ + ["demangle", "a00098.html#a0b97fc96e5f69236e81489c66d5b92ba", null], │ │ │ │ │ + ["ListOfOne", "a00098.html#a045b43784acac77531813c4ab6c4d281", null] │ │ │ │ │ ]; │ │ │ ├── ./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/types.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,220 +98,318 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Group.h
│ │ │ │ +
types.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/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 {
│ │ │ │ +
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
│ │ │ │ -
34struct group_tag {};
│ │ │ │ -
35
│ │ │ │ - │ │ │ │ - │ │ │ │ -
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)");
│ │ │ │ - │ │ │ │ -
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 flavor_tag flavor;
│ │ │ │ -
76 G e, g, h;
│ │ │ │ -
77 bool b;
│ │ │ │ -
78};
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
81template<typename G>
│ │ │ │ - │ │ │ │ -
83check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
│ │ │ │ -
84 G e = traits<G>::Identity();
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
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> {};
│ │ │ │ +
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
│ │ │ │ -
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> {};
│ │ │ │ +
108 /* ************************************************************************* */
│ │ │ │ +
113 template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
│ │ │ │ +
114 typename AS_CONST>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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()) {}
│ │ │ │ +
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 };
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
130 /* ************************************************************************* */
│ │ │ │ +
136 template<typename T, T defaultValue>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
138 T value;
│ │ │ │ +
139
│ │ │ │ +
141 ValueWithDefault() : value(defaultValue) {}
│ │ │ │ +
142
│ │ │ │ +
144 ValueWithDefault(const T& _value) : value(_value) {}
│ │ │ │
145
│ │ │ │ -
146 // Construct from two subgroup elements
│ │ │ │ -
147 DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
│ │ │ │ +
147 T& operator*() { return value; }
│ │ │ │
148
│ │ │ │ -
149 // identity
│ │ │ │ -
150 static DirectProduct Identity() { return DirectProduct(); }
│ │ │ │ +
150 const T& operator*() const { return value; }
│ │ │ │
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;}
│ │ │ │ +
153 operator T() const { return value; }
│ │ │ │ +
154 };
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
156 /* ************************************************************************* */
│ │ │ │ +
159 template<typename T>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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;
│ │ │ │ -
Concept check for values that can be used in unit tests.
│ │ │ │ +
177 template<typename T>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
192 {
│ │ │ │ +
193 int previousOpenMPThreads;
│ │ │ │ +
194
│ │ │ │ +
195 public:
│ │ │ │ +
196#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP
│ │ │ │ + │ │ │ │ +
198 previousOpenMPThreads(omp_get_num_threads())
│ │ │ │ +
199 {
│ │ │ │ +
200 omp_set_num_threads(omp_get_num_procs() / 4);
│ │ │ │ +
201 }
│ │ │ │ +
202
│ │ │ │ + │ │ │ │ +
204 {
│ │ │ │ +
205 omp_set_num_threads(previousOpenMPThreads);
│ │ │ │ +
206 }
│ │ │ │ +
207#else
│ │ │ │ +
208 TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {}
│ │ │ │ + │ │ │ │ +
210#endif
│ │ │ │ +
211 };
│ │ │ │ +
│ │ │ │ +
212
│ │ │ │ +
213#ifdef __clang__
│ │ │ │ +
214# pragma clang diagnostic pop
│ │ │ │ +
215#endif
│ │ │ │ +
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
│ │ │ │ +
259
│ │ │ │ +
260#endif
│ │ │ │ +
261
│ │ │ │ +
262#ifdef min
│ │ │ │ +
263#undef min
│ │ │ │ +
264#endif
│ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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;
│ │ │ │ +
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ -
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ -
Definition Group.h:38
│ │ │ │ -
Group Concept.
Definition Group.h:46
│ │ │ │ -
Definition Group.h:138
│ │ │ │ -
DirectProduct()
Default constructor yields identity.
Definition Group.h:144
│ │ │ │ -
Template to construct the direct sum of two additive groups Assumes existence of three additive opera...
Definition Group.h:169
│ │ │ │ -
DirectSum()
Default constructor yields identity.
Definition Group.h:178
│ │ │ │ +
ListOfOneContainer< T > ListOfOne(const T &element)
Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
Definition types.h:178
│ │ │ │ +
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ +
std::uint64_t FactorIndex
Integer nonlinear factor index type.
Definition types.h:103
│ │ │ │ +
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
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
│ │ │ │ +
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
Helper class that uses templates to select between two types based on whether TEST_TYPE is const or n...
Definition types.h:115
│ │ │ │ +
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │ +
T & operator*()
Operator to access the value.
Definition types.h:147
│ │ │ │ +
const T & operator*() const
Operator to access the value.
Definition types.h:150
│ │ │ │ +
ValueWithDefault()
Default constructor, initialize to default value supplied in template argument.
Definition types.h:141
│ │ │ │ +
ValueWithDefault(const T &_value)
Initialize to the given value.
Definition types.h:144
│ │ │ │ +
A helper class that behaves as a container with one element, and works with boost::range.
Definition types.h:160
│ │ │ │ +
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ +
A SFINAE trait to mark classes that need special alignment.
Definition types.h:295
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,237 +1,343 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Group.h │ │ │ │ │ +types.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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_/_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 { │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ -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 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 {}; │ │ │ │ │ +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 │ │ │ │ │ -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 {}; │ │ │ │ │ +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 │ │ │ │ │ -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()) │ │ │ │ │ -{} │ │ │ │ │ +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 }; │ │ │ │ │ +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 │ │ │ │ │ -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) {} │ │ │ │ │ +_1_4_7 T& _o_p_e_r_a_t_o_r_*() { return value; } │ │ │ │ │ 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(); } │ │ │ │ │ +_1_5_0 const T& _o_p_e_r_a_t_o_r_*() const { return value; } │ │ │ │ │ 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;} │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +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} │ │ │ │ │ +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 │ │ │ │ │ +259 │ │ │ │ │ +260#endif │ │ │ │ │ +261 │ │ │ │ │ +262#ifdef min │ │ │ │ │ +263#undef min │ │ │ │ │ +264#endif │ │ │ │ │ +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; │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _G_r_o_u_p_._h │ │ │ │ │ + * _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/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/VectorSerialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
FastVector.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
VectorSerialization.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

serialization for Vectors │ │ │ │ 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.
 

│ │ │ │ +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

│ │ │ │ -

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

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

serialization for Vectors

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
February 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,35 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_. │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const gtsam::Vector &v, unsigned │ │ │ │ │ + int) │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, gtsam::Vector &v, unsigned int) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const Eigen::Matrix< double, D, │ │ │ │ │ + 1 > &v, unsigned int) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< double, D, 1 > │ │ │ │ │ + &v, unsigned int) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +serialization for Vectors │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Feb 9, 2011 │ │ │ │ │ + February 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_V_e_c_t_o_r_._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/a00101_source.html │ │ │ │ @@ -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/base/VectorSerialization.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,47 +98,81 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastVector.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
│ │ │ │ - │ │ │ │ -
23#include <vector>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
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
│ │ │ │ -
32template <typename T>
│ │ │ │ - │ │ │ │ -
34 std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
│ │ │ │ -
35
│ │ │ │ -
36} // namespace gtsam
│ │ │ │ -
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ -
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
│ │ │ │ -
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
27namespace boost {
│ │ │ │ +
28namespace serialization {
│ │ │ │ +
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)
│ │ │ │ +
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
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,46 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastVector.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 │ │ │ │ │ -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 { │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +27namespace boost { │ │ │ │ │ +28namespace serialization { │ │ │ │ │ +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 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_V_e_c_t_o_r_._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/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/lieProxies.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,86 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ Functions
│ │ │ │ -
Manifold.h File Reference
│ │ │ │ +
lieProxies.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Provides convenient mappings of common member functions for testing. │ │ │ │ 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.
 
│ │ │ │ +template<class T >
gtsam::testing::between (const T &t1, const T &t2)
 binary functions
 
│ │ │ │ +template<class T >
gtsam::testing::compose (const T &t1, const T &t2)
 
│ │ │ │ +template<class T >
gtsam::testing::inverse (const T &t)
 unary functions
 
│ │ │ │ +template<class T , class P >
gtsam::testing::rotate (const T &r, const P &pt)
 rotation functions
 
│ │ │ │ +template<class T , class P >
gtsam::testing::unrotate (const T &r, const P &pt)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class and basic functions for Manifold types.

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

Provides convenient mappings of common member functions for testing.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,60 +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 | _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 │ │ │ │ │ +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_. │ │ │ │ │ -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. │ │ │ │ │ +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) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ +Provides convenient mappings of common member functions for testing. │ │ │ │ │ Author │ │ │ │ │ Alex Cunningham │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Mike Bosse │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_n_i_f_o_l_d_._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/a00104.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,5 @@ │ │ │ │ │ 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] │ │ │ │ │ + ["between", "a00104.html#a72a7c561cc91f437a22023297fb7f658", null], │ │ │ │ │ + ["inverse", "a00104.html#aaa623dd059a67523b029af6ec20a38da", null], │ │ │ │ │ + ["rotate", "a00104.html#a644bbe6719d8dd756e11e8b3d2fd229a", 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/lieProxies.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,189 +98,63 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Manifold.h
│ │ │ │ +
lieProxies.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
│ │ │ │ + │ │ │ │
21
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/base/Testable.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;
│ │ │ │ +
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
│ │ │ │ -
55namespace internal {
│ │ │ │ -
56
│ │ │ │ -
58template<class Class>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
60
│ │ │ │ -
61 enum { dim = Class::dimension };
│ │ │ │ -
62
│ │ │ │ -
63 Class p, q;
│ │ │ │ -
64 Eigen::Matrix<double, dim, 1> v;
│ │ │ │ - │ │ │ │ -
66
│ │ │ │ -
67 BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
│ │ │ │ -
68 v = p.localCoordinates(q);
│ │ │ │ -
69 q = p.retract(v);
│ │ │ │ -
70 }
│ │ │ │ -
71};
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
74template<class Class, int N>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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;
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ - │ │ │ │ -
127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
│ │ │ │ - │ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>;
│ │ │ │ -
Special class for optional Jacobian arguments.
│ │ │ │ -
Concept check for values that can be used in unit tests.
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
55
│ │ │ │ +
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ +
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ +
T inverse(const T &t)
unary functions
Definition lieProxies.h:43
│ │ │ │ +
Included from all GTSAM files.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ -
Requirements on type to pass it to Manifold template below.
Definition Manifold.h:59
│ │ │ │ -
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ -
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
│ │ │ │ -
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,206 +1,67 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Manifold.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 │ │ │ │ │ -20#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +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_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_n_i_f_o_l_d_._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/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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
17};
│ │ │ │ -
│ │ │ │ -
18
│ │ │ │ -
19template<typename T> struct traits;
│ │ │ │ -
20
│ │ │ │ -
21namespace internal {
│ │ │ │ -
22
│ │ │ │ -
24template<class Class, int N>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
60
│ │ │ │ +
61 enum { dim = Class::dimension };
│ │ │ │ +
62
│ │ │ │ +
63 Class p, q;
│ │ │ │ +
64 Eigen::Matrix<double, dim, 1> v;
│ │ │ │ + │ │ │ │ +
66
│ │ │ │ +
67 BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
│ │ │ │ +
68 v = p.localCoordinates(q);
│ │ │ │ +
69 q = p.retract(v);
│ │ │ │ +
70 }
│ │ │ │ +
71};
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
74template<class Class, int N>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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;
│ │ │ │ - │ │ │ │ -
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;
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ + │ │ │ │ +
127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
│ │ │ │ + │ │ │ │ + │ │ │ │ +
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>
│ │ │ │
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
191
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
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> > :
│ │ │ │ - │ │ │ │ -
285 Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
│ │ │ │ -
286
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
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;
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
355
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
Base class and basic functions for Lie types.
│ │ │ │ + │ │ │ │ +
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>;
│ │ │ │ +
Concept check for values that can be used in unit tests.
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Special class for optional Jacobian arguments.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
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
│ │ │ │ +
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
Definition Group.h:38
│ │ │ │ -
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ -
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ +
Requirements on type to pass it to Manifold template below.
Definition Manifold.h:59
│ │ │ │ +
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ +
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
│ │ │ │ +
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
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
│ │ │ │ - │ │ │ │ +
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
tag to assert a type is a vector space
Definition VectorSpace.h:16
│ │ │ │ -
VectorSpaceTraits Implementation for Fixed sizes.
Definition VectorSpace.h:25
│ │ │ │ -
Requirements on type to pass it to Manifold template below.
Definition VectorSpace.h:164
│ │ │ │ -
A helper that implements the traits interface for classes that define vector spaces To use this for y...
Definition VectorSpace.h:185
│ │ │ │ -
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ -
A helper that implements the traits interface for scalar vector spaces.
Definition VectorSpace.h:212
│ │ │ │ -
Definition VectorSpace.h:354
│ │ │ │ -
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. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_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_:_:_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/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/FastList.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
FastSet.h File Reference
│ │ │ │ +
FastList.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

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::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::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.
 
│ │ │ │

Detailed Description

│ │ │ │ -

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

│ │ │ │ +

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

│ │ │ │
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +
Date
Oct 22, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -FastSet.h File Reference │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ +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_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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ + Oct 22, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_S_e_t_._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/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/FastList.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastSet.h
│ │ │ │ +
FastList.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,146 +114,83 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
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>
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
56 typedef std::set<VALUE, std::less<VALUE>,
│ │ │ │ -
57 typename internal::FastDefaultAllocator<VALUE>::type> Base;
│ │ │ │ +
57 FastList(const Base& x) : Base(x) {}
│ │ │ │
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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}
│ │ │ │ -
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ -
Concept check for values that can be used in unit tests.
│ │ │ │ +
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:
│ │ │ │ + │ │ │ │ +
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}
│ │ │ │ +
An easy way to control which allocator is used for Fast* collections.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
Definition FastSet.h:50
│ │ │ │ -
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition FastSet.h:119
│ │ │ │ -
void print(const std::string &str="") const
Print to implement Testable: pretty basic.
Definition FastSet.h:101
│ │ │ │ -
FastSet(const INPUTCONTAINER &container)
Constructor from a iterable container, passes through to base class.
Definition FastSet.h:65
│ │ │ │ -
FastSet(const FastSet< VALUE > &x)
Copy constructor from another FastSet.
Definition FastSet.h:70
│ │ │ │ -
bool exists(const VALUE &e) const
Handy 'exists' function.
Definition FastSet.h:96
│ │ │ │ -
bool equals(const FastSet< VALUE > &other, double tol=1e-9) const
Check for equality within tolerance to implement Testable.
Definition FastSet.h:107
│ │ │ │ -
friend class boost::serialization::access
Serialization function.
Definition FastSet.h:125
│ │ │ │ -
FastSet(const Base &x)
Copy constructor from the base set class.
Definition FastSet.h:75
│ │ │ │ -
FastSet()=default
Default constructor.
│ │ │ │ -
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ +
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
FastList(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastList.h:51
│ │ │ │ +
FastList()
Default constructor.
Definition FastList.h:47
│ │ │ │ +
FastList(std::initializer_list< VALUE > l)
Construct from c++11 initializer list:
Definition FastList.h:60
│ │ │ │ +
FastList(const FastList< VALUE > &x)
Copy constructor from another FastList.
Definition FastList.h:54
│ │ │ │ +
friend class boost::serialization::access
Serialization function.
Definition FastList.h:80
│ │ │ │ +
FastList(const Base &x)
Copy constructor from the base list class.
Definition FastList.h:57
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastSet.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 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,161 +16,99 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 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: │ │ │ │ │ +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 │ │ │ │ │ -56 typedef std::set, │ │ │ │ │ -57 typename internal::FastDefaultAllocator::type> Base; │ │ │ │ │ +_5_7 _F_a_s_t_L_i_s_t(const Base& x) : Base(x) {} │ │ │ │ │ 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} │ │ │ │ │ +_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. │ │ │ │ │ -_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _F_a_s_t_S_e_t_._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/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/timing.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
VerticalBlockMatrix.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
timing.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

Timing utilities. │ │ │ │ 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

│ │ │ │ +GTSAM_EXPORT boost::shared_ptr< TimingOutlinegtsam::internal::gTimingRoot (new TimingOutline("Total", getTicTocID("Total")))
 
│ │ │ │ +GTSAM_EXPORT boost::weak_ptr< TimingOutlinegtsam::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

│ │ │ │ -

A matrix with column blocks of pre-defined sizes.

│ │ │ │ -

Used in JacobianFactor and GaussianConditional.

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

Timing utilities.

│ │ │ │ +
Author
Richard Roberts, Michael Kaess
│ │ │ │ +
Date
Oct 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -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. │ │ │ │ │ +Timing utilities. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Richard Roberts, Michael Kaess │ │ │ │ │ Date │ │ │ │ │ - Sep 18, 2010 │ │ │ │ │ + Oct 5, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Matrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -99,203 +99,294 @@ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ Typedefs | │ │ │ │ Functions
│ │ │ │ -
types.h File Reference
│ │ │ │ +
Matrix.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

typedef and functions to augment Eigen's MatrixXd │ │ │ │ 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::Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >
 Reshape functor. More...
 
struct  gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the non-const version. 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::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the const version. More...
struct  gtsam::Reshape< M, N, InOptions, M, N, InOptions >
 Reshape specialization that does nothing as shape stays the same. 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...
struct  gtsam::Reshape< N, M, InOptions, M, N, InOptions >
 Reshape specialization that does transpose. More...
 
class  gtsam::ListOfOneContainer< T >
 A helper class that behaves as a container with one element, and works with boost::range. More...
struct  gtsam::MultiplyWithInverse< N >
 Functor that implements multiplication of a vector b with the inverse of a matrix A. 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 > >
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_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.
 
#define GTSAM_MAKE_MATRIX_DEFS(N)
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ 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::
 
│ │ │ │ +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

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.
 
│ │ │ │ +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

│ │ │ │ -

Typedefs for easier changing of types.

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

typedef and functions to augment Eigen's MatrixXd

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

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

│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_MATRIX_DEFS

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF#define GTSAM_MAKE_MATRIX_DEFS( NeedsToAlign)N)
│ │ │ │
│ │ │ │ -Value:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
│ │ │ │ -
using _eigen_aligned_allocator_trait = void;
│ │ │ │ +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();
│ │ │ │
│ │ │ │ -

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,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 | _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_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_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_:_:_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_ _>_ _> │ │ │ │ │ +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  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() │ │ │ │ │ +#define  _G_T_S_A_M___M_A_K_E___M_A_T_R_I_X___D_E_F_S(N) │ │ │ │ │   │ │ │ │ │ -#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. │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef Eigen::MatrixXd  ggttssaamm::::MMaattrriixx │ │ │ │ │   │ │ │ │ │ -#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. │ │ │ │ │ +typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen:: │ │ │ │ │ + Dynamic, Eigen::RowMajor >  ggttssaamm::::MMaattrriixxRRoowwMMaajjoorr │ │ │ │ │   │ │ │ │ │ -#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. │ │ │ │ │ + typedef Eigen::Block< Matrix >  ggttssaamm::::SSuubbMMaattrriixx │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::uint64_t  ggttssaamm::::KKeeyy │ │ │ │ │ -  Integer nonlinear key type. │ │ │ │ │ + typedef Eigen::Block< const Matrix >  ggttssaamm::::CCoonnssttSSuubbMMaattrriixx │ │ │ │ │   │ │ │ │ │ -typedef std::uint64_t  ggttssaamm::::FFaaccttoorrIInnddeexx │ │ │ │ │ -  Integer nonlinear factor index type. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │   │ │ │ │ │ - typedef ptrdiff_t  ggttssaamm::::DDeennsseeIInnddeexx │ │ │ │ │ -  The index type for Eigen objects. │ │ │ │ │ +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) │ │ │ │ │   │ │ │ │ │ -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:: │ │ │ │ │ + Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ + Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │   │ │ │ │ │ -   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. │ │ │ │ │ + Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Aug 21, 2010 │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Kai Ni │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Alex Cunningham │ │ │ │ │ + Alex Hagiopol │ │ │ │ │ + Varun Agrawal │ │ │ │ │ ********** 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 ) │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__MMAATTRRIIXX__DDEEFFSS ********** │ │ │ │ │ +#define GTSAM_MAKE_MATRIX_DEFS (   N ) │ │ │ │ │ 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. │ │ │ │ │ +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 │ │ │ │ │ - * _t_y_p_e_s_._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/a00119.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,19 +1,46 @@ │ │ │ │ │ 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::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", "a00119.html#a3f9622226dfe06908f11b42bf0bdd22d", null], │ │ │ │ │ + ["assert_equal", "a00119.html#abb0e19bbbeaca95843e8161b89a12fda", null], │ │ │ │ │ + ["assert_inequal", "a00119.html#a7a116d0643f123ef3b15d91056506492", null], │ │ │ │ │ + ["backSubstituteLower", "a00119.html#a0d998e1b770c9864946ddb031b1c4522", null], │ │ │ │ │ + ["backSubstituteUpper", "a00119.html#a6c4876cbe85d5651a52eda0e97c60f2f", null], │ │ │ │ │ + ["backSubstituteUpper", "a00119.html#a8b0bf332d52b333dab2b20d763c8925b", null], │ │ │ │ │ + ["cholesky_inverse", "a00119.html#aba542e2cd85f08b76f80a0871a4ea713", null], │ │ │ │ │ + ["collect", "a00119.html#ad8b524ba6c9aed0d21a020999c9b5d88", null], │ │ │ │ │ + ["column", "a00119.html#a559dba69e2854eb66e34222f60f55722", null], │ │ │ │ │ + ["diag", "a00119.html#af1c32907adce74cf9edd6ee5bba5a085", null], │ │ │ │ │ + ["DLT", "a00119.html#ab2c65f1a69009a306b6a5f9ef31dcee2", null], │ │ │ │ │ + ["equal_with_abs_tol", "a00119.html#af31e657258505b2e5148846ebbaa3195", null], │ │ │ │ │ + ["expm", "a00119.html#ab15462d8c16813d0a7a5b1f76a2f64b7", null], │ │ │ │ │ + ["householder", "a00119.html#a7baa2f3184a444adce108633c0265e0c", null], │ │ │ │ │ + ["householder_", "a00119.html#abdd1ab05e8ac90b340fbd8f3b322dc6d", null], │ │ │ │ │ + ["inplace_QR", "a00119.html#a7eec9339ab5a008a28ddfaa6b2c94611", null], │ │ │ │ │ + ["insertSub", "a00119.html#ad1088fa2d1494e6a4123a6bc3f5c2d7e", null], │ │ │ │ │ + ["inverse_square_root", "a00119.html#aff78dee59ac0250432081f39deb5f6d1", null], │ │ │ │ │ + ["linear_dependent", "a00119.html#a4b81794af72954abafbb726fc712f5db", null], │ │ │ │ │ + ["linear_independent", "a00119.html#ad8f0349471464c1fb515819d9503849a", null], │ │ │ │ │ + ["operator!=", "a00119.html#ab235abf7505b634be2165e0db58239dd", null], │ │ │ │ │ + ["operator==", "a00119.html#afd5b3cf3f54adcbdd6d9e7403f1a792f", null], │ │ │ │ │ + ["operator>>", "a00119.html#a7d85957bab2d18cf56ab9aaf95a106de", null], │ │ │ │ │ + ["operator^", "a00119.html#a2218e53a2b99c449e70aa5b7805895fc", null], │ │ │ │ │ + ["print", "a00119.html#a54fa43c89c5334314c8c75939dd5c2d7", null], │ │ │ │ │ + ["print", "a00119.html#a44ead03912d5dcf094d8421e1702ee71", null], │ │ │ │ │ + ["prod", "a00119.html#abc29e3164ed30e785a3c48dfd1aa6ca5", null], │ │ │ │ │ + ["qr", "a00119.html#ae32c295e2c40c1e85f146a8a6266eaa8", null], │ │ │ │ │ + ["row", "a00119.html#a2754f325c8600303d627d9e8cf1f9949", null], │ │ │ │ │ + ["save", "a00119.html#a3eb7eed9019d3fda8fcf74fbf85b85c9", null], │ │ │ │ │ + ["skewSymmetric", "a00119.html#a97f108d19e52c83c331c55d35b23796e", null], │ │ │ │ │ + ["stack", "a00119.html#a5c82884a356ddd09229a5283aed04df9", null], │ │ │ │ │ + ["sub", "a00119.html#a9ae1d9fe2ccad044fbb88b5c1d9e035a", null], │ │ │ │ │ + ["svd", "a00119.html#a8d7e46204d953f64a39445599dbd7eee", null], │ │ │ │ │ + ["trans", "a00119.html#aa2e36d7ab63000feddaeb61bbfcf2db1", null], │ │ │ │ │ + ["vector_scale_inplace", "a00119.html#a24d3ad0252f91f0ec301593c45cf5af7", null], │ │ │ │ │ + ["weighted_eliminate", "a00119.html#a60019a03f26b92c6b2a08e43d153d4c0", null], │ │ │ │ │ + ["zeroBelowDiagonal", "a00119.html#a57edf6ed7312f63d35f73233665c334d", 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/Matrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,318 +98,397 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
types.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 * -------------------------------------------------------------------------- */
│ │ │ │
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
│ │ │ │ +
23// \callgraph
│ │ │ │ +
24
│ │ │ │ +
25#pragma once
│ │ │ │
26
│ │ │ │ -
27#include <cstddef>
│ │ │ │ -
28#include <cstdint>
│ │ │ │ -
29
│ │ │ │ -
30#include <exception>
│ │ │ │ -
31#include <string>
│ │ │ │ + │ │ │ │ +
28#include <gtsam/base/Vector.h>
│ │ │ │ +
29#include <boost/tuple/tuple.hpp>
│ │ │ │ +
30
│ │ │ │ +
31#include <vector>
│ │ │ │
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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 };
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
130 /* ************************************************************************* */
│ │ │ │ -
136 template<typename T, T defaultValue>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
192 {
│ │ │ │ -
193 int previousOpenMPThreads;
│ │ │ │ -
194
│ │ │ │ -
195 public:
│ │ │ │ -
196#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP
│ │ │ │ - │ │ │ │ -
198 previousOpenMPThreads(omp_get_num_threads())
│ │ │ │ -
199 {
│ │ │ │ -
200 omp_set_num_threads(omp_get_num_procs() / 4);
│ │ │ │ -
201 }
│ │ │ │ -
202
│ │ │ │ - │ │ │ │ -
204 {
│ │ │ │ -
205 omp_set_num_threads(previousOpenMPThreads);
│ │ │ │ -
206 }
│ │ │ │ -
207#else
│ │ │ │ -
208 TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {}
│ │ │ │ - │ │ │ │ -
210#endif
│ │ │ │ -
211 };
│ │ │ │ -
│ │ │ │ -
212
│ │ │ │ -
213#ifdef __clang__
│ │ │ │ -
214# pragma clang diagnostic pop
│ │ │ │ -
215#endif
│ │ │ │ -
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
│ │ │ │ -
259
│ │ │ │ -
260#endif
│ │ │ │ -
261
│ │ │ │ -
262#ifdef min
│ │ │ │ -
263#undef min
│ │ │ │ -
264#endif
│ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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;
│ │ │ │ +
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) {
│ │ │ │ +
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);
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Special class for optional Jacobian arguments.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
ListOfOneContainer< T > ListOfOne(const T &element)
Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
Definition types.h:178
│ │ │ │ -
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ -
std::uint64_t FactorIndex
Integer nonlinear factor index type.
Definition types.h:103
│ │ │ │ -
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
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
│ │ │ │ -
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
Helper class that uses templates to select between two types based on whether TEST_TYPE is const or n...
Definition types.h:115
│ │ │ │ -
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │ -
T & operator*()
Operator to access the value.
Definition types.h:147
│ │ │ │ -
const T & operator*() const
Operator to access the value.
Definition types.h:150
│ │ │ │ -
ValueWithDefault()
Default constructor, initialize to default value supplied in template argument.
Definition types.h:141
│ │ │ │ -
ValueWithDefault(const T &_value)
Initialize to the given value.
Definition types.h:144
│ │ │ │ -
A helper class that behaves as a container with one element, and works with boost::range.
Definition types.h:160
│ │ │ │ -
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ -
A SFINAE trait to mark classes that need special alignment.
Definition types.h:295
│ │ │ │ +
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition Matrix.cpp:367
│ │ │ │ +
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition Matrix.cpp:131
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
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
│ │ │ │ +
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition Matrix.cpp:377
│ │ │ │ +
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
│ │ │ │ +
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition Matrix.cpp:354
│ │ │ │ +
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition Matrix.cpp:174
│ │ │ │ +
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition Matrix.cpp:636
│ │ │ │ +
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │ +
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ +
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
│ │ │ │ +
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition Matrix.cpp:539
│ │ │ │ +
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition Matrix.h:144
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition Matrix.cpp:235
│ │ │ │ +
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition Matrix.cpp:207
│ │ │ │ +
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
│ │ │ │ +
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition Matrix.cpp:552
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
Reshape functor.
Definition Matrix.h:246
│ │ │ │ +
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition Matrix.h:456
│ │ │ │ +
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
│ │ │ │ +
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition Matrix.h:482
│ │ │ │ +
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
│ │ │ │ +
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition Matrix.h:494
│ │ │ │ +
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,343 +1,534 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -types.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 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include // for GTSAM_USE_TBB │ │ │ │ │ +23// \callgraph │ │ │ │ │ +24 │ │ │ │ │ +25#pragma once │ │ │ │ │ 26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ +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 │ │ │ │ │ -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 }; │ │ │ │ │ -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; } │ │ │ │ │ +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) { │ │ │ │ │ +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& 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 │ │ │ │ │ -_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} │ │ │ │ │ -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 │ │ │ │ │ -259 │ │ │ │ │ -260#endif │ │ │ │ │ -261 │ │ │ │ │ -262#ifdef min │ │ │ │ │ -263#undef min │ │ │ │ │ -264#endif │ │ │ │ │ -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; │ │ │ │ │ +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_:_:_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_:_:_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_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_:_:_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 │ │ │ │ │ - * _t_y_p_e_s_._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/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/cholesky.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,110 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Vector.cpp File Reference
│ │ │ │ +
cholesky.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

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

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

│ │ │ │ Namespaces

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

│ │ │ │ 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
 
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

│ │ │ │ -

typedef and functions to augment Eigen's Vectors

│ │ │ │ -
Author
Kai Ni
│ │ │ │ +

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

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Nov 5, 2010
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,105 +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 │ │ │ │ │ -Vector.cpp File Reference │ │ │ │ │ -typedef and functions to augment Eigen's Vectors _M_o_r_e_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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. │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ +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 ********** │ │ │ │ │ -typedef and functions to augment Eigen's Vectors │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + Nov 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_._c_p_p │ │ │ │ │ + * _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/a00125.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,19 +1,4 @@ │ │ │ │ │ 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] │ │ │ │ │ + ["choleskyCareful", "a00125.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ + ["choleskyPartial", "a00125.html#abe82fa6aceccfa2360cf314ab44f39ad", 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/GenericValue.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,140 +98,281 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ConcurrentMap.h
│ │ │ │ +
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#pragma once
│ │ │ │ -
20
│ │ │ │ - │ │ │ │ -
22
│ │ │ │ -
23// Change class depending on whether we are using TBB
│ │ │ │ -
24#ifdef GTSAM_USE_TBB
│ │ │ │ +
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 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>
│ │ │ │ - │ │ │ │ +
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
│ │ │ │ -
49#endif
│ │ │ │ +
49public:
│ │ │ │
50
│ │ │ │ -
51#include <boost/serialization/nvp.hpp>
│ │ │ │ -
52#include <boost/serialization/split_member.hpp>
│ │ │ │ -
53#include <boost/static_assert.hpp>
│ │ │ │ +
51 typedef T type;
│ │ │ │ +
52
│ │ │ │ +
53protected:
│ │ │ │
54
│ │ │ │ - │ │ │ │ + │ │ │ │
56
│ │ │ │ -
57namespace gtsam {
│ │ │ │ -
58
│ │ │ │ -
67template<typename KEY, typename VALUE>
│ │ │ │ -
│ │ │ │ -
68class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {
│ │ │ │ -
69
│ │ │ │ -
70public:
│ │ │ │ -
71
│ │ │ │ - │ │ │ │ -
73
│ │ │ │ - │ │ │ │ -
76
│ │ │ │ -
78 template<typename INPUTITERATOR>
│ │ │ │ -
79 ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ -
80
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ - │ │ │ │ -
119 ar & BOOST_SERIALIZATION_NVP(map);
│ │ │ │ -
120 this->insert(map.begin(), map.end());
│ │ │ │ -
121 }
│ │ │ │ -
122 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ -
123};
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
125}
│ │ │ │ -
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
Included from all GTSAM files.
│ │ │ │ -
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
│ │ │ │ +
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";
│ │ │ │ + │ │ │ │ +
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 }
│ │ │ │ +
│ │ │ │ +
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 {
│ │ │ │ + │ │ │ │ +
145 }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
│ │ │ │ +
148 Vector localCoordinates(const GenericValue& value2) const {
│ │ │ │ +
149 return localCoordinates_(value2);
│ │ │ │ +
150 }
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 size_t dim() const override {
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
172 Value::operator=(static_cast<Value const&>(rhs));
│ │ │ │ +
173 value_ = rhs.value_;
│ │ │ │ +
174 return *this;
│ │ │ │ +
175 }
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
177 private:
│ │ │ │ +
178
│ │ │ │ + │ │ │ │ +
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:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
213 return GenericValue<T>(v);
│ │ │ │ +
214}
│ │ │ │ +
│ │ │ │ +
215
│ │ │ │ +
216
│ │ │ │ +
217} /* namespace gtsam */
│ │ │ │ +
Typedefs for easier changing of types.
│ │ │ │ +
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
Base class and basic functions for Manifold types.
│ │ │ │ +
The base class for any variable that can be optimized or used in a factor.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
│ │ │ │ -
ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition ConcurrentMap.h:79
│ │ │ │ -
ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x)
Copy constructor from another ConcurrentMap.
Definition ConcurrentMap.h:82
│ │ │ │ -
bool exists(const KEY &e) const
Handy 'exists' function.
Definition ConcurrentMap.h:88
│ │ │ │ -
ConcurrentMap(const Base &x)
Copy constructor from the base map class.
Definition ConcurrentMap.h:85
│ │ │ │ -
ConcurrentMap()
Default constructor.
Definition ConcurrentMap.h:75
│ │ │ │ -
friend class boost::serialization::access
Serialization function.
Definition ConcurrentMap.h:104
│ │ │ │ -
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ +
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ +
Value * clone_() const override
Create a duplicate object returned as a pointer to the generic Value interface.
Definition GenericValue.h:102
│ │ │ │ +
T & value()
Return the value.
Definition GenericValue.h:72
│ │ │ │ +
Value * retract_(const Vector &delta) const override
Generic Value interface version of retract.
Definition GenericValue.h:122
│ │ │ │ +
Value & operator=(const Value &rhs) override
Assignment operator.
Definition GenericValue.h:158
│ │ │ │ +
void print(const std::string &str) const override
Virtual print function, uses traits.
Definition GenericValue.h:94
│ │ │ │ +
bool equals(const GenericValue &other, double tol=1e-9) const
non virtual equals function, uses traits
Definition GenericValue.h:89
│ │ │ │ +
size_t dim() const override
Return run-time dimensionality.
Definition GenericValue.h:153
│ │ │ │ +
GenericValue(const T &value)
Construct from value.
Definition GenericValue.h:62
│ │ │ │ +
void deallocate_() const override
Destroy and deallocate this object, only if it was originally allocated using clone_().
Definition GenericValue.h:110
│ │ │ │ +
T value_
The wrapped value.
Definition GenericValue.h:55
│ │ │ │ +
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
│ │ │ │ +
Vector localCoordinates(const GenericValue &value2) const
Non-virtual version of localCoordinates.
Definition GenericValue.h:148
│ │ │ │ +
friend class boost::serialization::access
Serialization function.
Definition GenericValue.h:180
│ │ │ │ +
bool equals_(const Value &p, double tol=1e-9) const override
equals implementing generic Value interface
Definition GenericValue.h:81
│ │ │ │ +
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ +
~GenericValue() override
Destructor.
Definition GenericValue.h:77
│ │ │ │ +
GenericValue retract(const Vector &delta) const
Non-virtual version of retract.
Definition GenericValue.h:143
│ │ │ │ +
Vector localCoordinates_(const Value &value2) const override
Generic Value interface version of localCoordinates.
Definition GenericValue.h:133
│ │ │ │ +
boost::shared_ptr< Value > clone() const override
Clone this value (normal clone on the heap, delete with 'delete' operator)
Definition GenericValue.h:117
│ │ │ │ +
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
const ValueType & cast() const
Cast to known ValueType.
Definition GenericValue.h:205
│ │ │ │ +
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,168 +1,324 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ConcurrentMap.h │ │ │ │ │ +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#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 │ │ │ │ │ +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 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_>; │ │ │ │ │ +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 │ │ │ │ │ -49#endif │ │ │ │ │ +49public: │ │ │ │ │ 50 │ │ │ │ │ -51#include │ │ │ │ │ -52#include │ │ │ │ │ -53#include │ │ │ │ │ +51 typedef T type; │ │ │ │ │ +52 │ │ │ │ │ +53protected: │ │ │ │ │ 54 │ │ │ │ │ -55#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +_5_5 T _v_a_l_u_e__; │ │ │ │ │ 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); │ │ │ │ │ +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 } │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +217} /* namespace gtsam */ │ │ │ │ │ +_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 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_V_a_l_u_e_._h │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ _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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * CCoonnccuurrrreennttMMaapp..hh │ │ │ │ │ + * 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/a00131.html │ │ │ │ @@ -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/Value.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,61 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
debug.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Value.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Global debugging flags. │ │ │ │ +

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.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -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 ()
 
│ │ │ │

Detailed Description

│ │ │ │ -

Global debugging flags.

│ │ │ │ +

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

│ │ │ │
Author
Richard Roberts
│ │ │ │ -
Date
Feb 1, 2011
│ │ │ │ +
Date
Jan 14, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 │ │ │ │ │ -debug.h File Reference │ │ │ │ │ -Global debugging flags. _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. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ -bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Global debugging flags. │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Feb 1, 2011 │ │ │ │ │ + Jan 14, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._h │ │ │ │ │ + * _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/a00131_source.html │ │ │ │ @@ -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/base/Value.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,86 +98,100 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
debug.h
│ │ │ │ +
Value.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/base/FastMap.h>
│ │ │ │ - │ │ │ │ -
21#include <string>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │
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}
│ │ │ │ +
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;
│ │ │ │ +
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
│ │ │ │ -
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)
│ │ │ │ -
67
│ │ │ │ -
68#endif
│ │ │ │ +
60 virtual size_t dim() const = 0;
│ │ │ │ +
61
│ │ │ │ +
68 virtual Value* retract_(const Vector& delta) const = 0;
│ │ │ │
69
│ │ │ │ -
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
Included from all GTSAM files.
│ │ │ │ +
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)
│ │ │ │ +
typedef and functions to augment Eigen's VectorXd
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
virtual size_t dim() const =0
Return the dimensionality of the tangent space of this value.
│ │ │ │ +
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 ...
│ │ │ │ +
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
│ │ │ │ +
virtual ~Value()
Virutal destructor.
Definition Value.h:89
│ │ │ │ +
virtual bool equals_(const Value &other, double tol=1e-9) const =0
Compare this Value with another for equality.
│ │ │ │ +
virtual Vector localCoordinates_(const Value &value) const =0
Compute the coordinates in the tangent space of this value that retract() would map to value.
│ │ │ │ +
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │ +
virtual void print(const std::string &str="") const =0
Print this value, for debugging and unit tests.
│ │ │ │ +
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
│ │ │ │ +
virtual boost::shared_ptr< Value > clone() const =0
Clone this value (normal clone on the heap, delete with 'delete' operator)
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,83 +1,119 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -debug.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 │ │ │ │ │ -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 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include // Configuration from CMake │ │ │ │ │ 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} │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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) │ │ │ │ │ -67 │ │ │ │ │ -68#endif │ │ │ │ │ +_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 │ │ │ │ │ -_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. │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._h │ │ │ │ │ + * _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/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/VerticalBlockMatrix.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymmetricBlockMatrix.h File Reference
│ │ │ │ +
VerticalBlockMatrix.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

A matrix with column blocks of pre-defined sizes. │ │ │ │ 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...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ 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
│ │ │ │ +

A matrix with column blocks of pre-defined sizes.

│ │ │ │ +

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** 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. │ │ │ │ │ +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 │ │ │ │ │ Sep 18, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/cholesky.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
WeightedSampler.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
cholesky.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Fast sampling without replacement. │ │ │ │ +

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky. │ │ │ │ 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

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

│ │ │ │ -

Fast sampling without replacement.

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

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

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Nov 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -WeightedSampler.h File Reference │ │ │ │ │ -Fast sampling without replacement. _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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Fast sampling without replacement. │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - May 2019 │ │ │ │ │ + Nov 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 │ │ │ │ │ + * _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/a00140.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00140 = [ │ │ │ │ │ - ["gtsam::WeightedSampler< Engine >", "a02664.html", "a02664"] │ │ │ │ │ + ["choleskyCareful", "a00140.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ + ["choleskyPartial", "a00140.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00140_source.html │ │ │ │ @@ -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/base/cholesky.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,152 +98,48 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
WeightedSampler.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
│ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Matrix.h>
│ │ │ │ +
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
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Definition WeightedSampler.h:37
│ │ │ │ -
WeightedSampler(Engine *engine)
Construct from random number generation engine We only store a pointer to it.
Definition WeightedSampler.h:46
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,150 +1,53 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -WeightedSampler.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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_._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/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/serializationTestHelpers.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,281 +98,202 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GenericValue.h
│ │ │ │ +
serializationTestHelpers.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 */
│ │ │ │ -
19
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23#include <gtsam/base/types.h>
│ │ │ │ -
24#include <gtsam/base/Value.h>
│ │ │ │ +
22#include <iostream>
│ │ │ │ +
23#include <sstream>
│ │ │ │ +
24#include <string>
│ │ │ │
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
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ -
│ │ │ │ + │ │ │ │ +
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;
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
94 void print(const std::string& str) const override {
│ │ │ │ -
95 std::cout << "(" << demangle(typeid(T).name()) << ")\n";
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ -
│ │ │ │ -
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);
│ │ │ │ +
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 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 }
│ │ │ │ -
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
143 GenericValue retract(const Vector& delta) const {
│ │ │ │ - │ │ │ │ -
145 }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
148 Vector localCoordinates(const GenericValue& value2) const {
│ │ │ │ -
149 return localCoordinates_(value2);
│ │ │ │ -
150 }
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 size_t dim() const override {
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ -
│ │ │ │ +
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 protected:
│ │ │ │ -
168
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
172 Value::operator=(static_cast<Value const&>(rhs));
│ │ │ │ -
173 value_ = rhs.value_;
│ │ │ │ -
174 return *this;
│ │ │ │ -
175 }
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ -
177 private:
│ │ │ │ -
178
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
213 return GenericValue<T>(v);
│ │ │ │ -
214}
│ │ │ │ -
│ │ │ │ -
215
│ │ │ │ -
216
│ │ │ │ -
217} /* namespace gtsam */
│ │ │ │ -
The base class for any variable that can be optimized or used in a factor.
│ │ │ │ -
Base class and basic functions for Manifold types.
│ │ │ │ -
Typedefs for easier changing of types.
│ │ │ │ -
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
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
│ │ │ │ +
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ -
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ -
Value * clone_() const override
Create a duplicate object returned as a pointer to the generic Value interface.
Definition GenericValue.h:102
│ │ │ │ -
T & value()
Return the value.
Definition GenericValue.h:72
│ │ │ │ -
Value * retract_(const Vector &delta) const override
Generic Value interface version of retract.
Definition GenericValue.h:122
│ │ │ │ -
Value & operator=(const Value &rhs) override
Assignment operator.
Definition GenericValue.h:158
│ │ │ │ -
void print(const std::string &str) const override
Virtual print function, uses traits.
Definition GenericValue.h:94
│ │ │ │ -
bool equals(const GenericValue &other, double tol=1e-9) const
non virtual equals function, uses traits
Definition GenericValue.h:89
│ │ │ │ -
size_t dim() const override
Return run-time dimensionality.
Definition GenericValue.h:153
│ │ │ │ -
GenericValue(const T &value)
Construct from value.
Definition GenericValue.h:62
│ │ │ │ -
void deallocate_() const override
Destroy and deallocate this object, only if it was originally allocated using clone_().
Definition GenericValue.h:110
│ │ │ │ -
T value_
The wrapped value.
Definition GenericValue.h:55
│ │ │ │ -
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
│ │ │ │ -
Vector localCoordinates(const GenericValue &value2) const
Non-virtual version of localCoordinates.
Definition GenericValue.h:148
│ │ │ │ -
friend class boost::serialization::access
Serialization function.
Definition GenericValue.h:180
│ │ │ │ -
bool equals_(const Value &p, double tol=1e-9) const override
equals implementing generic Value interface
Definition GenericValue.h:81
│ │ │ │ -
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ -
~GenericValue() override
Destructor.
Definition GenericValue.h:77
│ │ │ │ -
GenericValue retract(const Vector &delta) const
Non-virtual version of retract.
Definition GenericValue.h:143
│ │ │ │ -
Vector localCoordinates_(const Value &value2) const override
Generic Value interface version of localCoordinates.
Definition GenericValue.h:133
│ │ │ │ -
boost::shared_ptr< Value > clone() const override
Clone this value (normal clone on the heap, delete with 'delete' operator)
Definition GenericValue.h:117
│ │ │ │ -
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
const ValueType & cast() const
Cast to known ValueType.
Definition GenericValue.h:205
│ │ │ │ -
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │ +
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ +
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ +
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ +
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ +
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ +
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ +
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ +
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ +
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,324 +1,239 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GenericValue.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 │ │ │ │ │ -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> │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ 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 } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ -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); │ │ │ │ │ +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 _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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 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 │ │ │ │ │ -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 │ │ │ │ │ +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_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_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 │ │ │ │ │ - * GGeenneerriiccVVaalluuee..hh │ │ │ │ │ + * _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/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/Vector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,299 +94,186 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ Typedefs | │ │ │ │ Functions
│ │ │ │ -
Matrix.h File Reference
│ │ │ │ +
Vector.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

typedef and functions to augment Eigen's VectorXd │ │ │ │ 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)
 
│ │ │ │ +#define MKL_BLAS   MKL_DOMAIN_BLAS
 
#define GTSAM_MAKE_VECTOR_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
 
│ │ │ │ +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

│ │ │ │ -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::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
 
│ │ │ │

Detailed Description

│ │ │ │ -

typedef and functions to augment Eigen's MatrixXd

│ │ │ │ -
Author
Christian Potthast
│ │ │ │ -
│ │ │ │ -Kai Ni
│ │ │ │ +

typedef and functions to augment Eigen's VectorXd

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

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_MAKE_MATRIX_DEFS

│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_VECTOR_DEFS

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
#define GTSAM_MAKE_MATRIX_DEFS#define GTSAM_MAKE_VECTOR_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();
│ │ │ │ +Value:
using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ +
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,308 +1,147 @@ │ │ │ │ │ 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 | _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_. │ │ │ │ │ -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) │ │ │ │ │ +#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::MatrixXd  ggttssaamm::::MMaattrriixx │ │ │ │ │ + typedef Eigen::VectorXd  ggttssaamm::::VVeeccttoorr │ │ │ │ │   │ │ │ │ │ -typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen:: │ │ │ │ │ - Dynamic, Eigen::RowMajor >  ggttssaamm::::MMaattrriixxRRoowwMMaajjoorr │ │ │ │ │ + typedef Eigen::Matrix< double, 1, 1 >  ggttssaamm::::VVeeccttoorr11 │ │ │ │ │   │ │ │ │ │ - typedef Eigen::Block< Matrix >  ggttssaamm::::SSuubbMMaattrriixx │ │ │ │ │ + typedef Eigen::Vector2d  ggttssaamm::::VVeeccttoorr22 │ │ │ │ │   │ │ │ │ │ - typedef Eigen::Block< const Matrix >  ggttssaamm::::CCoonnssttSSuubbMMaattrriixx │ │ │ │ │ + typedef Eigen::Vector3d  ggttssaamm::::VVeeccttoorr33 │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │ + typedef Eigen::VectorBlock< Vector >  ggttssaamm::::SSuubbVVeeccttoorr │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ +typedef Eigen::VectorBlock< const Vector >  ggttssaamm::::CCoonnssttSSuubbVVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ - Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │ + double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ + Vector &weights, Vector &pseudo) │ │ │ │ │   │ │ │ │ │ - Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ 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 ) │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__VVEECCTTOORR__DDEEFFSS ********** │ │ │ │ │ +#define GTSAM_MAKE_VECTOR_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(); │ │ │ │ │ +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 │ │ │ │ │ - * _M_a_t_r_i_x_._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/a00146.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,46 +1,23 @@ │ │ │ │ │ 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_equal", "a00146.html#ac3cf8e8e70cd696d9c789c31bc1e89a7", null], │ │ │ │ │ + ["assert_equal", "a00146.html#a9f3ee662d25ffb8c04b4e35c4b02e90b", null], │ │ │ │ │ + ["assert_inequal", "a00146.html#a2ca3db47b15350977c1f03c5560ab332", null], │ │ │ │ │ + ["concatVectors", "a00146.html#ae1c88acfa9575bcea3f2e738cb637a85", null], │ │ │ │ │ + ["concatVectors", "a00146.html#a09610ec69a8b3f3a00399bb378eda9cf", null], │ │ │ │ │ + ["dot", "a00146.html#ad80249acf12bbea741e755cd8fc73042", null], │ │ │ │ │ + ["ediv_", "a00146.html#a14844c96706dc362917e0d5e76d65fbe", null], │ │ │ │ │ + ["equal", "a00146.html#a3d1cad2313f69f9fa5008fdc348d1526", null], │ │ │ │ │ + ["equal", "a00146.html#aa7ed1d4fe2dd6828cfeb2d8f488b818d", null], │ │ │ │ │ + ["equal_with_abs_tol", "a00146.html#a8bdced1844ffd6a53af0396d82eaa6da", null], │ │ │ │ │ + ["fpEqual", "a00146.html#ae19c359190a379cba44bf818a2293f7c", null], │ │ │ │ │ + ["greaterThanOrEqual", "a00146.html#a368ee40bd9c0124d572d2e49bcb077bf", null], │ │ │ │ │ + ["house", "a00146.html#afb77eefc25dea0fb3e0fcc8f246b617a", null], │ │ │ │ │ + ["houseInPlace", "a00146.html#ac9c0c05b0198ec0fb6e4115edb755910", null], │ │ │ │ │ + ["inner_prod", "a00146.html#a4ca41ba9ec9d6d21a2b9f5e00f9f25ed", null], │ │ │ │ │ + ["linear_dependent", "a00146.html#a2a0cfd7908b06491df49b6a9c9186775", null], │ │ │ │ │ + ["operator==", "a00146.html#a64988014ab746343803620dc42513646", null], │ │ │ │ │ + ["print", "a00146.html#a760cc36e9009b23cf564c436294b3504", null], │ │ │ │ │ + ["print", "a00146.html#a5e5d8018a0f44a6a299406aeda27d44b", null], │ │ │ │ │ + ["save", "a00146.html#a09f2bbdb9f9d633542362dbe8d79f9ab", null], │ │ │ │ │ + ["weightedPseudoinverse", "a00146.html#aca1c56d11a05464a7a5458dc32ccc777", 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/Vector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,397 +98,192 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Matrix.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
│ │ │ │ -
23// \callgraph
│ │ │ │ -
24
│ │ │ │ -
25#pragma once
│ │ │ │ -
26
│ │ │ │ - │ │ │ │ -
28#include <gtsam/base/Vector.h>
│ │ │ │ -
29#include <boost/tuple/tuple.hpp>
│ │ │ │ -
30
│ │ │ │ -
31#include <vector>
│ │ │ │ +
21// \callgraph
│ │ │ │ +
22
│ │ │ │ +
23#pragma once
│ │ │ │ +
24#ifndef MKL_BLAS
│ │ │ │ +
25#define MKL_BLAS MKL_DOMAIN_BLAS
│ │ │ │ +
26#endif
│ │ │ │ +
27
│ │ │ │ + │ │ │ │ +
29#include <Eigen/Core>
│ │ │ │ +
30#include <iosfwd>
│ │ │ │ +
31#include <list>
│ │ │ │
32
│ │ │ │ -
38namespace gtsam {
│ │ │ │ +
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
│ │ │ │ -
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) {
│ │ │ │ -
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}
│ │ │ │ -
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
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);
│ │ │ │ +
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 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);
│ │ │ │ +
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
│ │ │ │ -
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>
│ │ │ │ +
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>
│ │ │ │
│ │ │ │ -
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}
│ │ │ │ -
│ │ │ │ +
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
│ │ │ │ -
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);
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
Special class for optional Jacobian arguments.
│ │ │ │ -
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
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
│ │ │ │ +
Included from all GTSAM files.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition Matrix.cpp:367
│ │ │ │ -
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition Matrix.cpp:131
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ +
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition Vector.cpp:199
│ │ │ │ +
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
│ │ │ │
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
│ │ │ │
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │
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
│ │ │ │ +
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition Vector.h:202
│ │ │ │
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition Matrix.cpp:377
│ │ │ │ +
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │
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
│ │ │ │ -
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition Matrix.cpp:354
│ │ │ │ -
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition Matrix.cpp:174
│ │ │ │ -
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition Matrix.cpp:636
│ │ │ │ -
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │ -
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ -
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
│ │ │ │ -
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition Matrix.cpp:539
│ │ │ │ -
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition Matrix.h:144
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ +
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition Vector.cpp:212
│ │ │ │ +
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ +
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
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
│ │ │ │ -
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition Matrix.cpp:235
│ │ │ │ -
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition Matrix.cpp:207
│ │ │ │ +
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition Vector.cpp:302
│ │ │ │
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
│ │ │ │ +
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
│ │ │ │
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ -
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition Matrix.cpp:552
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
Reshape functor.
Definition Matrix.h:246
│ │ │ │ -
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition Matrix.h:456
│ │ │ │ -
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
│ │ │ │ -
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition Matrix.h:482
│ │ │ │ -
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
│ │ │ │ -
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition Matrix.h:494
│ │ │ │ -
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,534 +1,247 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Matrix.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 │ │ │ │ │ -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 │ │ │ │ │ +21// \callgraph │ │ │ │ │ +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 │ │ │ │ │ -38namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -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) { │ │ │ │ │ -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& 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& │ │ │ │ │ +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); │ │ │ │ │ -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:: │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -_2_4_2inline Matrix _t_r_a_n_s(const Matrix& A) { return A.transpose(); } │ │ │ │ │ -243 │ │ │ │ │ -245template │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_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_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_._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/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/BasisFactors.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Chebyshev.h 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

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...
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...
 
struct  gtsam::Chebyshev2Basis
 Basis of Chebyshev polynomials of the second kind. 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,32 +1,58 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -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_._._. │ │ │ │ │ +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_._h │ │ │ │ │ + * _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/a00152.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,6 @@ │ │ │ │ │ var a00152 = [ │ │ │ │ │ - ["gtsam::Chebyshev1Basis", "a02732.html", null], │ │ │ │ │ - ["gtsam::Chebyshev2Basis", "a02736.html", null] │ │ │ │ │ + ["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"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00152_source.html │ │ │ │ @@ -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/basis/BasisFactors.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Chebyshev.h
│ │ │ │ +
BasisFactors.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,56 +114,251 @@ │ │ │ │
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>
│ │ │ │ +
21#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
│ │ │ │ -
Base class and basic functions for Manifold types.
│ │ │ │ -
Compute an interpolating basis.
│ │ │ │ +
38template <class BASIS>
│ │ │ │ +
│ │ │ │ +
39class EvaluationFactor : public FunctorizedFactor<double, Vector> {
│ │ │ │ +
40 private:
│ │ │ │ + │ │ │ │ +
42
│ │ │ │ +
43 public:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
96 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ +
97 private:
│ │ │ │ + │ │ │ │ +
99
│ │ │ │ +
100 public:
│ │ │ │ + │ │ │ │ +
102
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
161 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ +
162 private:
│ │ │ │ + │ │ │ │ +
164
│ │ │ │ +
165 public:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
230 : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
│ │ │ │ +
231 private:
│ │ │ │ + │ │ │ │ +
233
│ │ │ │ +
234 public:
│ │ │ │ + │ │ │ │ +
236
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
248 const size_t N, double x)
│ │ │ │ +
249 : Base(key, z, model,
│ │ │ │ +
250 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
│ │ │ │ +
│ │ │ │ +
251
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
282 : public FunctorizedFactor<double, typename BASIS::Parameters> {
│ │ │ │ +
283 private:
│ │ │ │ + │ │ │ │ +
285
│ │ │ │ +
286 public:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
331 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ +
332 private:
│ │ │ │ + │ │ │ │ +
334 using Func = typename BASIS::template VectorDerivativeFunctor<M>;
│ │ │ │ +
335
│ │ │ │ +
336 public:
│ │ │ │ + │ │ │ │ +
338
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
350 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
351 double x)
│ │ │ │ +
352 : Base(key, z, model, Func(N, x)) {}
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
384 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ +
385 private:
│ │ │ │ + │ │ │ │ +
387 using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
│ │ │ │ +
388
│ │ │ │ +
389 public:
│ │ │ │ + │ │ │ │ +
391
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
406 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
407 size_t i, double x)
│ │ │ │ +
408 : Base(key, z, model, Func(N, i, x)) {}
│ │ │ │ +
│ │ │ │ +
409
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
Compute an interpolating basis.
│ │ │ │ + │ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#...
Definition Chebyshev.h:32
│ │ │ │ -
Basis of Chebyshev polynomials of the second kind.
Definition Chebyshev.h:80
│ │ │ │ +
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ +
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:55
│ │ │ │ +
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
│ │ │ │ +
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M,...
Definition BasisFactors.h:96
│ │ │ │ +
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
│ │ │ │ +
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:113
│ │ │ │ +
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P,...
Definition BasisFactors.h:161
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
For a measurement value of type T i.e.
Definition BasisFactors.h:230
│ │ │ │ +
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
│ │ │ │ +
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:247
│ │ │ │ +
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:282
│ │ │ │ +
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
│ │ │ │ +
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:299
│ │ │ │ +
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:331
│ │ │ │ +
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:349
│ │ │ │ +
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
│ │ │ │ +
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the ...
Definition BasisFactors.h:384
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ +
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Chebyshev.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,56 +16,297 @@ │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_._h │ │ │ │ │ + * _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/a00155_source.html │ │ │ │ @@ -114,16 +114,16 @@ │ │ │ │

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>
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │
24#include <gtsam/base/VectorSpace.h>
│ │ │ │
25
│ │ │ │
26#include <iostream>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │
37template <int M>
│ │ │ │ @@ -260,16 +260,16 @@ │ │ │ │
209inline std::ostream& operator<<(std::ostream& os,
│ │ │ │
210 const ParameterMatrix<M>& parameterMatrix) {
│ │ │ │
211 os << parameterMatrix.matrix();
│ │ │ │
212 return os;
│ │ │ │
213}
│ │ │ │
214
│ │ │ │
215} // namespace gtsam
│ │ │ │ -
Concept check for values that can be used in unit tests.
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Concept check for values that can be used in unit tests.
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
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
│ │ │ │
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │
MatrixType operator*(const Eigen::Matrix< double, -1, -1 > &other) const
Multiply ParameterMatrix with an Eigen matrix.
Definition ParameterMatrix.h:152
│ │ │ │
ParameterMatrix< M > operator-(const Eigen::Matrix< double, -1, 1 > &other) const
Subtract a MxN-sized vector from the ParameterMatrix.
Definition ParameterMatrix.h:141
│ │ │ ├── ./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/Basis.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,83 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Fourier.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Basis.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

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

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

│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ Classes

class  gtsam::FourierBasis
 Fourier basis. More...
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

│ │ │ │ -

Fourier decomposition, see e.g.

│ │ │ │ -

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

Author
Varun Agrawal, Frank Dellaert
│ │ │ │ +

Compute an interpolating basis.

│ │ │ │ +
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,71 @@ │ │ │ │ │ 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 | _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_:_:_F_o_u_r_i_e_r_B_a_s_i_s │ │ │ │ │ -  Fourier basis. _M_o_r_e_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -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 │ │ │ │ │ +Compute an interpolating basis. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal, Frank Dellaert │ │ │ │ │ + Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _F_o_u_r_i_e_r_._h │ │ │ │ │ + * _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/a00158.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,11 @@ │ │ │ │ │ var a00158 = [ │ │ │ │ │ - ["gtsam::FourierBasis", "a02748.html", null] │ │ │ │ │ + ["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", "a00158.html#ga2ce44d31cae1eda44c2a86d32d92b935", 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/Basis.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,106 +98,473 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Fourier.h
│ │ │ │ +
Basis.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/basis/Basis.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
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) {
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
Compute an interpolating basis.
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Matrix.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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
138 protected:
│ │ │ │ +
139 Weights weights_;
│ │ │ │ +
140
│ │ │ │ +
141 public:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
179 protected:
│ │ │ │ +
180 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ +
181 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ +
182 Jacobian H_;
│ │ │ │ +
183
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
193 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ +
194 }
│ │ │ │ +
│ │ │ │ +
195
│ │ │ │ +
196 public:
│ │ │ │ +
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
198
│ │ │ │ + │ │ │ │ +
201
│ │ │ │ +
│ │ │ │ +
203 VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
│ │ │ │ + │ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
│ │ │ │ +
208 VectorEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
209 : EvaluationFunctor(N, x, a, b) {
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
222 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
223 return apply(P, H);
│ │ │ │ +
224 }
│ │ │ │ +
│ │ │ │ +
225 };
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
234 template <int M>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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:
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
281 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
282 return apply(P, H);
│ │ │ │ +
283 }
│ │ │ │ +
│ │ │ │ +
284 };
│ │ │ │ +
│ │ │ │ +
285
│ │ │ │ +
299 template <class T>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
301 : public VectorEvaluationFunctor<traits<T>::dimension> {
│ │ │ │ +
302 enum { M = traits<T>::dimension };
│ │ │ │ +
303 using Base = VectorEvaluationFunctor<M>;
│ │ │ │ +
304
│ │ │ │ +
305 public:
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
337 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
338 return apply(P, H); // might call apply in derived
│ │ │ │ +
339 }
│ │ │ │ +
│ │ │ │ +
340 };
│ │ │ │ +
│ │ │ │ +
341
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
344 protected:
│ │ │ │ +
345 Weights weights_;
│ │ │ │ +
346
│ │ │ │ +
347 public:
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
370 public:
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
401 protected:
│ │ │ │ +
402 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ +
403 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ +
404 Jacobian H_;
│ │ │ │ +
405
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
415 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ +
416 }
│ │ │ │ +
│ │ │ │ +
417
│ │ │ │ +
418 public:
│ │ │ │ +
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
420
│ │ │ │ + │ │ │ │ +
423
│ │ │ │ +
│ │ │ │ +
425 VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
│ │ │ │ + │ │ │ │ +
427 }
│ │ │ │ +
│ │ │ │ +
428
│ │ │ │ +
│ │ │ │ +
430 VectorDerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
431 : DerivativeFunctorBase(N, x, a, b) {
│ │ │ │ + │ │ │ │ +
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>
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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:
│ │ │ │ + │ │ │ │ +
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 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
500 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
501 return apply(P, H);
│ │ │ │ +
502 }
│ │ │ │ +
│ │ │ │ +
503 };
│ │ │ │ +
│ │ │ │ +
504
│ │ │ │ +
505};
│ │ │ │ +
│ │ │ │ +
506
│ │ │ │ +
507} // namespace gtsam
│ │ │ │ +
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Special class for optional Jacobian arguments.
│ │ │ │ +
Define ParameterMatrix class which is used to store values at interpolation points.
│ │ │ │ +
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
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
Fourier basis.
Definition Fourier.h:27
│ │ │ │ -
static DiffMatrix DifferentiationMatrix(size_t N)
Compute D = differentiation matrix.
Definition Fourier.h:73
│ │ │ │ -
static Weights DerivativeWeights(size_t N, double x)
Get weights at a given x that calculate the derivative.
Definition Fourier.h:92
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ +
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
│ │ │ │ +
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
│ │ │ │ +
An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p.
Definition Basis.h:137
│ │ │ │ +
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
│ │ │ │ +
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
│ │ │ │ +
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
│ │ │ │ +
EvaluationFunctor()
For serialization.
Definition Basis.h:143
│ │ │ │ +
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
│ │ │ │ +
VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:178
│ │ │ │ +
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
│ │ │ │ +
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
│ │ │ │ +
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
│ │ │ │ +
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
│ │ │ │ +
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
│ │ │ │ +
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
│ │ │ │ +
Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor compute...
Definition Basis.h:235
│ │ │ │ +
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
│ │ │ │ +
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
│ │ │ │ +
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
│ │ │ │ +
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
│ │ │ │ +
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
│ │ │ │ +
Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:301
│ │ │ │ +
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
│ │ │ │ +
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
│ │ │ │ +
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
│ │ │ │ +
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
│ │ │ │ +
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
│ │ │ │ +
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
│ │ │ │ +
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
│ │ │ │ +
An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p.
Definition Basis.h:369
│ │ │ │ +
DerivativeFunctor()
For serialization.
Definition Basis.h:372
│ │ │ │ +
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
│ │ │ │ +
VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:400
│ │ │ │ +
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
│ │ │ │ +
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
│ │ │ │ +
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
│ │ │ │ +
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
│ │ │ │ +
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
│ │ │ │ +
Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor com...
Definition Basis.h:456
│ │ │ │ +
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
│ │ │ │ +
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
│ │ │ │ +
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
│ │ │ │ +
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
│ │ │ │ +
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
│ │ │ │ +
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,109 +1,554 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Fourier.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -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 } │ │ │ │ │ -109 │ │ │ │ │ -110}; // FourierBasis │ │ │ │ │ -111 │ │ │ │ │ -112} // namespace gtsam │ │ │ │ │ -_B_a_s_i_s_._h │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ +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() {} │ │ │ │ │ +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 │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _F_o_u_r_i_e_r_._h │ │ │ │ │ + * _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.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/Chebyshev.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Chebyshev2.h File Reference
│ │ │ │ +
Chebyshev.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

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

Chebyshev basis decompositions. │ │ │ │ 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...
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

│ │ │ │ -

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.

│ │ │ │ +

Chebyshev basis decompositions.

│ │ │ │
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -Chebyshev2.h File Reference │ │ │ │ │ -Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** 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. │ │ │ │ │ +Chebyshev basis decompositions. │ │ │ │ │ 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_._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/a00161.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00161 = [ │ │ │ │ │ - ["gtsam::Chebyshev2", "a02740.html", null] │ │ │ │ │ + ["gtsam::Chebyshev1Basis", "a02732.html", null], │ │ │ │ │ + ["gtsam::Chebyshev2Basis", "a02736.html", 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/Chebyshev.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,122 +98,72 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Chebyshev2.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
│ │ │ │ -
33#pragma once
│ │ │ │ +
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#include <gtsam/base/Manifold.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
│ │ │ │ -
Special class for optional Jacobian arguments.
│ │ │ │ -
Base class and basic functions for Manifold types.
│ │ │ │ -
Compute an interpolating basis.
│ │ │ │ +
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
│ │ │ │ +
Base class and basic functions for Manifold types.
│ │ │ │ +
Compute an interpolating basis.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points...
Definition Chebyshev2.h:46
│ │ │ │ -
static double Point(size_t N, int j)
Specific Chebyshev point.
Definition Chebyshev2.h:55
│ │ │ │ -
static double Point(size_t N, int j, double a, double b)
Specific Chebyshev point, within [a,b] interval.
Definition Chebyshev2.h:64
│ │ │ │ -
static Vector Points(size_t N)
All Chebyshev points.
Definition Chebyshev2.h:72
│ │ │ │ -
static Vector Points(size_t N, double a, double b)
All Chebyshev points, within [a,b] interval.
Definition Chebyshev2.h:79
│ │ │ │ -
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
│ │ │ │ +
Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#...
Definition Chebyshev.h:32
│ │ │ │ +
Basis of Chebyshev polynomials of the second kind.
Definition Chebyshev.h:80
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,128 +1,71 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Chebyshev2.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 │ │ │ │ │ -33#pragma once │ │ │ │ │ +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#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. │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_2_._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/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>
│ │ │ │ - │ │ │ │ +
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:
│ │ │ │ - │ │ │ │ -
42
│ │ │ │ -
43 public:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
96 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ -
97 private:
│ │ │ │ - │ │ │ │ -
99
│ │ │ │ -
100 public:
│ │ │ │ - │ │ │ │ -
102
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
161 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ -
162 private:
│ │ │ │ - │ │ │ │ -
164
│ │ │ │ -
165 public:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
230 : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
│ │ │ │ -
231 private:
│ │ │ │ - │ │ │ │ -
233
│ │ │ │ -
234 public:
│ │ │ │ - │ │ │ │ -
236
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
248 const size_t N, double x)
│ │ │ │ -
249 : Base(key, z, model,
│ │ │ │ -
250 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
│ │ │ │ -
│ │ │ │ -
251
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
282 : public FunctorizedFactor<double, typename BASIS::Parameters> {
│ │ │ │ -
283 private:
│ │ │ │ - │ │ │ │ -
285
│ │ │ │ -
286 public:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
331 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ -
332 private:
│ │ │ │ - │ │ │ │ -
334 using Func = typename BASIS::template VectorDerivativeFunctor<M>;
│ │ │ │ -
335
│ │ │ │ -
336 public:
│ │ │ │ - │ │ │ │ -
338
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
350 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
351 double x)
│ │ │ │ -
352 : Base(key, z, model, Func(N, x)) {}
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
384 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ -
385 private:
│ │ │ │ - │ │ │ │ -
387 using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
│ │ │ │ -
388
│ │ │ │ -
389 public:
│ │ │ │ - │ │ │ │ -
391
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
406 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
407 size_t i, double x)
│ │ │ │ -
408 : Base(key, z, model, Func(N, i, x)) {}
│ │ │ │ -
│ │ │ │ -
409
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
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) {
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ +
Compute an interpolating basis.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ -
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:55
│ │ │ │ -
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
│ │ │ │ -
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M,...
Definition BasisFactors.h:96
│ │ │ │ -
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
│ │ │ │ -
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:113
│ │ │ │ -
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P,...
Definition BasisFactors.h:161
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
For a measurement value of type T i.e.
Definition BasisFactors.h:230
│ │ │ │ -
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
│ │ │ │ -
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:247
│ │ │ │ -
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:282
│ │ │ │ -
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
│ │ │ │ -
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:299
│ │ │ │ -
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:331
│ │ │ │ -
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:349
│ │ │ │ -
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
│ │ │ │ -
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the ...
Definition BasisFactors.h:384
│ │ │ │ -
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
│ │ │ │ -
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
│ │ │ │ -
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ -
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ +
Fourier basis.
Definition Fourier.h:27
│ │ │ │ +
static DiffMatrix DifferentiationMatrix(size_t N)
Compute D = differentiation matrix.
Definition Fourier.h:73
│ │ │ │ +
static Weights DerivativeWeights(size_t N, double x)
Get weights at a given x that calculate the derivative.
Definition Fourier.h:92
│ │ │ │ +
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
│ │ │ │ +
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
│ │ │ │ +
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/Chebyshev2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

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

Chebyshev basis decompositions. │ │ │ │ +

Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ 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...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

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

Detailed Description

│ │ │ │ -

Chebyshev basis decompositions.

│ │ │ │ +

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,22 +1,39 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_:_:_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chebyshev basis decompositions. │ │ │ │ │ +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 │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_._c_p_p │ │ │ │ │ + * _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.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/FitBasis.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,82 +96,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Basis.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
FitBasis.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Compute an interpolating basis. │ │ │ │ +

Fit a Basis using least-squares. │ │ │ │ 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::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::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.
 
│ │ │ │ +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

│ │ │ │ -

Compute an interpolating basis.

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

Fit a Basis using least-squares.

│ │ │ │
Date
July 4, 2020
│ │ │ │ +
Author
Varun Agrawal, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,71 +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 | _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 | _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_:_:_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_:_:_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::::WWeeiigghhttss = Eigen::Matrix< double, 1, -1 > │ │ │ │ │ +using  ggttssaamm::::SSeeqquueennccee = std::map< double, double > │ │ │ │ │ +  Our sequence representation is a map of {x: y} values where y = f(x) │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +using  ggttssaamm::::SSaammppllee = std::pair< double, double > │ │ │ │ │ +  A sample is a key-value pair from a sequence. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ - 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 │ │ │ │ │ - * _B_a_s_i_s_._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/a00173.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,5 @@ │ │ │ │ │ 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::FitBasis< Basis >", "a02744.html", "a02744"], │ │ │ │ │ + ["Sample", "a00173.html#a9920fe883a2f506440aca7c1a2bd1a00", null], │ │ │ │ │ + ["Sequence", "a00173.html#a24ec5d6ec6b211d0cbaf645c11c198de", 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/FitBasis.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,473 +98,119 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Basis.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
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ - │ │ │ │ - │ │ │ │ +
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#include <iostream>
│ │ │ │ +
25#pragma once
│ │ │ │
26
│ │ │ │ -
68namespace gtsam {
│ │ │ │ -
69
│ │ │ │ -
70using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
│ │ │ │ +
27#include <gtsam/basis/Basis.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:
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
62 const SharedNoiseModel& model,
│ │ │ │ +
63 size_t N) {
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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}
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
138 protected:
│ │ │ │ -
139 Weights weights_;
│ │ │ │ -
140
│ │ │ │ -
141 public:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
179 protected:
│ │ │ │ -
180 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ -
181 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ -
182 Jacobian H_;
│ │ │ │ -
183
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
193 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ -
194 }
│ │ │ │ -
│ │ │ │ -
195
│ │ │ │ -
196 public:
│ │ │ │ -
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
198
│ │ │ │ - │ │ │ │ -
201
│ │ │ │ -
│ │ │ │ -
203 VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
│ │ │ │ - │ │ │ │ -
205 }
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
│ │ │ │ -
208 VectorEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
209 : EvaluationFunctor(N, x, a, b) {
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
222 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
223 return apply(P, H);
│ │ │ │ -
224 }
│ │ │ │ -
│ │ │ │ -
225 };
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
234 template <int M>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
281 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
282 return apply(P, H);
│ │ │ │ -
283 }
│ │ │ │ -
│ │ │ │ -
284 };
│ │ │ │ -
│ │ │ │ -
285
│ │ │ │ -
299 template <class T>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
301 : public VectorEvaluationFunctor<traits<T>::dimension> {
│ │ │ │ -
302 enum { M = traits<T>::dimension };
│ │ │ │ -
303 using Base = VectorEvaluationFunctor<M>;
│ │ │ │ -
304
│ │ │ │ -
305 public:
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
337 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
338 return apply(P, H); // might call apply in derived
│ │ │ │ -
339 }
│ │ │ │ -
│ │ │ │ -
340 };
│ │ │ │ -
│ │ │ │ -
341
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
344 protected:
│ │ │ │ -
345 Weights weights_;
│ │ │ │ -
346
│ │ │ │ -
347 public:
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
370 public:
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
401 protected:
│ │ │ │ -
402 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ -
403 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ -
404 Jacobian H_;
│ │ │ │ -
405
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
415 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ -
416 }
│ │ │ │ -
│ │ │ │ -
417
│ │ │ │ -
418 public:
│ │ │ │ -
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
420
│ │ │ │ - │ │ │ │ -
423
│ │ │ │ -
│ │ │ │ -
425 VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
│ │ │ │ - │ │ │ │ -
427 }
│ │ │ │ -
│ │ │ │ -
428
│ │ │ │ -
│ │ │ │ -
430 VectorDerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
431 : DerivativeFunctorBase(N, x, a, b) {
│ │ │ │ - │ │ │ │ -
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>
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
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:
│ │ │ │ - │ │ │ │ -
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 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
500 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
501 return apply(P, H);
│ │ │ │ -
502 }
│ │ │ │ -
│ │ │ │ -
503 };
│ │ │ │ -
│ │ │ │ -
504
│ │ │ │ -
505};
│ │ │ │ -
│ │ │ │ -
506
│ │ │ │ -
507} // namespace gtsam
│ │ │ │ -
Special class for optional Jacobian arguments.
│ │ │ │ -
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
Define ParameterMatrix class which is used to store values at interpolation points.
│ │ │ │ -
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
│ │ │ │ +
96 Parameters parameters() const { return parameters_; }
│ │ │ │ +
97};
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
99} // namespace gtsam
│ │ │ │ +
Factor definitions for various Basis functors.
│ │ │ │ +
Compute an interpolating basis.
│ │ │ │ +
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
Factor Graph Values.
│ │ │ │ +
Factor Graph consisting of non-linear factors.
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
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
│ │ │ │ -
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
│ │ │ │ -
An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p.
Definition Basis.h:137
│ │ │ │ -
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
│ │ │ │ -
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
│ │ │ │ -
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
│ │ │ │ -
EvaluationFunctor()
For serialization.
Definition Basis.h:143
│ │ │ │ -
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
│ │ │ │ -
VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:178
│ │ │ │ -
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
│ │ │ │ -
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
│ │ │ │ -
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
│ │ │ │ -
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
│ │ │ │ -
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
│ │ │ │ -
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
│ │ │ │ -
Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor compute...
Definition Basis.h:235
│ │ │ │ -
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
│ │ │ │ -
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
│ │ │ │ -
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
│ │ │ │ -
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
│ │ │ │ -
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
│ │ │ │ -
Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:301
│ │ │ │ -
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
│ │ │ │ -
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
│ │ │ │ -
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
│ │ │ │ -
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
│ │ │ │ -
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
│ │ │ │ -
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
│ │ │ │ -
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
│ │ │ │ -
An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p.
Definition Basis.h:369
│ │ │ │ -
DerivativeFunctor()
For serialization.
Definition Basis.h:372
│ │ │ │ -
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
│ │ │ │ -
VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:400
│ │ │ │ -
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
│ │ │ │ -
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
│ │ │ │ -
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
│ │ │ │ -
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
│ │ │ │ -
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
│ │ │ │ -
Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor com...
Definition Basis.h:456
│ │ │ │ -
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
│ │ │ │ -
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
│ │ │ │ -
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
│ │ │ │ -
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
│ │ │ │ -
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
│ │ │ │ -
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │ +
std::map< double, double > Sequence
Our sequence representation is a map of {x: y} values where y = f(x)
Definition FitBasis.h:36
│ │ │ │ +
std::pair< double, double > Sample
A sample is a key-value pair from a sequence.
Definition FitBasis.h:38
│ │ │ │ +
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ +
Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebysh...
Definition FitBasis.h:52
│ │ │ │ +
static NonlinearFactorGraph NonlinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create nonlinear FG from Sequence.
Definition FitBasis.h:61
│ │ │ │ +
Parameters parameters() const
Return Fourier coefficients.
Definition FitBasis.h:96
│ │ │ │ +
FitBasis(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Construct a new FitBasis object.
Definition FitBasis.h:89
│ │ │ │ +
static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create linear FG from Sequence.
Definition FitBasis.h:73
│ │ │ │ +
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
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
│ │ │ │ +
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
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,554 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Basis.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 │ │ │ │ │ -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> │ │ │ │ │ +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#include │ │ │ │ │ +25#pragma once │ │ │ │ │ 26 │ │ │ │ │ -68namespace _g_t_s_a_m { │ │ │ │ │ -69 │ │ │ │ │ -70using Weights = Eigen::Matrix; /* 1xN vector */ │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_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 │ │ │ │ │ - * _B_a_s_i_s_._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/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/a00179.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteDistribution.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
DiscreteDistribution.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Signature.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...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ 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);.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
December 2021
│ │ │ │ +

signatures for conditional densities

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 27, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteDistribution.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 │ │ │ │ │ +_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);. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - December 2021 │ │ │ │ │ +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_D_i_s_t_r_i_b_u_t_i_o_n_._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/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/DiscreteBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteFactorGraph.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
DiscreteBayesNet.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

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

│ │ │ │ +Classes

class  gtsam::DiscreteBayesNet
 A Bayes net made from discrete conditional distributions. More...
 
struct  gtsam::traits< DiscreteBayesNet >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ 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
│ │ │ │ +
Date
Feb 15, 2011
│ │ │ │
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Frank dellaert │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -DiscreteFactorGraph.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +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_B_a_y_e_s_N_e_t │ │ │ │ │ +  A Bayes net made from discrete conditional distributions. _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_ _> │ │ │ │ │ +  │ │ │ │ │ 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 │ │ │ │ │ + Feb 15, 2011 │ │ │ │ │ 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_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_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/a00182.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00182 = [ │ │ │ │ │ - ["EliminateDiscrete", "a00182.html#ga6996e9c84467350b071645649231223a", null], │ │ │ │ │ - ["EliminateForMPE", "a00182.html#ga9bd0b2859a7513198a369f5de3c8cde3", null] │ │ │ │ │ + ["gtsam::traits< DiscreteBayesNet >", "a02820.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/DiscreteKey.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DiscreteDistribution.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::DiscreteDistribution
 A prior probability on a set of discrete variables. More...
struct  gtsam::DiscreteKeys
 DiscreteKeys is a set of keys that can be assembled using the & operator. More...
 
struct  gtsam::traits< DiscreteDistribution >
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
December 2021
│ │ │ │ +

specialized key for discrete variables

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 28, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -DiscreteDistribution.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_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_:_:_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_D_i_s_t_r_i_b_u_t_i_o_n_ _> │ │ │ │ │ +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 │ │ │ │ │ - December 2021 │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + 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_D_i_s_t_r_i_b_u_t_i_o_n_._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/a00185.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00185 = [ │ │ │ │ │ - ["gtsam::traits< DiscreteDistribution >", "a02844.html", null] │ │ │ │ │ + ["gtsam::DiscreteKeys", "a02876.html", "a02876"], │ │ │ │ │ + ["gtsam::traits< DiscreteKeys >", "a02880.html", null], │ │ │ │ │ + ["DiscreteKey", "a00185.html#gad529b58f00471d8f8b25a89b48b7afc2", null], │ │ │ │ │ + ["operator&", "a00185.html#a0be4f243161c2fd937df60aee511eb16", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00185_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteDistribution.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteKey.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,105 +98,121 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteDistribution.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
│ │ │ │
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
│ │ │ │ - │ │ │ │ -
21
│ │ │ │ -
22#include <string>
│ │ │ │ -
23#include <vector>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
33class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
│ │ │ │ -
34 public:
│ │ │ │ - │ │ │ │ -
36
│ │ │ │ -
39
│ │ │ │ - │ │ │ │ -
42
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
45 : Base(f.size(), f) {}
│ │ │ │ -
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ -
52 explicit DiscreteDistribution(const Signature& s) : Base(s) {}
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
60 DiscreteDistribution(const DiscreteKey& key, const std::vector<double>& spec)
│ │ │ │ -
61 : DiscreteDistribution(Signature(key, {}, Signature::Table{spec})) {}
│ │ │ │ +
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
│ │ │ │ -
│ │ │ │ -
69 DiscreteDistribution(const DiscreteKey& key, const std::string& spec)
│ │ │ │ -
70 : DiscreteDistribution(Signature(key, {}, spec)) {}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
75
│ │ │ │ -
77 void print(
│ │ │ │ -
78 const std::string& s = "Discrete Prior: ",
│ │ │ │ -
79 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
80
│ │ │ │ -
84
│ │ │ │ -
86 double operator()(size_t value) const;
│ │ │ │ -
87
│ │ │ │ -
89 // TODO(dellaert): does not play well with wrapper!
│ │ │ │ -
90 // using Base::operator();
│ │ │ │ -
91
│ │ │ │ -
93 std::vector<double> pmf() const;
│ │ │ │ -
94
│ │ │ │ -
96#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
99 size_t GTSAM_DEPRECATED solve() const { return Base::solve({}); }
│ │ │ │ -
101#endif
│ │ │ │ -
102};
│ │ │ │ -
│ │ │ │ -
103// DiscreteDistribution
│ │ │ │ -
104
│ │ │ │ -
105// traits
│ │ │ │ -
106template <>
│ │ │ │ -
107struct traits<DiscreteDistribution> : public Testable<DiscreteDistribution> {};
│ │ │ │ -
108
│ │ │ │ -
109} // namespace gtsam
│ │ │ │ - │ │ │ │ +
64 KeyVector indices() const;
│ │ │ │ +
65
│ │ │ │ +
67 std::map<Key,size_t> cardinalities() const;
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
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
│ │ │ │ + │ │ │ │ +
Included from all GTSAM files.
│ │ │ │
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2)
Create a list from two keys.
Definition DiscreteKey.cpp:46
│ │ │ │ +
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
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
│ │ │ │
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
A prior probability on a set of discrete variables.
Definition DiscreteDistribution.h:33
│ │ │ │ -
DiscreteDistribution(const Signature &s)
Construct from a Signature.
Definition DiscreteDistribution.h:52
│ │ │ │ -
DiscreteDistribution(const DiscreteKey &key, const std::vector< double > &spec)
Construct from key and a vector of floats specifying the probability mass function (PMF).
Definition DiscreteDistribution.h:60
│ │ │ │ -
DiscreteDistribution(const DiscreteKey &key, const std::string &spec)
Construct from key and a string specifying the probability mass function (PMF).
Definition DiscreteDistribution.h:69
│ │ │ │ -
DiscreteDistribution()
Default constructor needed for serialization.
Definition DiscreteDistribution.h:41
│ │ │ │ -
DiscreteDistribution(const DecisionTreeFactor &f)
Constructor from factor.
Definition DiscreteDistribution.h:44
│ │ │ │ -
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │ +
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
DiscreteKeys & operator&(const DiscreteKey &key)
Add a key (non-const!)
Definition DiscreteKey.h:70
│ │ │ │ +
DiscreteKeys()
Constructor for serialization.
Definition DiscreteKey.h:45
│ │ │ │ +
DiscreteKeys(const DiscreteKey &key)
Construct from a key.
Definition DiscreteKey.h:48
│ │ │ │ +
DiscreteKeys(const std::vector< DiscreteKey > &keys)
Construct from a vector of keys.
Definition DiscreteKey.h:56
│ │ │ │ +
DiscreteKeys(std::map< Key, size_t > cardinalities)
Construct from cardinalities.
Definition DiscreteKey.h:51
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,125 +1,148 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteDistribution.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 │ │ │ │ │ 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 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_3class GTSAM_EXPORT _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n : public _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -34 public: │ │ │ │ │ -35 using _B_a_s_e = _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -36 │ │ │ │ │ -39 │ │ │ │ │ -_4_1 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n() {} │ │ │ │ │ -42 │ │ │ │ │ -_4_4 explicit _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) │ │ │ │ │ -45 : _B_a_s_e(f.size(), f) {} │ │ │ │ │ +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> │ │ │ │ │ +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 │ │ │ │ │ -_5_2 explicit _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _S_i_g_n_a_t_u_r_e& s) : _B_a_s_e(s) {} │ │ │ │ │ -53 │ │ │ │ │ -_6_0 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::vector& │ │ │ │ │ -spec) │ │ │ │ │ -61 : _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(_S_i_g_n_a_t_u_r_e(key, {}, Signature::Table{spec})) {} │ │ │ │ │ +_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 │ │ │ │ │ -_6_9 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) │ │ │ │ │ -70 : _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(_S_i_g_n_a_t_u_r_e(key, {}, spec)) {} │ │ │ │ │ -71 │ │ │ │ │ -75 │ │ │ │ │ -77 void print( │ │ │ │ │ -78 const std::string& s = "Discrete Prior: ", │ │ │ │ │ -79 const KeyFormatter& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -80 │ │ │ │ │ -84 │ │ │ │ │ -86 double operator()(size_t value) const; │ │ │ │ │ -87 │ │ │ │ │ -89 // TODO(dellaert): does not play well with wrapper! │ │ │ │ │ -90 // using Base::operator(); │ │ │ │ │ -91 │ │ │ │ │ -93 std::vector pmf() const; │ │ │ │ │ -94 │ │ │ │ │ -96#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -99 size_t GTSAM_DEPRECATED solve() const { return Base::solve({}); } │ │ │ │ │ -101#endif │ │ │ │ │ -102}; │ │ │ │ │ -103// DiscreteDistribution │ │ │ │ │ -104 │ │ │ │ │ -105// traits │ │ │ │ │ -106template <> │ │ │ │ │ -_1_0_7struct _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> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{}; │ │ │ │ │ -108 │ │ │ │ │ -109} // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._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_:_:_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_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_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -A prior probability on a set of discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:33 │ │ │ │ │ -_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -DiscreteDistribution(const Signature &s) │ │ │ │ │ -Construct from a Signature. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:52 │ │ │ │ │ -_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -DiscreteDistribution(const DiscreteKey &key, const std::vector< double > &spec) │ │ │ │ │ -Construct from key and a vector of floats specifying the probability mass │ │ │ │ │ -function (PMF). │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:60 │ │ │ │ │ -_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -DiscreteDistribution(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ -Construct from key and a string specifying the probability mass function (PMF). │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:69 │ │ │ │ │ -_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -DiscreteDistribution() │ │ │ │ │ -Default constructor needed for serialization. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:41 │ │ │ │ │ -_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -DiscreteDistribution(const DecisionTreeFactor &f) │ │ │ │ │ -Constructor from factor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteDistribution.h:44 │ │ │ │ │ -_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_:_:_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_D_i_s_t_r_i_b_u_t_i_o_n_._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/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/DiscreteFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
DiscreteBayesNet.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
DiscreteFactorGraph.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ 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 15, 2011
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ 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 │ │ │ │ │ -DiscreteBayesNet.cpp File Reference │ │ │ │ │ +_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_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 15, 2011 │ │ │ │ │ + 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_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ + * _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/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/DiscreteDistribution.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,58 +95,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteValues.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
DiscreteDistribution.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

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

│ │ │ │ Classes

class  gtsam::DiscreteValues
 A map from keys to values. More...
class  gtsam::DiscreteDistribution
 A prior probability on a set of discrete variables. More...
 
struct  gtsam::traits< DiscreteValues >
struct  gtsam::traits< DiscreteDistribution >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

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

│ │ │ │ -Functions

│ │ │ │ -std::vector< DiscreteValuesgtsam::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
│ │ │ │ +
Date
December 2021
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +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_V_a_l_u_e_s │ │ │ │ │ -  A map from keys to values. _M_o_r_e_._._. │ │ │ │ │ + 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_V_a_l_u_e_s_ _> │ │ │ │ │ +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< _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 │ │ │ │ │ + December 2021 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00194.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a00194 = [ │ │ │ │ │ - ["gtsam::traits< DiscreteValues >", "a02904.html", null], │ │ │ │ │ - ["cartesianProduct", "a00194.html#a55a86a4657d82f6ff048a10d058c529f", null], │ │ │ │ │ - ["html", "a00194.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ - ["markdown", "a00194.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ + ["gtsam::traits< DiscreteDistribution >", "a02844.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/DiscreteDistribution.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteValues.h
│ │ │ │ +
DiscreteDistribution.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,120 +114,89 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ - │ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
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;
│ │ │ │ + │ │ │ │ +
21
│ │ │ │ +
22#include <string>
│ │ │ │ +
23#include <vector>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
33class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional {
│ │ │ │ +
34 public:
│ │ │ │ + │ │ │ │ +
36
│ │ │ │ +
39
│ │ │ │ + │ │ │ │ +
42
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
45 : Base(f.size(), f) {}
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
52 explicit DiscreteDistribution(const Signature& s) : Base(s) {}
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
60 DiscreteDistribution(const DiscreteKey& key, const std::vector<double>& spec)
│ │ │ │ +
61 : DiscreteDistribution(Signature(key, {}, Signature::Table{spec})) {}
│ │ │ │ +
│ │ │ │
62
│ │ │ │ -
66
│ │ │ │ -
67 // insert in base class;
│ │ │ │ -
68 std::pair<iterator, bool> insert( const value_type& value ){
│ │ │ │ -
69 return Base::insert(value);
│ │ │ │ -
70 }
│ │ │ │ +
│ │ │ │ +
69 DiscreteDistribution(const DiscreteKey& key, const std::string& spec)
│ │ │ │ +
70 : DiscreteDistribution(Signature(key, {}, spec)) {}
│ │ │ │ +
│ │ │ │
71
│ │ │ │ -
74 DiscreteValues& insert(const DiscreteValues& values);
│ │ │ │
75
│ │ │ │ -
79 DiscreteValues& update(const DiscreteValues& values);
│ │ │ │ +
77 void print(
│ │ │ │ +
78 const std::string& s = "Discrete Prior: ",
│ │ │ │ +
79 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │
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) {
│ │ │ │ - │ │ │ │ -
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
│ │ │ │ -
specialized key for discrete variables
│ │ │ │ -
An assignment from labels to a discrete value index (size_t)
│ │ │ │ - │ │ │ │ +
84
│ │ │ │ +
86 double operator()(size_t value) const;
│ │ │ │ +
87
│ │ │ │ +
89 // TODO(dellaert): does not play well with wrapper!
│ │ │ │ +
90 // using Base::operator();
│ │ │ │ +
91
│ │ │ │ +
93 std::vector<double> pmf() const;
│ │ │ │ +
94
│ │ │ │ +
96#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
99 size_t GTSAM_DEPRECATED solve() const { return Base::solve({}); }
│ │ │ │ +
101#endif
│ │ │ │ +
102};
│ │ │ │ +
│ │ │ │ +
103// DiscreteDistribution
│ │ │ │ +
104
│ │ │ │ +
105// traits
│ │ │ │ +
106template <>
│ │ │ │ +
107struct traits<DiscreteDistribution> : public Testable<DiscreteDistribution> {};
│ │ │ │ +
108
│ │ │ │ +
109} // namespace gtsam
│ │ │ │ + │ │ │ │ +
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ -
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ -
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys)
Free version of CartesianProduct.
Definition DiscreteValues.h:124
│ │ │ │ -
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
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
│ │ │ │
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ -
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
std::map< Key, std::vector< std::string > > Names
Translation table from values to strings.
Definition DiscreteValues.h:95
│ │ │ │ -
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
Definition DiscreteValues.h:85
│ │ │ │ -
The Factor::error simply extracts the.
│ │ │ │ +
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
A prior probability on a set of discrete variables.
Definition DiscreteDistribution.h:33
│ │ │ │ +
DiscreteDistribution(const Signature &s)
Construct from a Signature.
Definition DiscreteDistribution.h:52
│ │ │ │ +
DiscreteDistribution(const DiscreteKey &key, const std::vector< double > &spec)
Construct from key and a vector of floats specifying the probability mass function (PMF).
Definition DiscreteDistribution.h:60
│ │ │ │ +
DiscreteDistribution(const DiscreteKey &key, const std::string &spec)
Construct from key and a string specifying the probability mass function (PMF).
Definition DiscreteDistribution.h:69
│ │ │ │ +
DiscreteDistribution()
Default constructor needed for serialization.
Definition DiscreteDistribution.h:41
│ │ │ │ +
DiscreteDistribution(const DecisionTreeFactor &f)
Constructor from factor.
Definition DiscreteDistribution.h:44
│ │ │ │ +
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteValues.h │ │ │ │ │ +DiscreteDistribution.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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,153 +16,110 @@ │ │ │ │ │ 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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_3class GTSAM_EXPORT _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n : public _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +34 public: │ │ │ │ │ +35 using _B_a_s_e = _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +36 │ │ │ │ │ +39 │ │ │ │ │ +_4_1 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n() {} │ │ │ │ │ +42 │ │ │ │ │ +_4_4 explicit _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) │ │ │ │ │ +45 : _B_a_s_e(f.size(), f) {} │ │ │ │ │ +46 │ │ │ │ │ +_5_2 explicit _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _S_i_g_n_a_t_u_r_e& s) : _B_a_s_e(s) {} │ │ │ │ │ +53 │ │ │ │ │ +_6_0 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::vector& │ │ │ │ │ +spec) │ │ │ │ │ +61 : _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(_S_i_g_n_a_t_u_r_e(key, {}, Signature::Table{spec})) {} │ │ │ │ │ 62 │ │ │ │ │ -66 │ │ │ │ │ -67 // insert in base class; │ │ │ │ │ -68 std::pair insert( const value_type& value ){ │ │ │ │ │ -69 return Base::insert(value); │ │ │ │ │ -70 } │ │ │ │ │ +_6_9 _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) │ │ │ │ │ +70 : _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n(_S_i_g_n_a_t_u_r_e(key, {}, spec)) {} │ │ │ │ │ 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 │ │ │ │ │ -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); │ │ │ │ │ +77 void print( │ │ │ │ │ +78 const std::string& s = "Discrete Prior: ", │ │ │ │ │ +79 const KeyFormatter& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ 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 │ │ │ │ │ +84 │ │ │ │ │ +86 double operator()(size_t value) const; │ │ │ │ │ +87 │ │ │ │ │ +89 // TODO(dellaert): does not play well with wrapper! │ │ │ │ │ +90 // using Base::operator(); │ │ │ │ │ +91 │ │ │ │ │ +93 std::vector pmf() const; │ │ │ │ │ +94 │ │ │ │ │ +96#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +99 size_t GTSAM_DEPRECATED solve() const { return Base::solve({}); } │ │ │ │ │ +101#endif │ │ │ │ │ +102}; │ │ │ │ │ +103// DiscreteDistribution │ │ │ │ │ +104 │ │ │ │ │ +105// traits │ │ │ │ │ +106template <> │ │ │ │ │ +_1_0_7struct _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> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{}; │ │ │ │ │ +108 │ │ │ │ │ +109} // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._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_:_:_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_:_:_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_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +A prior probability on a set of discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:33 │ │ │ │ │ +_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +DiscreteDistribution(const Signature &s) │ │ │ │ │ +Construct from a Signature. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:52 │ │ │ │ │ +_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +DiscreteDistribution(const DiscreteKey &key, const std::vector< double > &spec) │ │ │ │ │ +Construct from key and a vector of floats specifying the probability mass │ │ │ │ │ +function (PMF). │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:60 │ │ │ │ │ +_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +DiscreteDistribution(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ +Construct from key and a string specifying the probability mass function (PMF). │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:69 │ │ │ │ │ +_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +DiscreteDistribution() │ │ │ │ │ +Default constructor needed for serialization. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:41 │ │ │ │ │ +_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_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +DiscreteDistribution(const DecisionTreeFactor &f) │ │ │ │ │ +Constructor from factor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteDistribution.h:44 │ │ │ │ │ +_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 │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ + * _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/a00197.html │ │ │ │ @@ -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/discrete/DiscreteJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,37 +95,33 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
DecisionTreeFactor.cpp File Reference
│ │ │ │ +
DiscreteJunctionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

discrete factor │ │ │ │ -More...

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

│ │ │ │ Namespaces

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

Detailed Description

│ │ │ │ -

discrete factor

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,21 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +DiscreteJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -discrete factor │ │ │ │ │ Date │ │ │ │ │ - Feb 14, 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_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._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/a00200_source.html │ │ │ │ @@ -25,12888 +25,3140 @@ │ │ │ │ 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.
..
.
│ │ │ │ -00000f80: 0a0a 3c64 6976 2063 6c61 7373 3d22 6865 ..
.
DecisionTre │ │ │ │ -00000fd0: 652d 696e 6c2e 683c 2f64 6976 3e3c 2f64 e-inl.h
.
.
.< │ │ │ │ -00001010: 6469 7620 636c 6173 733d 2266 7261 676d div class="fragm │ │ │ │ -00001020: 656e 7422 3e3c 6469 7620 636c 6173 733d ent">
│ │ │ │ -00001070: 2031 3c2f 7370 616e 3e3c 7370 616e 2063 1/ │ │ │ │ -00001090: 2a20 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d * -------------- │ │ │ │ -000010a0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -000010b0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -000010c0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -000010d0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 3c2f --------------
.
< │ │ │ │ -00001120: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001130: 6e6f 223e 2020 2020 323c 2f73 7061 6e3e no"> 2 │ │ │ │ -00001140: 3c73 7061 6e20 636c 6173 733d 2263 6f6d .
│ │ │ │ -000011b0: 2033 3c2f 7370 616e 3e3c 7370 616e 2063 3 │ │ │ │ -000011d0: 2a20 4754 5341 4d20 436f 7079 7269 6768 * GTSAM Copyrigh │ │ │ │ -000011e0: 7420 3230 3130 2c20 4765 6f72 6769 6120 t 2010, Georgia │ │ │ │ -000011f0: 5465 6368 2052 6573 6561 7263 6820 436f Tech Research Co │ │ │ │ -00001200: 7270 6f72 6174 696f 6e2c 3c2f 7370 616e rporation,
.
│ │ │ │ -00001260: 2020 2020 343c 2f73 7061 6e3e 3c73 7061 4 * Atlanta, Ge │ │ │ │ -00001290: 6f72 6769 6120 3330 3333 322d 3034 3135 orgia 30332-0415 │ │ │ │ -000012a0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
. │ │ │ │ -000012c0: 3c61 2069 643d 226c 3030 3030 3522 206e 5 * All R │ │ │ │ -00001320: 6967 6874 7320 5265 7365 7276 6564 3c2f ights Reserved
.
< │ │ │ │ -00001370: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001380: 6e6f 223e 2020 2020 363c 2f73 7061 6e3e no"> 6 │ │ │ │ -00001390: 3c73 7061 6e20 636c 6173 733d 2263 6f6d * Authors │ │ │ │ -000013b0: 3a20 4672 616e 6b20 4465 6c6c 6165 7274 : Frank Dellaert │ │ │ │ -000013c0: 2c20 6574 2061 6c2e 2028 7365 6520 5448 , et al. (see TH │ │ │ │ -000013d0: 414e 4b53 2066 6f72 2074 6865 2066 756c ANKS for the ful │ │ │ │ -000013e0: 6c20 6175 7468 6f72 206c 6973 7429 3c2f l author list)
.
< │ │ │ │ -00001430: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001440: 6e6f 223e 2020 2020 373c 2f73 7061 6e3e no"> 7 │ │ │ │ -00001450: 3c73 7061 6e20 636c 6173 733d 2263 6f6d .
│ │ │ │ -000014c0: 2038 3c2f 7370 616e 3e3c 7370 616e 2063 8 │ │ │ │ -000014e0: 2a20 5365 6520 4c49 4345 4e53 4520 666f * See LICENSE fo │ │ │ │ -000014f0: 7220 7468 6520 6c69 6365 6e73 6520 696e r the license in │ │ │ │ -00001500: 666f 726d 6174 696f 6e3c 2f73 7061 6e3e formation │ │ │ │ -00001510: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
.
│ │ │ │ -00001560: 2020 2039 3c2f 7370 616e 3e3c 7370 616e 9
. │ │ │ │ -00001590: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
│ │ │ │ -000016a0: 2020 2031 313c 2f73 7061 6e3e 203c 2f64 11 .
2 │ │ │ │ -00001700: 303c 2f73 7061 6e3e 3c73 7061 6e20 636c 0#pragma once< │ │ │ │ -00001730: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
.< │ │ │ │ -00001750: 6120 6964 3d22 6c30 3030 3231 2220 6e61 a id="l00021" na │ │ │ │ -00001760: 6d65 3d22 6c30 3030 3231 223e 3c2f 613e me="l00021"> │ │ │ │ -00001770: 3c73 7061 6e20 636c 6173 733d 226c 696e 21
..
23 │ │ │ │ -000018c0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
.
│ │ │ │ -00001910: 2020 3234 3c2f 7370 616e 3e3c 7370 616e 24#include & │ │ │ │ -00001940: 6c74 3b61 6c67 6f72 6974 686d 2667 743b lt;algorithm> │ │ │ │ -00001950: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
. │ │ │ │ -00001970: 3c61 2069 643d 226c 3030 3032 3522 206e 25#in │ │ │ │ -000019d0: 636c 7564 6520 266c 743b 626f 6f73 742f clude <boost/ │ │ │ │ -000019e0: 666f 726d 6174 2e68 7070 2667 743b 3c2f format.hpp>
.
< │ │ │ │ -00001a30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001a40: 6e6f 223e 2020 2032 363c 2f73 7061 6e3e no"> 26 │ │ │ │ -00001a50: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ -00001a70: 7564 6520 266c 743b 626f 6f73 742f 6d61 ude <boost/ma │ │ │ │ -00001a80: 6b65 5f73 6861 7265 642e 6870 7026 6774 ke_shared.hpp> │ │ │ │ -00001a90: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
.< │ │ │ │ -00001aa0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00001ab0: 3e3c 6120 6964 3d22 6c30 3030 3237 2220 > 27#i │ │ │ │ -00001b10: 6e63 6c75 6465 2026 6c74 3b62 6f6f 7374 nclude <boost │ │ │ │ -00001b20: 2f6f 7074 696f 6e61 6c2e 6870 7026 6774 /optional.hpp> │ │ │ │ -00001b30: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
.< │ │ │ │ -00001b40: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00001b50: 3e3c 6120 6964 3d22 6c30 3030 3238 2220 > 28
.
29< │ │ │ │ -00001bf0: 7370 616e 2063 6c61 7373 3d22 7072 6570 span class="prep │ │ │ │ -00001c00: 726f 6365 7373 6f72 223e 2369 6e63 6c75 rocessor">#inclu │ │ │ │ -00001c10: 6465 2026 6c74 3b63 6d61 7468 2667 743b de <cmath> │ │ │ │ -00001c20: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
. │ │ │ │ -00001c40: 3c61 2069 643d 226c 3030 3033 3022 206e 30#in │ │ │ │ -00001ca0: 636c 7564 6520 266c 743b 6673 7472 6561 clude <fstrea │ │ │ │ -00001cb0: 6d26 6774 3b3c 2f73 7061 6e3e 3c2f 6469 m>.
31 │ │ │ │ -00001d10: 3c2f 7370 616e 3e3c 7370 616e 2063 6c61 #include <l │ │ │ │ -00001d40: 6973 7426 6774 3b3c 2f73 7061 6e3e 3c2f ist>.
│ │ │ │ -00001da0: 3332 3c2f 7370 616e 3e3c 7370 616e 2063 32#include < │ │ │ │ -00001dd0: 3b6d 6170 2667 743b 3c2f 7370 616e 3e3c ;map>< │ │ │ │ -00001de0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
│ │ │ │ -00001e30: 2033 333c 2f73 7061 6e3e 3c73 7061 6e20 33#include &l │ │ │ │ -00001e60: 743b 7365 7426 6774 3b3c 2f73 7061 6e3e t;set> │ │ │ │ -00001e70: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
.
│ │ │ │ -00001ec0: 2020 3334 3c2f 7370 616e 3e3c 7370 616e 34#include & │ │ │ │ -00001ef0: 6c74 3b73 7374 7265 616d 2667 743b 3c2f lt;sstream>
.
< │ │ │ │ -00001f40: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001f50: 6e6f 223e 2020 2033 353c 2f73 7061 6e3e no"> 35 │ │ │ │ -00001f60: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ -00001f80: 7564 6520 266c 743b 7374 7269 6e67 2667 ude <string&g │ │ │ │ -00001f90: 743b 3c2f 7370 616e 3e3c 2f64 6976 3e0a t;
. │ │ │ │ -00001fa0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
< │ │ │ │ -00001fd0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 36# │ │ │ │ -00002010: 696e 636c 7564 6520 266c 743b 7665 6374 include <vect │ │ │ │ -00002020: 6f72 2667 743b 3c2f 7370 616e 3e3c 2f64 or>.
3 │ │ │ │ -00002080: 373c 2f73 7061 6e3e 203c 2f64 6976 3e0a 7
. │ │ │ │ -00002090: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
.< │ │ │ │ -00002160: 6120 6964 3d22 6c30 3030 3339 2220 6e61 a id="l00039" na │ │ │ │ -00002170: 6d65 3d22 6c30 3030 3339 223e 3c2f 613e me="l00039"> │ │ │ │ -00002180: 3c73 7061 6e20 636c 6173 733d 226c 696e 39
.
40 /********** │ │ │ │ -00002220: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002230: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002240: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002260: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
│ │ │ │ -00002270: 0a3c 6469 7620 636c 6173 733d 226c 696e .
│ │ │ │ -000022a0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 41 // │ │ │ │ -000022e0: 4e6f 6465 3c2f 7370 616e 3e3c 2f64 6976 Node
.
42< │ │ │ │ -00002340: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span> /* │ │ │ │ -00002360: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002370: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002380: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002390: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000023a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f 7370 ***********/
.
43#ifdef │ │ │ │ -00002430: 4454 5f44 4542 5547 5f4d 454d 4f52 593c DT_DEBUG_MEMORY< │ │ │ │ -00002440: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
.< │ │ │ │ -00002460: 6120 6964 3d22 6c30 3030 3434 2220 6e61 a id="l00044" na │ │ │ │ -00002470: 6d65 3d22 6c30 3030 3434 223e 3c2f 613e me="l00044"> │ │ │ │ -00002480: 3c73 7061 6e20 636c 6173 733d 226c 696e 44 templat │ │ │ │ -000024c0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ -00002510: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ -00002520: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
.
│ │ │ │ -00002570: 2020 3435 3c2f 7370 616e 3e20 203c 7370 45 int DecisionTree&l │ │ │ │ -000025b0: 743b 4c2c 2059 2667 743b 3a3a 4e6f 6465 t;L, Y>::Node │ │ │ │ -000025c0: 3a3a 6e72 4e6f 6465 7320 3d20 303b 3c2f ::nrNodes = 0;.
│ │ │ │ -00002620: 3436 3c2f 7370 616e 3e3c 7370 616e 2063 46#endif
.
│ │ │ │ -000026a0: 2020 2034 373c 2f73 7061 6e3e 203c 2f64 47 .
4 │ │ │ │ -00002700: 383c 2f73 7061 6e3e 3c73 7061 6e20 636c 8 │ │ │ │ -00002720: 2f2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /*************** │ │ │ │ -00002730: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002740: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002750: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002760: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f *************/
.
< │ │ │ │ -000027b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000027c0: 6e6f 223e 2020 2034 393c 2f73 7061 6e3e no"> 49 │ │ │ │ -000027d0: 2020 3c73 7061 6e20 636c 6173 733d 2263 // Leaf< │ │ │ │ -000027f0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
.< │ │ │ │ -00002810: 6120 6964 3d22 6c30 3030 3530 2220 6e61 a id="l00050" na │ │ │ │ -00002820: 6d65 3d22 6c30 3030 3530 223e 3c2f 613e me="l00050"> │ │ │ │ -00002830: 3c73 7061 6e20 636c 6173 733d 226c 696e 50 /****** │ │ │ │ -00002870: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002880: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00002890: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000028a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000028b0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
│ │ │ │ -00002910: 3531 3c2f 7370 616e 3e20 203c 7370 616e 51 template │ │ │ │ -00002940: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ -00002960: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
. │ │ │ │ -000029a0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
.
< │ │ │ │ -00002a30: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00002a40: 7265 663d 2261 3032 3737 362e 6874 6d6c ref="a02776.html │ │ │ │ -00002a50: 223e 2020 2035 323c 2f61 3e3c 2f73 7061 "> 52 struct │ │ │ │ -00002a80: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 DecisionTree │ │ │ │ -00002ac0: 3c2f 613e 266c 743b 4c2c 2059 2667 743b <L, Y> │ │ │ │ -00002ad0: 3a3a 3c61 2063 6c61 7373 3d22 636f 6465 ::L │ │ │ │ -00002b00: 6561 663c 2f61 3e20 3a20 3c73 7061 6e20 eaf : │ │ │ │ -00002b20: 7075 626c 6963 3c2f 7370 616e 3e20 3c61 public Decisi │ │ │ │ -00002b60: 6f6e 5472 6565 3c2f 613e 266c 743b 4c2c onTree<L, │ │ │ │ -00002b70: 2059 2667 743b 3a3a 4e6f 6465 207b 3c2f Y>::Node {.. │ │ │ │ -00002ca0: 3c61 2069 643d 226c 3030 3035 3522 206e 55
.
59 │ │ │ │ -00002d80: 3c2f 7370 616e 3e20 2020 203c 7370 616e size_t nrAssignment │ │ │ │ -00002e10: 735f 3c2f 613e 3b3c 2f64 6976 3e0a 3c64 s_;
. │ │ │ │ -00002e30: 3c61 2069 643d 226c 3030 3036 3022 206e 60
.
62 │ │ │ │ -00002f10: 3c2f 7370 616e 3e20 2020 203c 6120 636c Leaf │ │ │ │ -00002f70: 3c2f 613e 2829 207b 7d3c 2f64 6976 3e0a () {}
. │ │ │ │ -00002f80: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
.
.
│ │ │ │ -000030b0: 2020 3635 3c2f 613e 3c2f 7370 616e 3e20 65 │ │ │ │ -000030c0: 2020 203c 6120 636c 6173 733d 2263 6f64 Leaf(const │ │ │ │ -00003140: 5926 616d 703b 2063 6f6e 7374 616e 742c Y& constant, │ │ │ │ -00003150: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ -00003170: 743c 2f73 7061 6e3e 206e 7241 7373 6967 t nrAssig │ │ │ │ -00003180: 6e6d 656e 7473 203d 2031 293c 2f64 6976 nments = 1)
.
66< │ │ │ │ -000031e0: 2f73 7061 6e3e 2020 2020 2020 2020 3a20 /span> : │ │ │ │ -000031f0: 636f 6e73 7461 6e74 5f28 636f 6e73 7461 constant_(consta │ │ │ │ -00003200: 6e74 292c 206e 7241 7373 6967 6e6d 656e nt), nrAssignmen │ │ │ │ -00003210: 7473 5f28 6e72 4173 7369 676e 6d65 6e74 ts_(nrAssignment │ │ │ │ -00003220: 7329 207b 7d3c 2f64 6976 3e0a 3c2f 6469 s) {}
..
67 │ │ │ │ -00003280: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
.< │ │ │ │ -00003290: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ -000032a0: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ -000032b0: 6e30 3030 3639 2220 6461 7461 2d73 7461 n00069" data-sta │ │ │ │ -000032c0: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ -000032d0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
69 cons │ │ │ │ -00003390: 743c 2f73 7061 6e3e 2059 2661 6d70 3b20 t Y& │ │ │ │ -000033a0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 constant()< │ │ │ │ -00003400: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00003410: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
.
70 │ │ │ │ -00003480: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -000034a0: 6574 7572 6e3c 2f73 7061 6e3e 2063 6f6e eturn con │ │ │ │ -000034b0: 7374 616e 745f 3b3c 2f64 6976 3e0a 3c64 stant_;
. │ │ │ │ -000034d0: 3c61 2069 643d 226c 3030 3037 3122 206e 71 }
..
│ │ │ │ -00003570: 3732 3c2f 7370 616e 3e20 3c2f 6469 763e 72
│ │ │ │ -00003580: 0a3c 6469 7620 636c 6173 733d 226c 696e .
│ │ │ │ -000035b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ -00003610: 2037 343c 2f61 3e3c 2f73 7061 6e3e 2020 74 │ │ │ │ -00003620: 2020 3c73 7061 6e20 636c 6173 733d 226b size │ │ │ │ -00003640: 5f74 3c2f 7370 616e 3e20 3c61 2063 6c61 _t nrAss │ │ │ │ -000036a0: 6967 6e6d 656e 7473 3c2f 613e 2829 3c73 ignments() const { re │ │ │ │ -000036f0: 7475 726e 3c2f 7370 616e 3e20 6e72 4173 turn nrAs │ │ │ │ -00003700: 7369 676e 6d65 6e74 735f 3b20 7d3c 2f64 signments_; }.
7 │ │ │ │ -00003760: 353c 2f73 7061 6e3e 203c 2f64 6976 3e0a 5
. │ │ │ │ -00003770: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
. │ │ │ │ -00003ae0: 3c61 2069 643d 226c 3030 3038 3022 206e 80
.
. │ │ │ │ -00003b70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
< │ │ │ │ -00003ba0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> │ │ │ │ -00003c00: 3832 3c2f 613e 3c2f 7370 616e 3e20 2020 82 │ │ │ │ -00003c10: 203c 7370 616e 2063 6c61 7373 3d22 6b65 bool< │ │ │ │ -00003c30: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> sameLeaf │ │ │ │ -00003c90: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ -00003cb0: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Node&am │ │ │ │ -00003cf0: 703b 2071 293c 7370 616e 2063 6c61 7373 p; q) cons │ │ │ │ -00003d10: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
.
83 │ │ │ │ -00003d80: 2020 2020 3c73 7061 6e20 636c 6173 733d re │ │ │ │ -00003da0: 7475 726e 3c2f 7370 616e 3e20 2871 2e69 turn (q.i │ │ │ │ -00003db0: 734c 6561 6628 2920 2661 6d70 3b26 616d sLeaf() &&am │ │ │ │ -00003dc0: 703b 2071 2e73 616d 654c 6561 6628 2a3c p; q.sameLeaf(*< │ │ │ │ -00003dd0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00003de0: 6f72 6422 3e74 6869 733c 2f73 7061 6e3e ord">this │ │ │ │ -00003df0: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
.
84 │ │ │ │ -00003e50: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
.
│ │ │ │ -00003e60: 0a3c 6469 7620 636c 6173 733d 226c 696e ...
│ │ │ │ -00003f90: 2020 2038 373c 2f61 3e3c 2f73 7061 6e3e 87 │ │ │ │ -00003fa0: 2020 2020 3c73 7061 6e20 636c 6173 733d bo │ │ │ │ -00003fc0: 6f6c 3c2f 7370 616e 3e20 3c61 2063 6c61 ol equal │ │ │ │ -00004020: 733c 2f61 3e28 3c73 7061 6e20 636c 6173 s(cons │ │ │ │ -00004040: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t Node&a │ │ │ │ -00004080: 6d70 3b20 712c 203c 7370 616e 2063 6c61 mp; q, con │ │ │ │ -000040a0: 7374 3c2f 7370 616e 3e20 436f 6d70 6172 st Compar │ │ │ │ -000040b0: 6546 756e 6326 616d 703b 2063 6f6d 7061 eFunc& compa │ │ │ │ -000040c0: 7265 293c 7370 616e 2063 6c61 7373 3d22 re) const │ │ │ │ -000040e0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -000040f0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
.
│ │ │ │ -00004140: 2020 2038 383c 2f73 7061 6e3e 2020 2020 88 │ │ │ │ -00004150: 2020 3c73 7061 6e20 636c 6173 733d 226b const Leaf* othe │ │ │ │ -000041b0: 7220 3d20 3c73 7061 6e20 636c 6173 733d r = dynami │ │ │ │ -000041d0: 635f 6361 7374 266c 743b 3c2f 7370 616e c_cast<const Leaf* │ │ │ │ -00004250: 2667 743b 3c2f 7370 616e 3e28 2661 6d70 >(& │ │ │ │ -00004260: 3b71 293b 3c2f 6469 763e 0a3c 6469 7620 ;q);
.
89 │ │ │ │ -000042c0: 2020 2020 203c 7370 616e 2063 6c61 7373 i │ │ │ │ -000042e0: 663c 2f73 7061 6e3e 2028 216f 7468 6572 f (!other │ │ │ │ -000042f0: 2920 3c73 7061 6e20 636c 6173 733d 226b ) retu │ │ │ │ -00004310: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ -00004330: 6661 6c73 653c 2f73 7061 6e3e 3b3c 2f64 false;.
9 │ │ │ │ -00004390: 303c 2f73 7061 6e3e 2020 2020 2020 3c73 0 return compare(th │ │ │ │ -000043d0: 6973 2d26 6774 3b63 6f6e 7374 616e 745f is->constant_ │ │ │ │ -000043e0: 2c20 6f74 6865 722d 2667 743b 3c61 2063 , other->con │ │ │ │ -00004440: 7374 616e 745f 3c2f 613e 293b 3c2f 6469 stant_);.
91 │ │ │ │ -000044a0: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
.
92 < │ │ │ │ -00004510: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
.
< │ │ │ │ -00004590: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000045a0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 94 void print(const │ │ │ │ -000046a0: 2073 7464 3a3a 7374 7269 6e67 2661 6d70 std::string& │ │ │ │ -000046b0: 3b20 732c 203c 7370 616e 2063 6c61 7373 ; s, const │ │ │ │ -000046d0: 3c2f 7370 616e 3e20 4c61 6265 6c46 6f72 LabelFor │ │ │ │ -000046e0: 6d61 7474 6572 2661 6d70 3b20 6c61 6265 matter& labe │ │ │ │ -000046f0: 6c46 6f72 6d61 7474 6572 2c3c 2f64 6976 lFormatter,
.
95< │ │ │ │ -00004750: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -00004760: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ -00004780: 3c2f 7370 616e 3e20 5661 6c75 6546 6f72 ValueFor │ │ │ │ -00004790: 6d61 7474 6572 2661 6d70 3b20 7661 6c75 matter& valu │ │ │ │ -000047a0: 6546 6f72 6d61 7474 6572 293c 7370 616e eFormatter) const override │ │ │ │ -000047d0: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
. │ │ │ │ -000047e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
.
97 │ │ │ │ -00004920: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
.
.
98< │ │ │ │ -00004980: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
...
│ │ │ │ -00004ba0: 2031 3031 3c2f 7370 616e 3e20 2020 2020 101 │ │ │ │ -00004bb0: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ -00004bd0: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ -00004be0: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ -00004bf0: 616c 7565 466f 726d 6174 7465 722c 3c2f alueFormatter,.
1 │ │ │ │ -00004c50: 3032 3c2f 7370 616e 3e20 2020 2020 2020 02 │ │ │ │ -00004c60: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ -00004c80: 626f 6f6c 3c2f 7370 616e 3e20 7368 6f77 bool show │ │ │ │ -00004c90: 5a65 726f 293c 7370 616e 2063 6c61 7373 Zero) cons │ │ │ │ -00004cb0: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
.
103 │ │ │ │ -00004d20: 2020 2020 7374 643a 3a73 7472 696e 6720 std::string │ │ │ │ -00004d30: 7661 6c75 6520 3d20 7661 6c75 6546 6f72 value = valueFor │ │ │ │ -00004d40: 6d61 7474 6572 2863 6f6e 7374 616e 745f matter(constant_ │ │ │ │ -00004d50: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
.
104 │ │ │ │ -00004db0: 2020 203c 7370 616e 2063 6c61 7373 3d22 if< │ │ │ │ -00004dd0: 2f73 7061 6e3e 2028 7368 6f77 5a65 726f /span> (showZero │ │ │ │ -00004de0: 207c 7c20 7661 6c75 652e 636f 6d70 6172 || value.compar │ │ │ │ -00004df0: 6528 3c73 7061 6e20 636c 6173 733d 2273 e(&q │ │ │ │ -00004e10: 756f 743b 3026 7175 6f74 3b3c 2f73 7061 uot;0"))
.
105 │ │ │ │ -00004e80: 2020 2020 2020 206f 7320 266c 743b 266c os <&l │ │ │ │ -00004e90: 743b 203c 7370 616e 2063 6c61 7373 3d22 t; & │ │ │ │ -00004eb0: 7175 6f74 3b5c 2671 756f 743b 2671 756f quot;\"&quo │ │ │ │ -00004ec0: 743b 3c2f 7370 616e 3e20 266c 743b 266c t; <&l │ │ │ │ -00004ed0: 743b 2074 6869 732d 2667 743b 6964 2829 t; this->id() │ │ │ │ -00004ee0: 2026 6c74 3b26 6c74 3b20 3c73 7061 6e20 << "\&qu │ │ │ │ -00004f10: 6f74 3b20 5b6c 6162 656c 3d5c 2671 756f ot; [label=\&quo │ │ │ │ -00004f20: 743b 2671 756f 743b 3c2f 7370 616e 3e20 t;" │ │ │ │ -00004f30: 266c 743b 266c 743b 2076 616c 7565 3c2f << value.
1 │ │ │ │ -00004f90: 3036 3c2f 7370 616e 3e20 2020 2020 2020 06 │ │ │ │ -00004fa0: 2020 2020 266c 743b 266c 743b 203c 7370 << "\ │ │ │ │ -00004fd0: 2671 756f 743b 2c20 7368 6170 653d 626f ", shape=bo │ │ │ │ -00004fe0: 782c 2072 616e 6b3d 7369 6e6b 2c20 6865 x, rank=sink, he │ │ │ │ -00004ff0: 6967 6874 3d30 2e33 352c 2066 6978 6564 ight=0.35, fixed │ │ │ │ -00005000: 7369 7a65 3d74 7275 655d 5c6e 2671 756f size=true]\n&quo │ │ │ │ -00005010: 743b 3c2f 7370 616e 3e3b 3c2f 6469 763e t;;
│ │ │ │ -00005020: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ -00005080: 0a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 .
.
.
  • ..< │ │ │ │ +00000290: 7363 7269 7074 2074 7970 653d 2274 6578 script type="tex │ │ │ │ +000002a0: 742f 6a61 7661 7363 7269 7074 2220 7372 t/javascript" sr │ │ │ │ +000002b0: 633d 2264 796e 7365 6374 696f 6e73 2e6a c="dynsections.j │ │ │ │ +000002c0: 7322 3e3c 2f73 6372 6970 743e 0a3c 6c69 s">.
  • ... │ │ │ │ +00000380: 3c73 6372 6970 7420 7479 7065 3d22 7465 ... │ │ │ │ +00000440: 3c73 6372 6970 7420 7479 7065 3d22 7465 . │ │ │ │ +00000480: 3c73 6372 6970 7420 7479 7065 3d22 7465 .< │ │ │ │ +00000510: 7363 7269 7074 2074 7970 653d 2274 6578 script type="tex │ │ │ │ +00000520: 742f 6a61 7661 7363 7269 7074 2220 6173 t/javascript" as │ │ │ │ +00000530: 796e 633d 2261 7379 6e63 2220 7372 633d ync="async" src= │ │ │ │ +00000540: 2266 696c 653a 2f2f 2f75 7372 2f73 6861 "file:///usr/sha │ │ │ │ +00000550: 7265 2f6a 6176 6173 6372 6970 742f 6d61 re/javascript/ma │ │ │ │ +00000560: 7468 6a61 782f 4d61 7468 4a61 782e 6a73 thjax/MathJax.js │ │ │ │ +00000570: 3f63 6f6e 6669 673d 5465 582d 4d4d 4c2d ?config=TeX-MML- │ │ │ │ +00000580: 414d 5f43 4854 4d4c 223e 3c2f 7363 7269 AM_CHTML">. │ │ │ │ +000005d0: 0a3c 2f68 6561 643e 0a3c 626f 6479 3e0a ... │ │ │ │ +000005e0: 3c64 6976 2069 643d 2274 6f70 223e 3c21
    .
    ..
  • . < │ │ │ │ +00000670: 7472 2069 643d 2270 726f 6a65 6374 726f tr id="projectro │ │ │ │ +00000680: 7722 3e0a 2020 3c74 6420 6964 3d22 7072 w">. . │ │ │ │ +00000720: 3c2f 7472 3e0a 203c 2f74 626f 6479 3e0a . . │ │ │ │ +00000730: 3c2f 7461 626c 653e 0a3c 2f64 6976 3e0a
    . gtsam 4.2.0. . │ │ │ │ -000006f0: 203c 6469 7620 6964 3d22 7072 6f6a 6563
    gtsam.
    . │ │ │ │ +000006a0: 3c64 6976 2069 643d 2270 726f 6a65 6374
    gtsam 4.2.0< │ │ │ │ +000006e0: 2f73 7061 6e3e 0a20 2020 3c2f 6469 763e /span>.
    │ │ │ │ +000006f0: 0a20 2020 3c64 6976 2069 643d 2270 726f .
    gtsam │ │ │ │ +00000710: 3c2f 6469 763e 0a20 203c 2f74 643e 0a20
    .
    .
    . │ │ │ │ +00000740: 3c21 2d2d 2065 6e64 2068 6561 6465 7220 .../* │ │ │ │ +000007a0: 406c 6963 656e 7365 206d 6167 6e65 743a @license magnet: │ │ │ │ +000007b0: 3f78 743d 7572 6e3a 6274 6968 3a64 3364 ?xt=urn:btih:d3d │ │ │ │ +000007c0: 3961 3961 3635 3935 3532 3166 3936 3636 9a9a6595521f9666 │ │ │ │ +000007d0: 6135 6539 3463 6338 3330 6461 6238 3362 a5e94cc830dab83b │ │ │ │ +000007e0: 3635 3639 3926 616d 703b 646e 3d65 7870 65699&dn=exp │ │ │ │ +000007f0: 6174 2e74 7874 204d 4954 202a 2f0a 7661 at.txt MIT */.va │ │ │ │ +00000800: 7220 7365 6172 6368 426f 7820 3d20 6e65 r searchBox = ne │ │ │ │ +00000810: 7720 5365 6172 6368 426f 7828 2273 6561 w SearchBox("sea │ │ │ │ +00000820: 7263 6842 6f78 222c 2022 7365 6172 6368 rchBox", "search │ │ │ │ +00000830: 2f22 2c27 2e68 746d 6c27 293b 0a2f 2a20 /",'.html');./* │ │ │ │ +00000840: 406c 6963 656e 7365 2d65 6e64 202a 2f0a @license-end */. │ │ │ │ +00000850: 3c2f 7363 7269 7074 3e0a 3c73 6372 6970 .
    .../* @ │ │ │ │ +000008f0: 6c69 6365 6e73 6520 6d61 676e 6574 3a3f license magnet:? │ │ │ │ +00000900: 7874 3d75 726e 3a62 7469 683a 6433 6439 xt=urn:btih:d3d9 │ │ │ │ +00000910: 6139 6136 3539 3535 3231 6639 3636 3661 a9a6595521f9666a │ │ │ │ +00000920: 3565 3934 6363 3833 3064 6162 3833 6236 5e94cc830dab83b6 │ │ │ │ +00000930: 3536 3939 2661 6d70 3b64 6e3d 6578 7061 5699&dn=expa │ │ │ │ +00000940: 742e 7478 7420 4d49 5420 2a2f 0a24 2866 t.txt MIT */.$(f │ │ │ │ +00000950: 756e 6374 696f 6e28 2920 7b0a 2020 696e unction() {. in │ │ │ │ +00000960: 6974 4d65 6e75 2827 272c 7472 7565 2c66 itMenu('',true,f │ │ │ │ +00000970: 616c 7365 2c27 7365 6172 6368 2e70 6870 alse,'search.php │ │ │ │ +00000980: 272c 2753 6561 7263 6827 293b 0a20 2024 ','Search');. $ │ │ │ │ +00000990: 2864 6f63 756d 656e 7429 2e72 6561 6479 (document).ready │ │ │ │ +000009a0: 2866 756e 6374 696f 6e28 2920 7b20 696e (function() { in │ │ │ │ +000009b0: 6974 5f73 6561 7263 6828 293b 207d 293b it_search(); }); │ │ │ │ +000009c0: 0a7d 293b 0a2f 2a20 406c 6963 656e 7365 .});./* @license │ │ │ │ +000009d0: 2d65 6e64 202a 2f0a 3c2f 7363 7269 7074 -end */..../* @ │ │ │ │ +00000a20: 6c69 6365 6e73 6520 6d61 676e 6574 3a3f license magnet:? │ │ │ │ +00000a30: 7874 3d75 726e 3a62 7469 683a 6433 6439 xt=urn:btih:d3d9 │ │ │ │ +00000a40: 6139 6136 3539 3535 3231 6639 3636 3661 a9a6595521f9666a │ │ │ │ +00000a50: 3565 3934 6363 3833 3064 6162 3833 6236 5e94cc830dab83b6 │ │ │ │ +00000a60: 3536 3939 2661 6d70 3b64 6e3d 6578 7061 5699&dn=expa │ │ │ │ +00000a70: 742e 7478 7420 4d49 5420 2a2f 0a24 2864 t.txt MIT */.$(d │ │ │ │ +00000a80: 6f63 756d 656e 7429 2e72 6561 6479 2866 ocument).ready(f │ │ │ │ +00000a90: 756e 6374 696f 6e28 2920 7b20 696e 6974 unction() { init │ │ │ │ +00000aa0: 5f63 6f64 6566 6f6c 6428 3029 3b20 7d29 _codefold(0); }) │ │ │ │ +00000ab0: 3b0a 2f2a 2040 6c69 6365 6e73 652d 656e ;./* @license-en │ │ │ │ +00000ac0: 6420 2a2f 0a3c 2f73 6372 6970 743e 0a3c d */..< │ │ │ │ +00000ad0: 2f64 6976 3e3c 212d 2d20 746f 7020 2d2d /div>.
    .
    ...
    .
    .
    .
    .
    < │ │ │ │ +00000ec0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    Loading │ │ │ │ +00000ef0: 2e2e 2e3c 2f64 6976 3e0a 3c64 6976 2063 ...
    .
    S │ │ │ │ +00000f20: 6561 7263 6869 6e67 2e2e 2e3c 2f64 6976 earching...
    .
    No Matche │ │ │ │ +00000f60: 733c 2f64 6976 3e0a 3c2f 6469 763e 0a3c s
    .
    .< │ │ │ │ +00000f70: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ...
    .
    Discrete │ │ │ │ +00000fd0: 4661 6374 6f72 4772 6170 682e 683c 2f64 FactorGraph.h
    .
    │ │ │ │ +00000ff0: 3c21 2d2d 6865 6164 6572 2d2d 3e0a 3c64 ..Go to │ │ │ │ +00001030: 2074 6865 2064 6f63 756d 656e 7461 7469 the documentati │ │ │ │ +00001040: 6f6e 206f 6620 7468 6973 2066 696c 652e on of this file. │ │ │ │ +00001050: 3c2f 613e 3c64 6976 2063 6c61 7373 3d22
    1/* --------- │ │ │ │ +000010e0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +000010f0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001100: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001110: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001120: 2d2d 2d3c 2f73 7061 6e3e 3c2f 6469 763e ---
    │ │ │ │ +00001130: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    3 * GTSAM Cop │ │ │ │ +00001220: 7972 6967 6874 2032 3031 302c 2047 656f yright 2010, Geo │ │ │ │ +00001230: 7267 6961 2054 6563 6820 5265 7365 6172 rgia Tech Resear │ │ │ │ +00001240: 6368 2043 6f72 706f 7261 7469 6f6e 2c3c ch Corporation,< │ │ │ │ +00001250: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00001270: 6120 6964 3d22 6c30 3030 3034 2220 6e61 a id="l00004" na │ │ │ │ +00001280: 6d65 3d22 6c30 3030 3034 223e 3c2f 613e me="l00004"> │ │ │ │ +00001290: 3c73 7061 6e20 636c 6173 733d 226c 696e 4 * Atlant │ │ │ │ +000012d0: 612c 2047 656f 7267 6961 2033 3033 3332 a, Georgia 30332 │ │ │ │ +000012e0: 2d30 3431 353c 2f73 7061 6e3e 3c2f 6469 -0415.
    5 │ │ │ │ +00001340: 3c2f 7370 616e 3e3c 7370 616e 2063 6c61 * │ │ │ │ +00001360: 416c 6c20 5269 6768 7473 2052 6573 6572 All Rights Reser │ │ │ │ +00001370: 7665 643c 2f73 7061 6e3e 3c2f 6469 763e ved
    │ │ │ │ +00001380: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000013b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 6 * Au │ │ │ │ +000013f0: 7468 6f72 733a 2046 7261 6e6b 2044 656c thors: Frank Del │ │ │ │ +00001400: 6c61 6572 742c 2065 7420 616c 2e20 2873 laert, et al. (s │ │ │ │ +00001410: 6565 2054 4841 4e4b 5320 666f 7220 7468 ee THANKS for th │ │ │ │ +00001420: 6520 6675 6c6c 2061 7574 686f 7220 6c69 e full author li │ │ │ │ +00001430: 7374 293c 2f73 7061 6e3e 3c2f 6469 763e st)
    │ │ │ │ +00001440: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    8 * See LICEN │ │ │ │ +00001530: 5345 2066 6f72 2074 6865 206c 6963 656e SE for the licen │ │ │ │ +00001540: 7365 2069 6e66 6f72 6d61 7469 6f6e 3c2f se information
    .
    < │ │ │ │ +00001590: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000015a0: 6e6f 223e 2020 2020 393c 2f73 7061 6e3e no"> 9 │ │ │ │ +000015b0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d .
    │ │ │ │ +00001620: 3130 3c2f 7370 616e 3e3c 7370 616e 2063 10 │ │ │ │ +00001640: 2a20 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d * -------------- │ │ │ │ +00001650: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001660: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001670: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00001680: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 202a 2f3c ------------ */< │ │ │ │ +00001690: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +000016b0: 6120 6964 3d22 6c30 3030 3131 2220 6e61 a id="l00011" na │ │ │ │ +000016c0: 6d65 3d22 6c30 3030 3131 223e 3c2f 613e me="l00011"> │ │ │ │ +000016d0: 3c73 7061 6e20 636c 6173 733d 226c 696e 11
    .
    19#pragma │ │ │ │ +00001770: 6f6e 6365 3c2f 7370 616e 3e3c 2f64 6976 once
    .
    20< │ │ │ │ +000017d0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +000017f0: 3c61 2069 643d 226c 3030 3032 3122 206e 21#in │ │ │ │ +00001850: 636c 7564 6520 266c 743b 3c61 2063 6c61 clude <gts │ │ │ │ +00001880: 616d 2f64 6973 6372 6574 652f 4465 6369 am/discrete/Deci │ │ │ │ +00001890: 7369 6f6e 5472 6565 4661 6374 6f72 2e68 sionTreeFactor.h │ │ │ │ +000018a0: 3c2f 613e 2667 743b 3c2f 7370 616e 3e3c >< │ │ │ │ +000018b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00001900: 2032 323c 2f73 7061 6e3e 3c73 7061 6e20 22#include &l │ │ │ │ +00001930: 743b 3c61 2063 6c61 7373 3d22 636f 6465 t;gtsam/discr │ │ │ │ +00001960: 6574 652f 4469 7363 7265 7465 4c6f 6f6b ete/DiscreteLook │ │ │ │ +00001970: 7570 4441 472e 683c 2f61 3e26 6774 3b3c upDAG.h>< │ │ │ │ +00001980: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +000019a0: 6120 6964 3d22 6c30 3030 3233 2220 6e61 a id="l00023" na │ │ │ │ +000019b0: 6d65 3d22 6c30 3030 3233 223e 3c2f 613e me="l00023"> │ │ │ │ +000019c0: 3c73 7061 6e20 636c 6173 733d 226c 696e 23#inc │ │ │ │ +00001a00: 6c75 6465 2026 6c74 3b3c 6120 636c 6173 lude <gtsa │ │ │ │ +00001a30: 6d2f 696e 6665 7265 6e63 652f 456c 696d m/inference/Elim │ │ │ │ +00001a40: 696e 6174 6561 626c 6546 6163 746f 7247 inateableFactorG │ │ │ │ +00001a50: 7261 7068 2e68 3c2f 613e 2667 743b 3c2f raph.h>
    .
    < │ │ │ │ +00001aa0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001ab0: 6e6f 223e 2020 2032 343c 2f73 7061 6e3e no"> 24 │ │ │ │ +00001ac0: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ +00001ae0: 7564 6520 266c 743b 3c61 2063 6c61 7373 ude <gtsam │ │ │ │ +00001b10: 2f69 6e66 6572 656e 6365 2f46 6163 746f /inference/Facto │ │ │ │ +00001b20: 7247 7261 7068 2e68 3c2f 613e 2667 743b rGraph.h> │ │ │ │ +00001b30: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00001b50: 3c61 2069 643d 226c 3030 3032 3522 206e 25#in │ │ │ │ +00001bb0: 636c 7564 6520 266c 743b 3c61 2063 6c61 clude <gts │ │ │ │ +00001be0: 616d 2f69 6e66 6572 656e 6365 2f4f 7264 am/inference/Ord │ │ │ │ +00001bf0: 6572 696e 672e 683c 2f61 3e26 6774 3b3c ering.h>< │ │ │ │ +00001c00: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00001c20: 6120 6964 3d22 6c30 3030 3236 2220 6e61 a id="l00026" na │ │ │ │ +00001c30: 6d65 3d22 6c30 3030 3236 223e 3c2f 613e me="l00026"> │ │ │ │ +00001c40: 3c73 7061 6e20 636c 6173 733d 226c 696e 26#inc │ │ │ │ +00001c80: 6c75 6465 2026 6c74 3b3c 6120 636c 6173 lude <gtsa │ │ │ │ +00001cb0: 6d2f 6261 7365 2f46 6173 7453 6574 2e68 m/base/FastSet.h │ │ │ │ +00001cc0: 3c2f 613e 2667 743b 3c2f 7370 616e 3e3c >< │ │ │ │ +00001cd0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00001d20: 2032 373c 2f73 7061 6e3e 203c 2f64 6976 27
    .
    28< │ │ │ │ +00001d80: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span>#include <bo │ │ │ │ +00001db0: 6f73 742f 6d61 6b65 5f73 6861 7265 642e ost/make_shared. │ │ │ │ +00001dc0: 6870 7026 6774 3b3c 2f73 7061 6e3e 3c2f hpp>.
    │ │ │ │ +00001e20: 3239 3c2f 7370 616e 3e3c 7370 616e 2063 29#include < │ │ │ │ +00001e50: 3b73 7472 696e 6726 6774 3b3c 2f73 7061 ;string>
    .
    30#include │ │ │ │ +00001ee0: 2026 6c74 3b75 7469 6c69 7479 2667 743b <utility> │ │ │ │ +00001ef0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00001f10: 3c61 2069 643d 226c 3030 3033 3122 206e 31#in │ │ │ │ +00001f70: 636c 7564 6520 266c 743b 7665 6374 6f72 clude <vector │ │ │ │ +00001f80: 2667 743b 3c2f 7370 616e 3e3c 2f64 6976 >
    .
    32< │ │ │ │ +00001fe0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00002000: 3c61 2069 643d 226c 3030 3033 3322 206e 33namespac │ │ │ │ +00002060: 6520 3c2f 7370 616e 3e3c 6120 636c 6173 e gtsam {
    .
    34 │ │ │ │ +00002100: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00002150: 2020 3335 3c2f 7370 616e 3e3c 7370 616e 35// Forward decl │ │ │ │ +00002180: 6172 6174 696f 6e73 3c2f 7370 616e 3e3c arations< │ │ │ │ +00002190: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000021e0: 2033 363c 2f73 7061 6e3e 3c73 7061 6e20 36 │ │ │ │ +00002200: 636c 6173 7320 3c2f 7370 616e 3e44 6973 class Dis │ │ │ │ +00002210: 6372 6574 6546 6163 746f 7247 7261 7068 creteFactorGraph │ │ │ │ +00002220: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +00002270: 2020 2033 373c 2f73 7061 6e3e 3c73 7061 37class D │ │ │ │ +000022a0: 6973 6372 6574 6543 6f6e 6469 7469 6f6e iscreteCondition │ │ │ │ +000022b0: 616c 3b3c 2f64 6976 3e0a 3c64 6976 2063 al;
    .
    38class DiscreteBayesNe │ │ │ │ +00002340: 743b 3c2f 6469 763e 0a3c 6469 7620 636c t;
    .
    39class │ │ │ │ +000023c0: 4469 7363 7265 7465 456c 696d 696e 6174 DiscreteEliminat │ │ │ │ +000023d0: 696f 6e54 7265 653b 3c2f 6469 763e 0a3c ionTree;
    .< │ │ │ │ +000023e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000023f0: 3e3c 6120 6964 3d22 6c30 3030 3430 2220 > 40class < │ │ │ │ +00002450: 2f73 7061 6e3e 4469 7363 7265 7465 4261 /span>DiscreteBa │ │ │ │ +00002460: 7965 7354 7265 653b 3c2f 6469 763e 0a3c yesTree;
    .< │ │ │ │ +00002470: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00002480: 3e3c 6120 6964 3d22 6c30 3030 3431 2220 > 41class < │ │ │ │ +000024e0: 2f73 7061 6e3e 4469 7363 7265 7465 4a75 /span>DiscreteJu │ │ │ │ +000024f0: 6e63 7469 6f6e 5472 6565 3b3c 2f64 6976 nctionTree;
    .
    42< │ │ │ │ +00002550: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00002570: 3c61 2069 643d 226c 3030 3035 3122 206e 51GTSAM_EXPORT.
    │ │ │ │ +00002610: 3532 3c2f 7370 616e 3e73 7464 3a3a 7061 52std::pa │ │ │ │ +00002620: 6972 266c 743b 4469 7363 7265 7465 436f ir<DiscreteCo │ │ │ │ +00002630: 6e64 6974 696f 6e61 6c3a 3a73 6861 7265 nditional::share │ │ │ │ +00002640: 645f 7074 722c 2044 6563 6973 696f 6e54 d_ptr, DecisionT │ │ │ │ +00002650: 7265 6546 6163 746f 723a 3a73 6861 7265 reeFactor::share │ │ │ │ +00002660: 645f 7074 7226 6774 3b3c 2f64 6976 3e0a d_ptr>
    . │ │ │ │ +00002670: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000026a0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 53EliminateD │ │ │ │ +00002720: 6973 6372 6574 653c 2f61 3e28 3c73 7061 iscrete(const D │ │ │ │ +00002750: 6973 6372 6574 6546 6163 746f 7247 7261 iscreteFactorGra │ │ │ │ +00002760: 7068 2661 6d70 3b20 6661 6374 6f72 732c ph& factors, │ │ │ │ +00002770: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000027c0: 2020 3534 3c2f 7370 616e 3e20 2020 2020 54 │ │ │ │ +000027d0: 2020 2020 2020 2020 2020 2020 203c 7370 const │ │ │ │ +00002800: 4f72 6465 7269 6e67 2661 6d70 3b20 6672 Ordering& fr │ │ │ │ +00002810: 6f6e 7461 6c4b 6579 7329 3b3c 2f64 6976 ontalKeys);
    .
    55< │ │ │ │ +00002870: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00002890: 3c61 2069 643d 226c 3030 3036 3422 206e 64GTSAM_EXPORT.
    │ │ │ │ +00002930: 3635 3c2f 7370 616e 3e73 7464 3a3a 7061 65std::pa │ │ │ │ +00002940: 6972 266c 743b 4469 7363 7265 7465 436f ir<DiscreteCo │ │ │ │ +00002950: 6e64 6974 696f 6e61 6c3a 3a73 6861 7265 nditional::share │ │ │ │ +00002960: 645f 7074 722c 2044 6563 6973 696f 6e54 d_ptr, DecisionT │ │ │ │ +00002970: 7265 6546 6163 746f 723a 3a73 6861 7265 reeFactor::share │ │ │ │ +00002980: 645f 7074 7226 6774 3b3c 2f64 6976 3e0a d_ptr>
    . │ │ │ │ +00002990: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000029c0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 66EliminateF │ │ │ │ +00002a40: 6f72 4d50 453c 2f61 3e28 3c73 7061 6e20 orMPE( │ │ │ │ +00002a60: 636f 6e73 743c 2f73 7061 6e3e 2044 6973 const Dis │ │ │ │ +00002a70: 6372 6574 6546 6163 746f 7247 7261 7068 creteFactorGraph │ │ │ │ +00002a80: 2661 6d70 3b20 6661 6374 6f72 732c 3c2f & factors,.
    │ │ │ │ +00002ae0: 3637 3c2f 7370 616e 3e20 2020 2020 2020 67 │ │ │ │ +00002af0: 2020 2020 2020 2020 203c 7370 616e 2063 c │ │ │ │ +00002b10: 6f6e 7374 3c2f 7370 616e 3e20 4f72 6465 onst Orde │ │ │ │ +00002b20: 7269 6e67 2661 6d70 3b20 6672 6f6e 7461 ring& fronta │ │ │ │ +00002b30: 6c4b 6579 7329 3b3c 2f64 6976 3e0a 3c64 lKeys);
    . │ │ │ │ +00002b50: 3c61 2069 643d 226c 3030 3036 3822 206e 68
    .
    │ │ │ │ +00002be0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    70< │ │ │ │ +00002d90: 2f73 7061 6e3e 7b3c 2f64 6976 3e0a 3c64 /span>{
    . │ │ │ │ +00002db0: 3c61 2069 643d 226c 3030 3037 3122 206e 71 │ │ │ │ +00002e30: 3c2f 613e 3c2f 7370 616e 3e20 203c 7370 typedef Di │ │ │ │ +00002e90: 7363 7265 7465 4661 6374 6f72 3c2f 613e screteFactor │ │ │ │ +00002ea0: 203c 6120 636c 6173 733d 2263 6f64 6520 FactorType; │ │ │ │ +00002f00: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00002f10: 2020 203c 2f64 6976 3e0a 3c64 6976 2063
    .
    72 │ │ │ │ +00002fb0: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +00002fd0: 7970 6564 6566 3c2f 7370 616e 3e20 3c61 ypedef Discre │ │ │ │ +00003010: 7465 4661 6374 6f72 4772 6170 683c 2f61 teFactorGraph FactorGraphTyp │ │ │ │ +00003080: 653c 2f61 3e3b 2020 2020 2020 2020 203c e; < │ │ │ │ +00003090: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>... │ │ │ │ +000033a0: 3c61 2069 643d 226c 3030 3037 3522 206e 75 │ │ │ │ +00003420: 3c2f 613e 3c2f 7370 616e 3e20 203c 7370 typedef Di │ │ │ │ +00003480: 7363 7265 7465 456c 696d 696e 6174 696f screteEliminatio │ │ │ │ +00003490: 6e54 7265 653c 2f61 3e20 3c61 2063 6c61 nTree Elimin │ │ │ │ +000034f0: 6174 696f 6e54 7265 6554 7970 653c 2f61 ationTreeType;
    .
    76 │ │ │ │ +000035a0: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +000035c0: 7970 6564 6566 3c2f 7370 616e 3e20 3c61 ypedef Discre │ │ │ │ +00003600: 7465 4261 7965 7354 7265 653c 2f61 3e20 teBayesTree │ │ │ │ +00003610: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ +00003660: 4261 7965 7354 7265 6554 7970 653c 2f61 BayesTreeType; < │ │ │ │ +00003680: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    77 typed │ │ │ │ +00003740: 6566 3c2f 7370 616e 3e20 3c61 2063 6c61 ef DiscreteJu │ │ │ │ +00003780: 6e63 7469 6f6e 5472 6565 3c2f 613e 203c nctionTree < │ │ │ │ +00003790: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +000037a0: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ +000037b0: 6130 3238 3630 2e68 746d 6c23 6133 3130 a02860.html#a310 │ │ │ │ +000037c0: 3563 6436 3531 3264 3136 3734 6436 6434 5cd6512d1674d6d4 │ │ │ │ +000037d0: 3333 3033 3463 3837 6234 6530 6322 3e4a 33034c87b4e0c">J │ │ │ │ +000037e0: 756e 6374 696f 6e54 7265 6554 7970 653c unctionTreeType< │ │ │ │ +000037f0: 2f61 3e3b 2020 2020 2020 203c 2f64 6976 /a>;
    .
    78< │ │ │ │ +00003850: 2f73 7061 6e3e 2020 3c2f 6469 763e 0a3c /span>
    .< │ │ │ │ +00003860: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00003870: 3e3c 6120 6964 3d22 6c30 3030 3830 2220 > 80
    stati │ │ │ │ +000038d0: 633c 2f73 7061 6e3e 2073 7464 3a3a 7061 c std::pa │ │ │ │ +000038e0: 6972 266c 743b 626f 6f73 743a 3a73 6861 ir<boost::sha │ │ │ │ +000038f0: 7265 645f 7074 7226 6c74 3b43 6f6e 6469 red_ptr<Condi │ │ │ │ +00003900: 7469 6f6e 616c 5479 7065 2667 743b 2c3c tionalType>,< │ │ │ │ +00003910: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00003960: 2038 313c 2f73 7061 6e3e 2020 2020 2020 81 │ │ │ │ +00003970: 2020 2020 2020 2020 2020 2020 2062 6f6f boo │ │ │ │ +00003980: 7374 3a3a 7368 6172 6564 5f70 7472 266c st::shared_ptr&l │ │ │ │ +00003990: 743b 4661 6374 6f72 5479 7065 2667 743b t;FactorType> │ │ │ │ +000039a0: 2026 6774 3b3c 2f64 6976 3e0a 3c64 6976 >
    .
    .
    │ │ │ │ +00003a80: 2020 3832 3c2f 613e 3c2f 7370 616e 3e20 82 │ │ │ │ +00003a90: 203c 6120 636c 6173 733d 2263 6f64 6520 DefaultElimina │ │ │ │ +00003af0: 7465 3c2f 613e 283c 7370 616e 2063 6c61 te(con │ │ │ │ +00003b10: 7374 3c2f 7370 616e 3e20 3c61 2063 6c61 st FactorGrap │ │ │ │ +00003b50: 6854 7970 653c 2f61 3e26 616d 703b 2066 hType& f │ │ │ │ +00003b60: 6163 746f 7273 2c20 3c73 7061 6e20 636c actors, co │ │ │ │ +00003b80: 6e73 743c 2f73 7061 6e3e 203c 6120 636c nst Ordering< │ │ │ │ +00003bc0: 2f61 3e26 616d 703b 206b 6579 7329 207b /a>& keys) { │ │ │ │ +00003bd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00003c20: 2020 3833 3c2f 7370 616e 3e20 2020 203c 83 < │ │ │ │ +00003c30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00003c40: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ +00003c50: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> Elimina │ │ │ │ +00003cb0: 7465 4469 7363 7265 7465 3c2f 613e 2866 teDiscrete(f │ │ │ │ +00003cc0: 6163 746f 7273 2c20 6b65 7973 293b 3c2f actors, keys);.
    │ │ │ │ +00003d20: 3834 3c2f 7370 616e 3e20 207d 3c2f 6469 84 }.
    .
    85 │ │ │ │ +00003d90: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ +00003df0: 6120 6964 3d22 6c30 3030 3837 2220 6e61 a id="l00087" na │ │ │ │ +00003e00: 6d65 3d22 6c30 3030 3837 223e 3c2f 613e me="l00087"> │ │ │ │ +00003e10: 3c73 7061 6e20 636c 6173 733d 226c 696e 87< │ │ │ │ +00003e70: 2f61 3e3c 2f73 7061 6e3e 2020 3c73 7061 /a> static │ │ │ │ +00003ea0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Orde │ │ │ │ +00003ed0: 7269 6e67 3c2f 613e 203c 6120 636c 6173 ring Defaul │ │ │ │ +00003f30: 744f 7264 6572 696e 6746 756e 633c 2f61 tOrderingFunc(
    .
    88 │ │ │ │ +00003fa0: 2020 203c 7370 616e 2063 6c61 7373 3d22 const FactorGraphTyp │ │ │ │ +00004000: 653c 2f61 3e26 616d 703b 2067 7261 7068 e& graph │ │ │ │ +00004010: 2c3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ,
    .
    │ │ │ │ +00004060: 2020 2038 393c 2f73 7061 6e3e 2020 2020 89 │ │ │ │ +00004070: 2020 626f 6f73 743a 3a6f 7074 696f 6e61 boost::optiona │ │ │ │ +00004080: 6c26 6c74 3b63 6f6e 7374 2056 6172 6961 l<const Varia │ │ │ │ +00004090: 626c 6549 6e64 6578 2661 6d70 3b26 6774 bleIndex&> │ │ │ │ +000040a0: 3b20 7661 7269 6162 6c65 496e 6465 7829 ; variableIndex) │ │ │ │ +000040b0: 207b 3c2f 6469 763e 0a3c 6469 7620 636c {
    .
    90 │ │ │ │ +00004110: 203c 7370 616e 2063 6c61 7373 3d22 6b65 retur │ │ │ │ +00004130: 6e3c 2f73 7061 6e3e 203c 6120 636c 6173 n Orderi │ │ │ │ +00004190: 6e67 3a3a 436f 6c61 6d64 3c2f 613e 282a ng::Colamd(* │ │ │ │ +000041a0: 7661 7269 6162 6c65 496e 6465 7829 3b3c variableIndex);< │ │ │ │ +000041b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00004200: 2039 313c 2f73 7061 6e3e 2020 7d3c 2f64 91 }.
    .
    92} │ │ │ │ +00004270: 3b3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c ;
    .
    .< │ │ │ │ +00004280: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00004290: 3e3c 6120 6964 3d22 6c30 3030 3933 2220 > 93
    .
    .
    │ │ │ │ +00004390: 3939 3c2f 613e 3c2f 7370 616e 3e3c 7370 99class │ │ │ │ +000043c0: 4754 5341 4d5f 4558 504f 5254 203c 6120 GTSAM_EXPORT Discret │ │ │ │ +00004400: 6546 6163 746f 7247 7261 7068 3c2f 613e eFactorGraph │ │ │ │ +00004410: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00004460: 2031 3030 3c2f 7370 616e 3e20 2020 203a 100 : │ │ │ │ +00004470: 203c 7370 616e 2063 6c61 7373 3d22 6b65 public FactorGraph │ │ │ │ +000044d0: 266c 743b 4469 7363 7265 7465 4661 6374 <DiscreteFact │ │ │ │ +000044e0: 6f72 2667 743b 2c3c 2f64 6976 3e0a 3c64 or>,
    . │ │ │ │ +00004500: 3c61 2069 643d 226c 3030 3130 3122 206e 101 pu │ │ │ │ +00004560: 626c 6963 3c2f 7370 616e 3e20 3c61 2063 blic Eliminat │ │ │ │ +000045a0: 6561 626c 6546 6163 746f 7247 7261 7068 eableFactorGraph │ │ │ │ +000045b0: 3c2f 613e 266c 743b 4469 7363 7265 7465 <Discrete │ │ │ │ +000045c0: 4661 6374 6f72 4772 6170 6826 6774 3b20 FactorGraph> │ │ │ │ +000045d0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +00004620: 2020 3130 323c 2f73 7061 6e3e 203c 7370 102 public │ │ │ │ +00004650: 3a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 :
    .
    │ │ │ │ +000046a0: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 103 usi │ │ │ │ +00004710: 6e67 203c 2f73 7061 6e3e 3c61 2063 6c61 ng This = │ │ │ │ +00004750: 203c 6120 636c 6173 733d 2263 6f64 6520 Dis │ │ │ │ +00004780: 6372 6574 6546 6163 746f 7247 7261 7068 creteFactorGraph │ │ │ │ +00004790: 3c2f 613e 3b20 2020 2020 2020 2020 203c ; < │ │ │ │ +000047a0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    104 using │ │ │ │ +00004860: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 Base = < │ │ │ │ +000048a0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +000048b0: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ +000048c0: 3335 3234 2e68 746d 6c22 3e46 6163 746f 3524.html">Facto │ │ │ │ +000048d0: 7247 7261 7068 266c 743b 4469 7363 7265 rGraph<Discre │ │ │ │ +000048e0: 7465 4661 6374 6f72 2667 743b 3c2f 613e teFactor> │ │ │ │ +000048f0: 3b20 203c 2f64 6976 3e0a 3c64 6976 2063 ;
    .
    105 │ │ │ │ +00004990: 3c2f 7370 616e 3e20 203c 7370 616e 2063 u │ │ │ │ +000049b0: 7369 6e67 203c 2f73 7061 6e3e 3c61 2063 sing Base │ │ │ │ +00004a10: 456c 696d 696e 6174 6561 626c 653c 2f61 Eliminateable =
    ..
    107 │ │ │ │ +00004b80: 3c2f 7370 616e 3e20 203c 7370 616e 2063 u │ │ │ │ +00004ba0: 7369 6e67 203c 2f73 7061 6e3e 3c61 2063 sing shar │ │ │ │ +00004c00: 6564 5f70 7472 3c2f 613e 203d 2062 6f6f ed_ptr = boo │ │ │ │ +00004c10: 7374 3a3a 7368 6172 6564 5f70 7472 266c st::shared_ptr&l │ │ │ │ +00004c20: 743b 5468 6973 2667 743b 3b20 203c 2f64 t;This>; .
    10 │ │ │ │ +00004c80: 383c 2f73 7061 6e3e 203c 2f64 6976 3e0a 8
    . │ │ │ │ +00004c90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00004cc0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 1 │ │ │ │ +00004d20: 3039 3c2f 613e 3c2f 7370 616e 3e20 203c 09 < │ │ │ │ +00004d30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00004d40: 6f72 6422 3e75 7369 6e67 203c 2f73 7061 ord">using Va │ │ │ │ +00004d80: 6c75 6573 3c2f 613e 203d 203c 6120 636c lues = DiscreteV │ │ │ │ +00004dc0: 616c 7565 733c 2f61 3e3b 2020 3c2f 6469 alues; .
    110 │ │ │ │ +00004e20: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +00004e30: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00004e40: 3e3c 6120 6964 3d22 6c30 3031 3131 2220 > 111 using │ │ │ │ +00004ea0: 203c 2f73 7061 6e3e 496e 6469 6365 7320 Indices │ │ │ │ +00004eb0: 3d20 3c61 2063 6c61 7373 3d22 636f 6465 = KeyVector; │ │ │ │ +00004f10: 2020 3c2f 6469 763e 0a3c 6469 7620 636c
    .
    112 .
    114 DiscreteFac │ │ │ │ +00005070: 746f 7247 7261 7068 3c2f 613e 2829 207b torGraph() { │ │ │ │ +00005080: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -000050d0: 2020 3130 383c 2f73 7061 6e3e 203c 2f64 108 .
    .
    110 │ │ │ │ -000051c0: 3c2f 7370 616e 3e20 2020 203c 7370 616e const Y& │ │ │ │ -000051f0: 616d 703b 203c 6120 636c 6173 733d 2263 amp; operator() │ │ │ │ -00005250: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ -00005270: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Assignment&l │ │ │ │ -000052b0: 743b 4c26 6774 3b3c 2f61 3e26 616d 703b t;L>& │ │ │ │ -000052c0: 2078 293c 7370 616e 2063 6c61 7373 3d22 x) const │ │ │ │ -000052e0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -000052f0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -00005340: 2020 3131 313c 2f73 7061 6e3e 2020 2020 111 │ │ │ │ -00005350: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ -00005370: 726e 3c2f 7370 616e 3e20 636f 6e73 7461 rn consta │ │ │ │ -00005380: 6e74 5f3b 3c2f 6469 763e 0a3c 6469 7620 nt_;
    .
    112 │ │ │ │ -000053e0: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
    .
    .
    113< │ │ │ │ -00005440: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    ..
    115 NodePtr │ │ │ │ -00005590: 203c 6120 636c 6173 733d 2263 6f64 6520 apply(const < │ │ │ │ -00005610: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00005620: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ -00005630: 6130 3237 3936 2e68 746d 6c23 6136 3065 a02796.html#a60e │ │ │ │ -00005640: 6436 3136 3235 3934 3564 3039 3636 3130 d61625945d096610 │ │ │ │ -00005650: 3361 3539 6439 6334 3763 3861 3622 3e55 3a59d9c47c8a6">U │ │ │ │ -00005660: 6e61 7279 3c2f 613e 2661 6d70 3b20 6f70 nary& op │ │ │ │ -00005670: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const ov │ │ │ │ -00005690: 6572 7269 6465 203c 2f73 7061 6e3e 7b3c erride {< │ │ │ │ -000056a0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -000056f0: 3131 363c 2f73 7061 6e3e 2020 2020 2020 116 │ │ │ │ -00005700: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ -00005750: 4e6f 6465 5074 723c 2f61 3e20 6628 3c73 NodePtr f(new < │ │ │ │ -00005780: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00005790: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -000057a0: 3032 3737 362e 6874 6d6c 223e 4c65 6166 02776.html">Leaf │ │ │ │ -000057b0: 3c2f 613e 286f 7028 636f 6e73 7461 6e74 (op(constant │ │ │ │ -000057c0: 5f29 2c20 6e72 4173 7369 676e 6d65 6e74 _), nrAssignment │ │ │ │ -000057d0: 735f 2929 3b3c 2f64 6976 3e0a 3c64 6976 s_));
    .
    < │ │ │ │ -00005810: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00005820: 6e6f 223e 2020 3131 373c 2f73 7061 6e3e no"> 117 │ │ │ │ -00005830: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ -00005850: 7265 7475 726e 3c2f 7370 616e 3e20 663b return f; │ │ │ │ -00005860: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -000058b0: 2031 3138 3c2f 7370 616e 3e20 2020 207d 118 } │ │ │ │ -000058c0: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c64
    .
    . │ │ │ │ -000058e0: 3c61 2069 643d 226c 3030 3131 3922 206e 119
    .
    . │ │ │ │ -00005970: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -000059a0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 1 │ │ │ │ -00005a00: 3231 3c2f 613e 3c2f 7370 616e 3e20 2020 21 │ │ │ │ -00005a10: 203c 6120 636c 6173 733d 2263 6f64 6520 NodePtr ap │ │ │ │ -00005ac0: 706c 793c 2f61 3e28 3c73 7061 6e20 636c ply(co │ │ │ │ -00005ae0: 6e73 743c 2f73 7061 6e3e 2055 6e61 7279 nst Unary │ │ │ │ -00005af0: 4173 7369 676e 6d65 6e74 2661 6d70 3b20 Assignment& │ │ │ │ -00005b00: 6f70 2c3c 2f64 6976 3e0a 3c64 6976 2063 op,
    .
    122 │ │ │ │ -00005b60: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00005b70: 3c73 7061 6e20 636c 6173 733d 226b 6579 const A │ │ │ │ -00005bc0: 7373 6967 6e6d 656e 7426 6c74 3b4c 2667 ssignment<L&g │ │ │ │ -00005bd0: 743b 3c2f 613e 2661 6d70 3b20 6173 7369 t;& assi │ │ │ │ -00005be0: 676e 6d65 6e74 293c 7370 616e 2063 6c61 gnment) co │ │ │ │ -00005c00: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override {
    .
    < │ │ │ │ -00005c50: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00005c60: 6e6f 223e 2020 3132 333c 2f73 7061 6e3e no"> 123 │ │ │ │ -00005c70: 2020 2020 2020 3c61 2063 6c61 7373 3d22 NodePtr f(new Leaf(op(as │ │ │ │ -00005d30: 7369 676e 6d65 6e74 2c20 636f 6e73 7461 signment, consta │ │ │ │ -00005d40: 6e74 5f29 2c20 6e72 4173 7369 676e 6d65 nt_), nrAssignme │ │ │ │ -00005d50: 6e74 735f 2929 3b3c 2f64 6976 3e0a 3c64 nts_));
    . │ │ │ │ -00005d70: 3c61 2069 643d 226c 3030 3132 3422 206e 124 return │ │ │ │ -00005de0: 663b 3c2f 6469 763e 0a3c 6469 7620 636c f;
    .
    125 │ │ │ │ -00005e40: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ -00005e50: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ -00005ee0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00005ef0: 6e6f 223e 2020 3132 373c 2f73 7061 6e3e no"> 127 │ │ │ │ -00005f00: 2020 2020 3c73 7061 6e20 636c 6173 733d // App │ │ │ │ -00005f20: 6c79 2062 696e 6172 7920 6f70 6572 6174 ly binary operat │ │ │ │ -00005f30: 6f72 2026 7175 6f74 3b68 203d 2066 206f or "h = f o │ │ │ │ -00005f40: 7020 6726 7175 6f74 3b20 6f6e 204c 6561 p g" on Lea │ │ │ │ -00005f50: 6620 6e6f 6465 3c2f 7370 616e 3e3c 2f64 f node.
    12 │ │ │ │ -00005fb0: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 // Note op is │ │ │ │ -00005fe0: 6e6f 7420 6173 7375 6d65 6420 636f 6d6d not assumed comm │ │ │ │ -00005ff0: 7574 6174 6976 6520 736f 2077 6520 6e65 utative so we ne │ │ │ │ -00006000: 6564 2074 6f20 6b65 6570 2074 7261 636b ed to keep track │ │ │ │ -00006010: 206f 6620 6f72 6465 723c 2f73 7061 6e3e of order │ │ │ │ -00006020: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00006070: 2031 3239 3c2f 7370 616e 3e20 2020 203c 129 < │ │ │ │ -00006080: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00006090: 656e 7422 3e2f 2f20 5369 6d70 6c79 2063 ent">// Simply c │ │ │ │ -000060a0: 616c 6c73 2061 7070 6c79 206f 6e20 6172 alls apply on ar │ │ │ │ -000060b0: 6775 6d65 6e74 2074 6f20 6361 6c6c 2063 gument to call c │ │ │ │ -000060c0: 6f72 7265 6374 2076 6972 7475 616c 206d orrect virtual m │ │ │ │ -000060d0: 6574 686f 643a 3c2f 7370 616e 3e3c 2f64 ethod:.
    13 │ │ │ │ -00006130: 303c 2f73 7061 6e3e 2020 2020 3c73 7061 0 // fL.apply_f_ │ │ │ │ -00006160: 6f70 5f67 2867 4c29 202d 2667 743b 2067 op_g(gL) -> g │ │ │ │ -00006170: 4c2e 6170 706c 795f 675f 6f70 5f66 4c28 L.apply_g_op_fL( │ │ │ │ -00006180: 664c 2920 2862 656c 6f77 293c 2f73 7061 fL) (below)
    .
    131 │ │ │ │ -000061f0: 203c 7370 616e 2063 6c61 7373 3d22 636f // fL.app │ │ │ │ -00006210: 6c79 5f66 5f6f 705f 6728 6743 2920 2d26 ly_f_op_g(gC) -& │ │ │ │ -00006220: 6774 3b20 6743 2e61 7070 6c79 5f67 5f6f gt; gC.apply_g_o │ │ │ │ -00006230: 705f 664c 2866 4c29 2028 4368 6f69 6365 p_fL(fL) (Choice │ │ │ │ -00006240: 293c 2f73 7061 6e3e 3c2f 6469 763e 0a3c )
    .< │ │ │ │ -00006250: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00006260: 3e3c 6120 6964 3d22 6c30 3031 3332 2220 > 132 NodePtr apply_f_op_g( │ │ │ │ -00006310: 3c73 7061 6e20 636c 6173 733d 226b 6579 const │ │ │ │ -00006360: 4e6f 6465 3c2f 613e 2661 6d70 3b20 672c Node& g, │ │ │ │ -00006370: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const Binary& │ │ │ │ -000063a0: 6f70 293c 7370 616e 2063 6c61 7373 3d22 op) const │ │ │ │ -000063c0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -000063d0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -00006420: 2020 3133 333c 2f73 7061 6e3e 2020 2020 133 │ │ │ │ -00006430: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ -00006450: 726e 3c2f 7370 616e 3e20 672e 6170 706c rn g.appl │ │ │ │ -00006460: 795f 675f 6f70 5f66 4c28 2a3c 7370 616e y_g_op_fL(*this, op │ │ │ │ -00006490: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    134 │ │ │ │ -000064f0: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    135 .
    1 │ │ │ │ -000065a0: 3336 3c2f 7370 616e 3e20 2020 203c 7370 36 // Applying b │ │ │ │ -000065d0: 696e 6172 7920 6f70 6572 6174 6f72 2074 inary operator t │ │ │ │ -000065e0: 6f20 7477 6f20 6c65 6176 6573 2072 6573 o two leaves res │ │ │ │ -000065f0: 756c 7473 2069 6e20 6120 6c65 6166 3c2f ults in a leaf
    .
    < │ │ │ │ -00006640: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00006650: 6e6f 223e 2020 3133 373c 2f73 7061 6e3e no"> 137 │ │ │ │ -00006660: 2020 2020 3c61 2063 6c61 7373 3d22 636f NodePtr │ │ │ │ -000066c0: 6170 706c 795f 675f 6f70 5f66 4c28 3c73 apply_g_op_fL(const │ │ │ │ -000066f0: 204c 6561 6626 616d 703b 2066 4c2c 203c Leaf& fL, < │ │ │ │ -00006700: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00006710: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const Binary& op │ │ │ │ -00006730: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const ov │ │ │ │ -00006750: 6572 7269 6465 203c 2f73 7061 6e3e 7b3c erride {< │ │ │ │ -00006760: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -000067b0: 3133 383c 2f73 7061 6e3e 2020 2020 2020 138 │ │ │ │ -000067c0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // fL op g │ │ │ │ -000067e0: 4c3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c L
    .< │ │ │ │ -000067f0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00006800: 3e3c 6120 6964 3d22 6c30 3031 3339 2220 > 139 NodePtr │ │ │ │ -000068a0: 3c2f 613e 2068 283c 7370 616e 2063 6c61 h(new │ │ │ │ -000068c0: 3c2f 7370 616e 3e20 4c65 6166 286f 7028 Leaf(op( │ │ │ │ -000068d0: 664c 2e63 6f6e 7374 616e 745f 2c20 636f fL.constant_, co │ │ │ │ -000068e0: 6e73 7461 6e74 5f29 2c20 6e72 4173 7369 nstant_), nrAssi │ │ │ │ -000068f0: 676e 6d65 6e74 735f 2929 3b3c 2f64 6976 gnments_));
    .
    140< │ │ │ │ -00006950: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return h;
    .< │ │ │ │ -000069a0: 6120 6964 3d22 6c30 3031 3431 2220 6e61 a id="l00141" na │ │ │ │ -000069b0: 6d65 3d22 6c30 3031 3431 223e 3c2f 613e me="l00141"> │ │ │ │ -000069c0: 3c73 7061 6e20 636c 6173 733d 226c 696e 141 }
    .< │ │ │ │ -00006a00: 6120 6964 3d22 6c30 3031 3432 2220 6e61 a id="l00142" na │ │ │ │ -00006a10: 6d65 3d22 6c30 3031 3432 223e 3c2f 613e me="l00142"> │ │ │ │ -00006a20: 3c73 7061 6e20 636c 6173 733d 226c 696e 142
    .
    143 │ │ │ │ -00006aa0: 203c 7370 616e 2063 6c61 7373 3d22 636f // If sec │ │ │ │ -00006ac0: 6f6e 6420 6172 6775 6d65 6e74 2069 7320 ond argument is │ │ │ │ -00006ad0: 6120 4368 6f69 6365 206e 6f64 652c 2063 a Choice node, c │ │ │ │ -00006ae0: 616c 6c20 6974 2623 3339 3b73 2061 7070 all it's app │ │ │ │ -00006af0: 6c79 2077 6974 6820 6c65 6166 2061 7320 ly with leaf as │ │ │ │ -00006b00: 7365 636f 6e64 3c2f 7370 616e 3e3c 2f64 second.
    14 │ │ │ │ -00006b60: 343c 2f73 7061 6e3e 2020 2020 3c61 2063 4 Node │ │ │ │ -00006bc0: 5074 723c 2f61 3e20 6170 706c 795f 675f Ptr apply_g_ │ │ │ │ -00006bd0: 6f70 5f66 4328 3c73 7061 6e20 636c 6173 op_fC(cons │ │ │ │ -00006bf0: 743c 2f73 7061 6e3e 2043 686f 6963 6526 t Choice& │ │ │ │ -00006c00: 616d 703b 2066 432c 203c 7370 616e 2063 amp; fC, c │ │ │ │ -00006c20: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ -00006c30: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const override │ │ │ │ -00006c60: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ -00006c70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    │ │ │ │ -00006d70: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ -00006dd0: 0a3c 6469 7620 636c 6173 733d 226c 696e ...
    │ │ │ │ -00006f00: 2020 3134 393c 2f61 3e3c 2f73 7061 6e3e 149 │ │ │ │ -00006f10: 2020 2020 3c61 2063 6c61 7373 3d22 636f NodePtr │ │ │ │ -00006f70: 3c61 2063 6c61 7373 3d22 636f 6465 2068 choose(const L │ │ │ │ -00006ff0: 2661 6d70 3b20 6c61 6265 6c2c 203c 7370 & label, size_t index) const override │ │ │ │ -00007050: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ -00007060: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00007090: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 150 return NodePtr(new < │ │ │ │ -00007160: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00007170: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -00007180: 3032 3737 362e 6874 6d6c 223e 4c65 6166 02776.html">Leaf │ │ │ │ -00007190: 3c2f 613e 2863 6f6e 7374 616e 7428 292c (constant(), │ │ │ │ -000071a0: 206e 7241 7373 6967 6e6d 656e 7473 2829 nrAssignments() │ │ │ │ -000071b0: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
    .
    151 │ │ │ │ -00007210: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -00007220: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00007290: 6120 6964 3d22 6c30 3031 3533 2220 6e61 a id="l00153" na │ │ │ │ -000072a0: 6d65 3d22 6c30 3031 3533 223e 3c2f 613e me="l00153"> │ │ │ │ -000072b0: 3c73 7061 6e20 636c 6173 733d 226c 696e 153 b │ │ │ │ -000072f0: 6f6f 6c3c 2f73 7061 6e3e 2069 734c 6561 ool isLea │ │ │ │ -00007300: 6628 293c 7370 616e 2063 6c61 7373 3d22 f() const │ │ │ │ -00007320: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -00007330: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ -00007350: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ -00007370: 7472 7565 3c2f 7370 616e 3e3b 207d 3c2f true; }.
    .. │ │ │ │ -00007470: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -000074a0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 155 pri │ │ │ │ -000074e0: 7661 7465 3c2f 7370 616e 3e3a 3c2f 6469 vate:.
    156 │ │ │ │ -00007540: 3c2f 7370 616e 3e20 2020 203c 7370 616e using Ba │ │ │ │ -00007570: 7365 203d 203c 6120 636c 6173 733d 2263 se = DecisionTree&l │ │ │ │ -000075b0: 743b 4c2c 2059 2667 743b 3a3a 4e6f 6465 t;L, Y>::Node │ │ │ │ -000075c0: 3c2f 613e 3b3c 2f64 6976 3e0a 3c64 6976 ;
    .
    < │ │ │ │ -00007600: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00007610: 6e6f 223e 2020 3135 373c 2f73 7061 6e3e no"> 157 │ │ │ │ -00007620: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    │ │ │ │ -00007670: 2020 3135 393c 2f73 7061 6e3e 2020 2020 159 │ │ │ │ -00007680: 3c73 7061 6e20 636c 6173 733d 226b 6579 friend class │ │ │ │ -000076c0: 3c2f 7370 616e 3e62 6f6f 7374 3a3a 7365 boost::se │ │ │ │ -000076d0: 7269 616c 697a 6174 696f 6e3a 3a61 6363 rialization::acc │ │ │ │ -000076e0: 6573 733b 3c2f 6469 763e 0a3c 6469 7620 ess;
    .
    160 │ │ │ │ -00007740: 2020 203c 7370 616e 2063 6c61 7373 3d22 templat │ │ │ │ -00007760: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <class │ │ │ │ -00007790: 4152 4348 4956 4526 6774 3b3c 2f64 6976 ARCHIVE>
    .
    161< │ │ │ │ -000077f0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> void │ │ │ │ -00007820: 7365 7269 616c 697a 6528 4152 4348 4956 serialize(ARCHIV │ │ │ │ -00007830: 4526 616d 703b 2061 722c 203c 7370 616e E& ar, const unsigned │ │ │ │ -00007880: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c int /*version*/) {
    .< │ │ │ │ -000078e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000078f0: 3e3c 6120 6964 3d22 6c30 3031 3632 2220 > 162 ar & │ │ │ │ -00007940: 3b20 424f 4f53 545f 5345 5249 414c 495a ; BOOST_SERIALIZ │ │ │ │ -00007950: 4154 494f 4e5f 4241 5345 5f4f 424a 4543 ATION_BASE_OBJEC │ │ │ │ -00007960: 545f 4e56 5028 4261 7365 293b 3c2f 6469 T_NVP(Base);.
    163 │ │ │ │ -000079c0: 3c2f 7370 616e 3e20 2020 2020 2061 7226 ar& │ │ │ │ -000079d0: 616d 703b 2042 4f4f 5354 5f53 4552 4941 amp; BOOST_SERIA │ │ │ │ -000079e0: 4c49 5a41 5449 4f4e 5f4e 5650 2863 6f6e LIZATION_NVP(con │ │ │ │ -000079f0: 7374 616e 745f 293b 3c2f 6469 763e 0a3c stant_);
    .< │ │ │ │ -00007a00: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00007a10: 3e3c 6120 6964 3d22 6c30 3031 3634 2220 > 164 ar& │ │ │ │ -00007a60: 2042 4f4f 5354 5f53 4552 4941 4c49 5a41 BOOST_SERIALIZA │ │ │ │ -00007a70: 5449 4f4e 5f4e 5650 286e 7241 7373 6967 TION_NVP(nrAssig │ │ │ │ -00007a80: 6e6d 656e 7473 5f29 3b3c 2f64 6976 3e0a nments_);
    . │ │ │ │ -00007a90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 . │ │ │ │ -00007af0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00007b20: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 166 }; │ │ │ │ -00007b60: 2f2f 204c 6561 663c 2f73 7061 6e3e 3c2f // Leaf.
    .. │ │ │ │ -00007c60: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    │ │ │ │ -00007d70: 2020 3136 393c 2f73 7061 6e3e 2020 3c73 169 // Choice
    .
    170< │ │ │ │ -00007e00: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00007e10: 656e 7422 3e20 202f 2a2a 2a2a 2a2a 2a2a ent"> /******** │ │ │ │ -00007e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00007e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00007e40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00007e50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00007e60: 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 ****/.
    171 │ │ │ │ -00007ec0: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ -00007ee0: 656d 706c 6174 653c 2f73 7061 6e3e 266c emplate&l │ │ │ │ -00007ef0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ -00007f10: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ -00007f40: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ -00008000: 2031 3732 3c2f 613e 3c2f 7370 616e 3e20 172 │ │ │ │ -00008010: 203c 7370 616e 2063 6c61 7373 3d22 6b65 struct DecisionTree<L, Y>::< │ │ │ │ -00008080: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00008090: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -000080a0: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ -000080b0: 6365 3c2f 613e 3a20 3c73 7061 6e20 636c ce: pu │ │ │ │ -000080d0: 626c 6963 3c2f 7370 616e 3e20 3c61 2063 blic Decision │ │ │ │ -00008110: 5472 6565 3c2f 613e 266c 743b 4c2c 2059 Tree<L, Y │ │ │ │ -00008120: 2667 743b 3a3a 4e6f 6465 207b 3c2f 6469 >::Node {.
    │ │ │ │ -000081c0: 2020 3137 343c 2f61 3e3c 2f73 7061 6e3e 174 │ │ │ │ -000081d0: 2020 2020 4c20 3c61 2063 6c61 7373 3d22 L label_;
    .
    175 .
    177 std::vecto │ │ │ │ -00008340: 7226 6c74 3b4e 6f64 6550 7472 2667 743b r<NodePtr> │ │ │ │ -00008350: 203c 6120 636c 6173 733d 2263 6f64 6520 branches_; │ │ │ │ -000083b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00008400: 2031 3738 3c2f 7370 616e 3e20 3c2f 6469 178 .
    .
    179< │ │ │ │ -000084f0: 2f73 7061 6e3e 2020 203c 7370 616e 2063 /span> p │ │ │ │ -00008510: 7269 7661 7465 3c2f 7370 616e 3e3a 3c2f rivate:.
    1 │ │ │ │ -00008570: 3834 3c2f 7370 616e 3e20 2020 203c 7370 84 size_t allSame_;.
    1 │ │ │ │ -00008600: 3835 3c2f 7370 616e 3e20 3c2f 6469 763e 85
    │ │ │ │ -00008610: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00008640: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 186 u │ │ │ │ -00008680: 7369 6e67 203c 2f73 7061 6e3e 4368 6f69 sing Choi │ │ │ │ -00008690: 6365 5074 7220 3d20 626f 6f73 743a 3a73 cePtr = boost::s │ │ │ │ -000086a0: 6861 7265 645f 7074 7226 6c74 3b63 6f6e hared_ptr<con │ │ │ │ -000086b0: 7374 2043 686f 6963 6526 6774 3b3b 3c2f st Choice>;.
    1 │ │ │ │ -00008710: 3837 3c2f 7370 616e 3e20 3c2f 6469 763e 87
    │ │ │ │ -00008720: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -000087b0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -000087c0: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -000087d0: 2361 6530 6238 3434 3437 3338 3233 6161 #ae0b844473823aa │ │ │ │ -000087e0: 3039 6337 3165 3636 3834 3439 3530 3963 09c71e668449509c │ │ │ │ -000087f0: 6432 223e 2020 3138 383c 2f61 3e3c 2f73 d2"> 188 pub │ │ │ │ -00008820: 6c69 633c 2f73 7061 6e3e 3a3c 2f64 6976 lic:
    .
    │ │ │ │ -000088c0: 2031 3930 3c2f 613e 3c2f 7370 616e 3e20 190 │ │ │ │ -000088d0: 2020 203c 6120 636c 6173 733d 2263 6f64 Choice() │ │ │ │ -00008930: 207b 7d3c 2f64 6976 3e0a 3c64 6976 2063 {}
    .
    . │ │ │ │ -00008980: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -000089b0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 1 │ │ │ │ -00008a10: 3931 3c2f 613e 3c2f 7370 616e 3e20 3c2f 91 .
    1 │ │ │ │ -00008a70: 3932 3c2f 7370 616e 3e20 2020 207e 4368 92 ~Ch │ │ │ │ -00008a80: 6f69 6365 2829 3c73 7061 6e20 636c 6173 oice() ove │ │ │ │ -00008aa0: 7272 6964 6520 3c2f 7370 616e 3e7b 3c2f rride {.
    1 │ │ │ │ -00008b00: 3933 3c2f 7370 616e 3e3c 7370 616e 2063 93#ifdef DT_DE │ │ │ │ -00008b30: 4255 475f 4d45 4d4f 5259 3c2f 7370 616e BUG_MEMORY
    .
    . │ │ │ │ -00008ba0: 3c61 2069 643d 226c 3030 3139 3422 206e 194 │ │ │ │ -00008c20: 3c2f 613e 3c2f 7370 616e 3e20 2020 2020 │ │ │ │ -00008c30: 2073 7464 3a3a 7374 643a 3a63 6f75 7420 std::std::cout │ │ │ │ -00008c40: 266c 743b 266c 743b 204e 6f64 653a 3a6e << Node::n │ │ │ │ -00008c50: 724e 6f64 6573 2026 6c74 3b26 6c74 3b20 rNodes << │ │ │ │ -00008c60: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ -00008c80: 743b 2064 6573 7472 7563 7469 6e67 2028 t; destructing ( │ │ │ │ -00008c90: 4368 6f69 6365 2920 2671 756f 743b 3c2f Choice) " << t │ │ │ │ -00008cb0: 6869 732d 2667 743b 6964 2829 3c2f 6469 his->id().
    195 │ │ │ │ -00008d10: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ -00008d20: 2020 2020 2020 2020 2020 2020 266c 743b < │ │ │ │ -00008d30: 266c 743b 2073 7464 3a3a 7374 643a 3a65 < std::std::e │ │ │ │ -00008d40: 6e64 6c3b 3c2f 6469 763e 0a3c 6469 7620 ndl;
    .
    196< │ │ │ │ -00008da0: 7370 616e 2063 6c61 7373 3d22 7072 6570 span class="prep │ │ │ │ -00008db0: 726f 6365 7373 6f72 223e 2365 6e64 6966 rocessor">#endif │ │ │ │ -00008dc0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    ...< │ │ │ │ -00008ed0: 6120 6964 3d22 6c30 3031 3938 2220 6e61 a id="l00198" na │ │ │ │ -00008ee0: 6d65 3d22 6c30 3031 3938 223e 3c2f 613e me="l00198"> │ │ │ │ -00008ef0: 3c73 7061 6e20 636c 6173 733d 226c 696e 198
    .
    .< │ │ │ │ -00008f60: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00008f70: 3e3c 6120 6964 3d22 6c30 3032 3030 2220 > 20 │ │ │ │ -00008ff0: 303c 2f61 3e3c 2f73 7061 6e3e 2020 2020 0 │ │ │ │ -00009000: 3c73 7061 6e20 636c 6173 733d 226b 6579 static NodePtr │ │ │ │ -00009080: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Unique(const C │ │ │ │ -00009100: 686f 6963 6550 7472 2661 6d70 3b20 6629 hoicePtr& f) │ │ │ │ -00009110: 207b 3c2f 6469 763e 0a3c 6469 7620 636c {
    .
    .< │ │ │ │ -00009160: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00009170: 3e3c 6120 6964 3d22 6c30 3032 3031 2220 > 20 │ │ │ │ -000091f0: 313c 2f61 3e3c 2f73 7061 6e3e 3c73 7061 1#ifndef G │ │ │ │ -00009220: 5453 414d 5f44 545f 4e4f 5f50 5255 4e49 TSAM_DT_NO_PRUNI │ │ │ │ -00009230: 4e47 3c2f 7370 616e 3e3c 2f64 6976 3e0a NG
    . │ │ │ │ -00009240: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -00009320: 2032 3033 3c2f 7370 616e 3e20 2020 2020 203 │ │ │ │ -00009330: 2020 2061 7373 6572 7428 662d 2667 743b assert(f-> │ │ │ │ -00009340: 6272 616e 6368 6573 2829 2e73 697a 6528 branches().size( │ │ │ │ -00009350: 2920 2667 743b 2030 293b 3c2f 6469 763e ) > 0);
    │ │ │ │ -00009360: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -000093f0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00009400: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -00009410: 2361 6662 3635 6266 6535 6433 6463 3262 #afb65bfe5d3dc2b │ │ │ │ -00009420: 3866 3239 3465 6138 3934 6161 6263 3235 8f294ea894aabc25 │ │ │ │ -00009430: 6133 223e 2020 3230 343c 2f61 3e3c 2f73 a3"> 204 Node │ │ │ │ -000094a0: 5074 723c 2f61 3e20 6630 203d 2066 2d26 Ptr f0 = f-& │ │ │ │ -000094b0: 6774 3b62 7261 6e63 6865 735f 5b30 5d3b gt;branches_[0]; │ │ │ │ -000094c0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00009510: 2032 3035 3c2f 7370 616e 3e20 3c2f 6469 205 .
    206 │ │ │ │ -00009570: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ -00009580: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00009590: 6f72 6474 7970 6522 3e73 697a 655f 743c ordtype">size_t< │ │ │ │ -000095a0: 2f73 7061 6e3e 206e 7241 7373 6967 6e6d /span> nrAssignm │ │ │ │ -000095b0: 656e 7473 203d 2030 3b3c 2f64 6976 3e0a ents = 0;
    . │ │ │ │ -000095c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -000095f0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 207 for │ │ │ │ -00009640: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (auto branch: f-> │ │ │ │ -00009670: 3b62 7261 6e63 6865 7328 2929 207b 3c2f ;branches()) {.
    2 │ │ │ │ -000096d0: 3038 3c2f 7370 616e 3e20 2020 2020 2020 08 │ │ │ │ -000096e0: 2020 2061 7373 6572 7428 6272 616e 6368 assert(branch │ │ │ │ -000096f0: 2d26 6774 3b69 734c 6561 6628 2929 3b3c ->isLeaf());< │ │ │ │ -00009700: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00009750: 3230 393c 2f73 7061 6e3e 2020 2020 2020 209 │ │ │ │ -00009760: 2020 2020 6e72 4173 7369 676e 6d65 6e74 nrAssignment │ │ │ │ -00009770: 7320 2b3d 3c2f 6469 763e 0a3c 6469 7620 s +=
    .
    210 │ │ │ │ -000097d0: 2020 2020 2020 2020 2020 2020 2062 6f6f boo │ │ │ │ -000097e0: 7374 3a3a 6479 6e61 6d69 635f 706f 696e st::dynamic_poin │ │ │ │ -000097f0: 7465 725f 6361 7374 266c 743b 636f 6e73 ter_cast<cons │ │ │ │ -00009800: 7420 4c65 6166 2667 743b 2862 7261 6e63 t Leaf>(branc │ │ │ │ -00009810: 6829 2d26 6774 3b6e 7241 7373 6967 6e6d h)->nrAssignm │ │ │ │ -00009820: 656e 7473 2829 3b3c 2f64 6976 3e0a 3c64 ents();
    . │ │ │ │ -00009840: 3c61 2069 643d 226c 3030 3231 3122 206e 211 }
    .
    212< │ │ │ │ -000098e0: 2f73 7061 6e3e 2020 2020 2020 2020 3c61 /span> No │ │ │ │ -00009940: 6465 5074 723c 2f61 3e20 6e65 774c 6561 dePtr newLea │ │ │ │ -00009950: 6628 3c2f 6469 763e 0a3c 6469 7620 636c f(
    .
    213 │ │ │ │ -000099b0: 2020 2020 2020 2020 203c 7370 616e 2063 n │ │ │ │ -000099d0: 6577 3c2f 7370 616e 3e20 4c65 6166 2862 ew Leaf(b │ │ │ │ -000099e0: 6f6f 7374 3a3a 6479 6e61 6d69 635f 706f oost::dynamic_po │ │ │ │ -000099f0: 696e 7465 725f 6361 7374 266c 743b 636f inter_cast<co │ │ │ │ -00009a00: 6e73 7420 4c65 6166 2667 743b 2866 3029 nst Leaf>(f0) │ │ │ │ -00009a10: 2d26 6774 3b63 6f6e 7374 616e 7428 292c ->constant(), │ │ │ │ -00009a20: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00009a70: 2032 3134 3c2f 7370 616e 3e20 2020 2020 214 │ │ │ │ -00009a80: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00009a90: 6e72 4173 7369 676e 6d65 6e74 7329 293b nrAssignments)); │ │ │ │ -00009aa0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ -00009b00: 6120 6964 3d22 6c30 3032 3135 2220 6e61 a id="l00215" na │ │ │ │ -00009b10: 6d65 3d22 6c30 3032 3135 223e 3c2f 613e me="l00215"> │ │ │ │ -00009b20: 3c73 7061 6e20 636c 6173 733d 226c 696e 215< │ │ │ │ -00009b80: 2f61 3e3c 2f73 7061 6e3e 2020 2020 2020 /a> │ │ │ │ -00009b90: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ -00009bb0: 726e 3c2f 7370 616e 3e20 6e65 774c 6561 rn newLea │ │ │ │ -00009bc0: 663b 3c2f 6469 763e 0a3c 6469 7620 636c f;
    .
    216 │ │ │ │ -00009c20: 2020 207d 203c 7370 616e 2063 6c61 7373 } e │ │ │ │ -00009c40: 6c73 653c 2f73 7061 6e3e 3c2f 6469 763e lse
    │ │ │ │ -00009c50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00009c80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 217 │ │ │ │ -00009cc0: 2365 6e64 6966 3c2f 7370 616e 3e3c 2f64 #endif.
    21 │ │ │ │ -00009d20: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ -00009d30: 3c73 7061 6e20 636c 6173 733d 226b 6579 return │ │ │ │ -00009d50: 3c2f 7370 616e 3e20 663b 3c2f 6469 763e f;
    │ │ │ │ -00009d60: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ -00009dc0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00009e30: 6120 6964 3d22 6c30 3032 3231 2220 6e61 a id="l00221" na │ │ │ │ -00009e40: 6d65 3d22 6c30 3032 3231 223e 3c2f 613e me="l00221"> │ │ │ │ -00009e50: 3c73 7061 6e20 636c 6173 733d 226c 696e 221 b │ │ │ │ -00009e90: 6f6f 6c3c 2f73 7061 6e3e 2069 734c 6561 ool isLea │ │ │ │ -00009ea0: 6628 293c 7370 616e 2063 6c61 7373 3d22 f() const │ │ │ │ -00009ec0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -00009ed0: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ -00009ef0: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ -00009f10: 6661 6c73 653c 2f73 7061 6e3e 3b20 7d3c false; }< │ │ │ │ -00009f20: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00009f70: 3232 323c 2f73 7061 6e3e 203c 2f64 6976 222
    .
    .
    │ │ │ │ -0000a010: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 224 Choice │ │ │ │ -0000a0c0: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ -0000a0e0: 3c2f 7370 616e 3e20 4c26 616d 703b 206c L& l │ │ │ │ -0000a0f0: 6162 656c 2c20 3c73 7061 6e20 636c 6173 abel, │ │ │ │ -0000a110: 7369 7a65 5f74 3c2f 7370 616e 3e20 636f size_t co │ │ │ │ -0000a120: 756e 7429 203a 3c2f 6469 763e 0a3c 6469 unt) :
    .< │ │ │ │ -0000a140: 6120 6964 3d22 6c30 3032 3235 2220 6e61 a id="l00225" na │ │ │ │ -0000a150: 6d65 3d22 6c30 3032 3235 223e 3c2f 613e me="l00225"> │ │ │ │ -0000a160: 3c73 7061 6e20 636c 6173 733d 226c 696e 225 label_(la │ │ │ │ -0000a190: 6265 6c29 2c20 616c 6c53 616d 655f 2874 bel), allSame_(t │ │ │ │ -0000a1a0: 7275 6529 207b 3c2f 6469 763e 0a3c 6469 rue) {
    .< │ │ │ │ -0000a1c0: 6120 6964 3d22 6c30 3032 3236 2220 6e61 a id="l00226" na │ │ │ │ -0000a1d0: 6d65 3d22 6c30 3032 3236 223e 3c2f 613e me="l00226"> │ │ │ │ -0000a1e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 226 branches_ │ │ │ │ -0000a210: 2e72 6573 6572 7665 2863 6f75 6e74 293b .reserve(count); │ │ │ │ -0000a220: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000a270: 2032 3237 3c2f 7370 616e 3e20 2020 207d 227 } │ │ │ │ -0000a280: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c64
    .
    ...
    .< │ │ │ │ -0000a3c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000a3d0: 3e3c 6120 6964 3d22 6c30 3032 3330 2220 > 23 │ │ │ │ -0000a450: 303c 2f61 3e3c 2f73 7061 6e3e 2020 2020 0 │ │ │ │ -0000a460: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Choice(const < │ │ │ │ -0000a4e0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0000a4f0: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -0000a500: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ -0000a510: 6365 3c2f 613e 2661 6d70 3b20 662c 203c ce& f, < │ │ │ │ -0000a520: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000a530: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const C │ │ │ │ -0000a570: 686f 6963 653c 2f61 3e26 616d 703b 2067 hoice& g │ │ │ │ -0000a580: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const Binary& │ │ │ │ -0000a5b0: 206f 7029 203a 3c2f 6469 763e 0a3c 6469 op) :
    .< │ │ │ │ -0000a5d0: 6120 6964 3d22 6c30 3032 3331 2220 6e61 a id="l00231" na │ │ │ │ -0000a5e0: 6d65 3d22 6c30 3032 3331 223e 3c2f 613e me="l00231"> │ │ │ │ -0000a5f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 231 allSame_( │ │ │ │ -0000a620: 7472 7565 2920 7b3c 2f64 6976 3e0a 3c64 true) {
    . │ │ │ │ -0000a640: 3c61 2069 643d 226c 3030 3233 3222 206e 232 // │ │ │ │ -0000a6a0: 2043 686f 6f73 6520 7768 6174 2074 6f20 Choose what to │ │ │ │ -0000a6b0: 646f 2062 6173 6564 206f 6e20 6c61 6265 do based on labe │ │ │ │ -0000a6c0: 6c3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c l
    .< │ │ │ │ -0000a6d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000a6e0: 3e3c 6120 6964 3d22 6c30 3032 3333 2220 > 233 if (f. │ │ │ │ -0000a750: 3c61 2063 6c61 7373 3d22 636f 6465 2068 label() > │ │ │ │ -0000a7b0: 3b20 672e 3c61 2063 6c61 7373 3d22 636f ; g.label() │ │ │ │ -0000a810: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    234 │ │ │ │ -0000a870: 2020 2020 2020 3c73 7061 6e20 636c 6173 // f │ │ │ │ -0000a890: 2068 6967 6865 7220 7468 616e 2067 3c2f higher than g
    .
    < │ │ │ │ -0000a8e0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000a8f0: 6e6f 223e 2020 3233 353c 2f73 7061 6e3e no"> 235 │ │ │ │ -0000a900: 2020 2020 2020 2020 6c61 6265 6c5f 203d label_ = │ │ │ │ -0000a910: 2066 2e3c 6120 636c 6173 733d 2263 6f64 f.label(); │ │ │ │ -0000a970: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000a9c0: 2032 3336 3c2f 7370 616e 3e20 2020 2020 236 │ │ │ │ -0000a9d0: 2020 203c 7370 616e 2063 6c61 7373 3d22 siz │ │ │ │ -0000a9f0: 655f 743c 2f73 7061 6e3e 2063 6f75 6e74 e_t count │ │ │ │ -0000aa00: 203d 2066 2e6e 7243 686f 6963 6573 2829 = f.nrChoices() │ │ │ │ -0000aa10: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0000aa60: 2020 3233 373c 2f73 7061 6e3e 2020 2020 237 │ │ │ │ -0000aa70: 2020 2020 6272 616e 6368 6573 5f2e 7265 branches_.re │ │ │ │ -0000aa80: 7365 7276 6528 636f 756e 7429 3b3c 2f64 serve(count);.
    23 │ │ │ │ -0000aae0: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ -0000aaf0: 3c73 7061 6e20 636c 6173 733d 226b 6579 for ( │ │ │ │ -0000ab30: 7369 7a65 5f74 3c2f 7370 616e 3e20 6920 size_t i │ │ │ │ -0000ab40: 3d20 303b 2069 2026 6c74 3b20 636f 756e = 0; i < coun │ │ │ │ -0000ab50: 743b 2069 2b2b 293c 2f64 6976 3e0a 3c64 t; i++)
    . │ │ │ │ -0000ab70: 3c61 2069 643d 226c 3030 3233 3922 206e 239 push │ │ │ │ -0000abc0: 5f62 6163 6b28 662e 3c61 2063 6c61 7373 _back(f.branche │ │ │ │ -0000ac20: 735f 3c2f 613e 5b69 5d2d 2667 743b 6170 s_[i]->ap │ │ │ │ -0000ac30: 706c 795f 665f 6f70 5f67 2867 2c20 6f70 ply_f_op_g(g, op │ │ │ │ -0000ac40: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
    .
    240 │ │ │ │ -0000aca0: 2020 2020 7d20 3c73 7061 6e20 636c 6173 } │ │ │ │ -0000acc0: 656c 7365 3c2f 7370 616e 3e20 3c73 7061 else if │ │ │ │ -0000acf0: 2867 2e3c 6120 636c 6173 733d 2263 6f64 (g.label() │ │ │ │ -0000ad50: 2667 743b 2066 2e3c 6120 636c 6173 733d > f.label()) {
    .< │ │ │ │ -0000add0: 6120 6964 3d22 6c30 3032 3431 2220 6e61 a id="l00241" na │ │ │ │ -0000ade0: 6d65 3d22 6c30 3032 3431 223e 3c2f 613e me="l00241"> │ │ │ │ -0000adf0: 3c73 7061 6e20 636c 6173 733d 226c 696e 241 / │ │ │ │ -0000ae30: 2f20 6620 6c6f 7765 7220 7468 616e 2067 / f lower than g │ │ │ │ -0000ae40: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    ..
    242 label_ │ │ │ │ -0000af40: 3d20 672e 3c61 2063 6c61 7373 3d22 636f = g.label() │ │ │ │ -0000afa0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0000aff0: 2020 3234 333c 2f73 7061 6e3e 2020 2020 243 │ │ │ │ -0000b000: 2020 2020 3c73 7061 6e20 636c 6173 733d si │ │ │ │ -0000b020: 7a65 5f74 3c2f 7370 616e 3e20 636f 756e ze_t coun │ │ │ │ -0000b030: 7420 3d20 672e 6e72 4368 6f69 6365 7328 t = g.nrChoices( │ │ │ │ -0000b040: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    244 │ │ │ │ -0000b0a0: 2020 2020 2062 7261 6e63 6865 735f 2e72 branches_.r │ │ │ │ -0000b0b0: 6573 6572 7665 2863 6f75 6e74 293b 3c2f eserve(count);.
    2 │ │ │ │ -0000b110: 3435 3c2f 7370 616e 3e20 2020 2020 2020 45 │ │ │ │ -0000b120: 203c 7370 616e 2063 6c61 7373 3d22 6b65 for (size_t i │ │ │ │ -0000b170: 203d 2030 3b20 6920 266c 743b 2063 6f75 = 0; i < cou │ │ │ │ -0000b180: 6e74 3b20 692b 2b29 3c2f 6469 763e 0a3c nt; i++)
    .< │ │ │ │ -0000b190: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000b1a0: 3e3c 6120 6964 3d22 6c30 3032 3436 2220 > 246 pus │ │ │ │ -0000b1f0: 685f 6261 636b 2867 2e3c 6120 636c 6173 h_back(g.branch │ │ │ │ -0000b250: 6573 5f3c 2f61 3e5b 695d 2d26 6774 3b61 es_[i]->a │ │ │ │ -0000b260: 7070 6c79 5f67 5f6f 705f 6643 2866 2c20 pply_g_op_fC(f, │ │ │ │ -0000b270: 6f70 2929 3b3c 2f64 6976 3e0a 3c64 6976 op));
    .
    < │ │ │ │ -0000b2b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000b2c0: 6e6f 223e 2020 3234 373c 2f73 7061 6e3e no"> 247 │ │ │ │ -0000b2d0: 2020 2020 2020 7d20 3c73 7061 6e20 636c } else {< │ │ │ │ -0000b300: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0000b350: 3234 383c 2f73 7061 6e3e 2020 2020 2020 248 │ │ │ │ -0000b360: 2020 3c73 7061 6e20 636c 6173 733d 2263 // f sam │ │ │ │ -0000b380: 6520 6c65 7665 6c20 6173 2067 3c2f 7370 e level as g
    .
    249 │ │ │ │ -0000b3f0: 2020 2020 2020 6c61 6265 6c5f 203d 2066 label_ = f │ │ │ │ -0000b400: 2e3c 6120 636c 6173 733d 2263 6f64 6520 .label();.
    2 │ │ │ │ -0000b4b0: 3530 3c2f 7370 616e 3e20 2020 2020 2020 50 │ │ │ │ -0000b4c0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ -0000b4e0: 743c 2f73 7061 6e3e 2063 6f75 6e74 203d t count = │ │ │ │ -0000b4f0: 2066 2e6e 7243 686f 6963 6573 2829 3b3c f.nrChoices();< │ │ │ │ -0000b500: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0000b550: 3235 313c 2f73 7061 6e3e 2020 2020 2020 251 │ │ │ │ -0000b560: 2020 6272 616e 6368 6573 5f2e 7265 7365 branches_.rese │ │ │ │ -0000b570: 7276 6528 636f 756e 7429 3b3c 2f64 6976 rve(count);
    .
    252< │ │ │ │ -0000b5d0: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> for (si │ │ │ │ -0000b620: 7a65 5f74 3c2f 7370 616e 3e20 6920 3d20 ze_t i = │ │ │ │ -0000b630: 303b 2069 2026 6c74 3b20 636f 756e 743b 0; i < count; │ │ │ │ -0000b640: 2069 2b2b 293c 2f64 6976 3e0a 3c64 6976 i++)
    .
    < │ │ │ │ -0000b680: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000b690: 6e6f 223e 2020 3235 333c 2f73 7061 6e3e no"> 253 │ │ │ │ -0000b6a0: 2020 2020 2020 2020 2020 7075 7368 5f62 push_b │ │ │ │ -0000b6b0: 6163 6b28 662e 3c61 2063 6c61 7373 3d22 ack(f.branches_ │ │ │ │ -0000b710: 3c2f 613e 5b69 5d2d 2667 743b 6170 706c [i]->appl │ │ │ │ -0000b720: 795f 665f 6f70 5f67 282a 672e 3c61 2063 y_f_op_g(*g.bra │ │ │ │ -0000b780: 6e63 6865 735f 3c2f 613e 5b69 5d2c 206f nches_[i], o │ │ │ │ -0000b790: 7029 293b 3c2f 6469 763e 0a3c 6469 7620 p));
    .
    254 │ │ │ │ -0000b7f0: 2020 2020 207d 3c2f 6469 763e 0a3c 6469 }
    .< │ │ │ │ -0000b810: 6120 6964 3d22 6c30 3032 3535 2220 6e61 a id="l00255" na │ │ │ │ -0000b820: 6d65 3d22 6c30 3032 3535 223e 3c2f 613e me="l00255"> │ │ │ │ -0000b830: 3c73 7061 6e20 636c 6173 733d 226c 696e 255 }
    .< │ │ │ │ -0000b870: 6120 6964 3d22 6c30 3032 3536 2220 6e61 a id="l00256" na │ │ │ │ -0000b880: 6d65 3d22 6c30 3032 3536 223e 3c2f 613e me="l00256"> │ │ │ │ -0000b890: 3c73 7061 6e20 636c 6173 733d 226c 696e 256
    .
    .< │ │ │ │ -0000b900: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000b910: 3e3c 6120 6964 3d22 6c30 3032 3538 2220 > 25 │ │ │ │ -0000b990: 383c 2f61 3e3c 2f73 7061 6e3e 2020 2020 8 │ │ │ │ -0000b9a0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const L& label │ │ │ │ -0000ba20: 3c2f 613e 2829 3c73 7061 6e20 636c 6173 () con │ │ │ │ -0000ba40: 7374 203c 2f73 7061 6e3e 7b3c 2f64 6976 st {
    .
    259< │ │ │ │ -0000baa0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return label_;
    .
    │ │ │ │ -0000bb70: 2032 3630 3c2f 613e 3c2f 7370 616e 3e20 260 │ │ │ │ -0000bb80: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
    .
    .
    261< │ │ │ │ -0000bbe0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ -0000bc00: 3c61 2069 643d 226c 3030 3236 3222 206e 262 │ │ │ │ -0000bc60: 7369 7a65 5f74 3c2f 7370 616e 3e20 6e72 size_t nr │ │ │ │ -0000bc70: 4368 6f69 6365 7328 293c 7370 616e 2063 Choices() │ │ │ │ -0000bc90: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    .
    263 return branches_. │ │ │ │ -0000bdc0: 7369 7a65 2829 3b3c 2f64 6976 3e0a 3c64 size();
    . │ │ │ │ -0000bde0: 3c61 2069 643d 226c 3030 3236 3422 206e 264 }
    . │ │ │ │ -0000be40: 3c61 2069 643d 226c 3030 3236 3522 206e 265
    .
    266 │ │ │ │ -0000bee0: 2020 3c73 7061 6e20 636c 6173 733d 226b const std::vector │ │ │ │ -0000bf10: 266c 743b 4e6f 6465 5074 7226 6774 3b26 <NodePtr>& │ │ │ │ -0000bf20: 616d 703b 2062 7261 6e63 6865 7328 293c amp; branches()< │ │ │ │ -0000bf30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000bf40: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    267 │ │ │ │ -0000bfb0: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -0000bfd0: 6574 7572 6e3c 2f73 7061 6e3e 2062 7261 eturn bra │ │ │ │ -0000bfe0: 6e63 6865 735f 3b3c 2f64 6976 3e0a 3c64 nches_;
    . │ │ │ │ -0000c000: 3c61 2069 643d 226c 3030 3236 3822 206e 268 }
    . │ │ │ │ -0000c060: 3c61 2069 643d 226c 3030 3236 3922 206e 269
    .
    . │ │ │ │ -0000c0f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -0000c120: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 2 │ │ │ │ -0000c180: 3731 3c2f 613e 3c2f 7370 616e 3e20 2020 71 │ │ │ │ -0000c190: 203c 7370 616e 2063 6c61 7373 3d22 6b65 void< │ │ │ │ -0000c1b0: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> push_bac │ │ │ │ -0000c210: 6b3c 2f61 3e28 3c73 7061 6e20 636c 6173 k(cons │ │ │ │ -0000c230: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t NodePtr │ │ │ │ -0000c290: 3c2f 613e 2661 6d70 3b20 6e6f 6465 2920 & node) │ │ │ │ -0000c2a0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -0000c2f0: 2020 3237 323c 2f73 7061 6e3e 2020 2020 272 │ │ │ │ -0000c300: 2020 3c73 7061 6e20 636c 6173 733d 2263 // allSa │ │ │ │ -0000c320: 6d65 5f20 6973 2072 6573 7472 6963 7465 me_ is restricte │ │ │ │ -0000c330: 6420 746f 206c 6561 6620 6e6f 6465 7320 d to leaf nodes │ │ │ │ -0000c340: 696e 2061 2064 6563 6973 696f 6e20 7472 in a decision tr │ │ │ │ -0000c350: 6565 3c2f 7370 616e 3e3c 2f64 6976 3e0a ee
    . │ │ │ │ -0000c360: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .< │ │ │ │ -0000c410: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000c420: 3e3c 6120 6964 3d22 6c30 3032 3734 2220 > 274 allSa │ │ │ │ -0000c470: 6d65 5f20 3d20 6e6f 6465 2d26 6774 3b73 me_ = node->s │ │ │ │ -0000c480: 616d 654c 6561 6628 2a62 7261 6e63 6865 ameLeaf(*branche │ │ │ │ -0000c490: 735f 2e62 6163 6b28 2929 3b3c 2f64 6976 s_.back());
    .
    275< │ │ │ │ -0000c4f0: 2f73 7061 6e3e 2020 2020 2020 7d3c 2f64 /span> }.
    27 │ │ │ │ -0000c550: 363c 2f73 7061 6e3e 2020 2020 2020 6272 6 br │ │ │ │ -0000c560: 616e 6368 6573 5f2e 7075 7368 5f62 6163 anches_.push_bac │ │ │ │ -0000c570: 6b28 6e6f 6465 293b 3c2f 6469 763e 0a3c k(node);
    .< │ │ │ │ -0000c580: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000c590: 3e3c 6120 6964 3d22 6c30 3032 3737 2220 > 277 }
    .< │ │ │ │ -0000c5e0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0000c630: 3237 383c 2f73 7061 6e3e 203c 2f64 6976 278
    .
    .
    │ │ │ │ -0000c6d0: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 280 void < │ │ │ │ -0000c750: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0000c760: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ -0000c770: 2261 3032 3738 302e 6874 6d6c 2361 3437 "a02780.html#a47 │ │ │ │ -0000c780: 6366 3766 3361 6563 6538 6335 3365 6165 cf7f3aece8c53eae │ │ │ │ -0000c790: 6531 3039 3664 3665 3565 3062 3034 223e e1096d6e5e0b04"> │ │ │ │ -0000c7a0: 7072 696e 743c 2f61 3e28 3c73 7061 6e20 print( │ │ │ │ -0000c7c0: 636f 6e73 743c 2f73 7061 6e3e 2073 7464 const std │ │ │ │ -0000c7d0: 3a3a 7374 7269 6e67 2661 6d70 3b20 732c ::string& s, │ │ │ │ -0000c7e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const LabelFormatt │ │ │ │ -0000c810: 6572 2661 6d70 3b20 6c61 6265 6c46 6f72 er& labelFor │ │ │ │ -0000c820: 6d61 7474 6572 2c3c 2f64 6976 3e0a 3c64 matter,
    . │ │ │ │ -0000c840: 3c61 2069 643d 226c 3030 3238 3122 206e 281 │ │ │ │ -0000c890: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const ValueFormatt │ │ │ │ -0000c8c0: 6572 2661 6d70 3b20 7661 6c75 6546 6f72 er& valueFor │ │ │ │ -0000c8d0: 6d61 7474 6572 293c 7370 616e 2063 6c61 matter) co │ │ │ │ -0000c8f0: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override
    {
    .
    < │ │ │ │ -0000c940: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000c950: 6e6f 223e 2020 3238 323c 2f73 7061 6e3e no"> 282 │ │ │ │ -0000c960: 2020 2020 2020 7374 643a 3a63 6f75 7420 std::cout │ │ │ │ -0000c970: 266c 743b 266c 743b 2073 2026 6c74 3b26 << s <& │ │ │ │ -0000c980: 6c74 3b20 3c73 7061 6e20 636c 6173 733d lt; │ │ │ │ -0000c9a0: 2671 756f 743b 2043 686f 6963 6528 2671 " Choice(&q │ │ │ │ -0000c9b0: 756f 743b 3c2f 7370 616e 3e3b 3c2f 6469 uot;;.
    283 │ │ │ │ -0000ca10: 3c2f 7370 616e 3e20 2020 2020 2073 7464 std │ │ │ │ -0000ca20: 3a3a 636f 7574 2026 6c74 3b26 6c74 3b20 ::cout << │ │ │ │ -0000ca30: 6c61 6265 6c46 6f72 6d61 7474 6572 286c labelFormatter(l │ │ │ │ -0000ca40: 6162 656c 5f29 2026 6c74 3b26 6c74 3b20 abel_) << │ │ │ │ -0000ca50: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ -0000ca70: 743b 2920 2671 756f 743b 3c2f 7370 616e t;) " << std:: │ │ │ │ -0000ca90: 656e 646c 3b3c 2f64 6976 3e0a 3c64 6976 endl;
    .
    < │ │ │ │ -0000cad0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000cae0: 6e6f 223e 2020 3238 343c 2f73 7061 6e3e no"> 284 │ │ │ │ -0000caf0: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ -0000cb10: 666f 723c 2f73 7061 6e3e 2028 3c73 7061 for (size_t i = 0; i < │ │ │ │ -0000cb50: 3b20 6272 616e 6368 6573 5f2e 7369 7a65 ; branches_.size │ │ │ │ -0000cb60: 2829 3b20 692b 2b29 3c2f 6469 763e 0a3c (); i++)
    .< │ │ │ │ -0000cb70: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000cb80: 3e3c 6120 6964 3d22 6c30 3032 3835 2220 > 285 branc │ │ │ │ -0000cbd0: 6865 735f 5b69 5d2d 2667 743b 3c61 2063 hes_[i]->pri │ │ │ │ -0000cc30: 6e74 3c2f 613e 2828 626f 6f73 743a 3a66 nt((boost::f │ │ │ │ -0000cc40: 6f72 6d61 7428 3c73 7061 6e20 636c 6173 ormat("%s %d&qu │ │ │ │ -0000cc70: 6f74 3b3c 2f73 7061 6e3e 2920 2520 7320 ot;) % s │ │ │ │ -0000cc80: 2520 6929 2e73 7472 2829 2c3c 2f64 6976 % i).str(),
    .
    286< │ │ │ │ -0000cce0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -0000ccf0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0000cd00: 2020 6c61 6265 6c46 6f72 6d61 7474 6572 labelFormatter │ │ │ │ -0000cd10: 2c20 7661 6c75 6546 6f72 6d61 7474 6572 , valueFormatter │ │ │ │ -0000cd20: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    287 │ │ │ │ -0000cd80: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ -0000cd90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    .
    │ │ │ │ -0000cec0: 2032 3930 3c2f 613e 3c2f 7370 616e 3e20 290 │ │ │ │ -0000ced0: 2020 203c 7370 616e 2063 6c61 7373 3d22 voi │ │ │ │ -0000cef0: 643c 2f73 7061 6e3e 203c 6120 636c 6173 d dot(std::ostream&a │ │ │ │ -0000cf60: 6d70 3b20 6f73 2c20 3c73 7061 6e20 636c mp; os, co │ │ │ │ -0000cf80: 6e73 743c 2f73 7061 6e3e 204c 6162 656c nst Label │ │ │ │ -0000cf90: 466f 726d 6174 7465 7226 616d 703b 206c Formatter& l │ │ │ │ -0000cfa0: 6162 656c 466f 726d 6174 7465 722c 3c2f abelFormatter,.
    2 │ │ │ │ -0000d000: 3931 3c2f 7370 616e 3e20 2020 2020 2020 91 │ │ │ │ -0000d010: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ -0000d030: 743c 2f73 7061 6e3e 2056 616c 7565 466f t ValueFo │ │ │ │ -0000d040: 726d 6174 7465 7226 616d 703b 2076 616c rmatter& val │ │ │ │ -0000d050: 7565 466f 726d 6174 7465 722c 3c2f 6469 ueFormatter,.
    292 │ │ │ │ -0000d0b0: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ -0000d0c0: 2020 2020 3c73 7061 6e20 636c 6173 733d bo │ │ │ │ -0000d0e0: 6f6c 3c2f 7370 616e 3e20 7368 6f77 5a65 ol showZe │ │ │ │ -0000d0f0: 726f 293c 7370 616e 2063 6c61 7373 3d22 ro) const │ │ │ │ -0000d110: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ -0000d120: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -0000d170: 2020 3239 333c 2f73 7061 6e3e 2020 2020 293 │ │ │ │ -0000d180: 2020 6f73 2026 6c74 3b26 6c74 3b20 3c73 os << " │ │ │ │ -0000d1b0: 5c26 7175 6f74 3b26 7175 6f74 3b3c 2f73 \"" << th │ │ │ │ -0000d1d0: 6973 2d26 6774 3b69 6428 2920 266c 743b is->id() < │ │ │ │ -0000d1e0: 266c 743b 203c 7370 616e 2063 6c61 7373 < "\" [ │ │ │ │ -0000d210: 7368 6170 653d 6369 7263 6c65 2c20 6c61 shape=circle, la │ │ │ │ -0000d220: 6265 6c3d 5c26 7175 6f74 3b26 7175 6f74 bel=\"" │ │ │ │ -0000d230: 3b3c 2f73 7061 6e3e 2026 6c74 3b26 6c74 ; << │ │ │ │ -0000d240: 3b20 6c61 6265 6c5f 3c2f 6469 763e 0a3c ; label_
    .< │ │ │ │ -0000d250: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000d260: 3e3c 6120 6964 3d22 6c30 3032 3934 2220 > 294 < │ │ │ │ -0000d2b0: 3b26 6c74 3b20 3c73 7061 6e20 636c 6173 ;< "\"] │ │ │ │ -0000d2e0: 5c6e 2671 756f 743b 3c2f 7370 616e 3e3b \n"; │ │ │ │ -0000d2f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000d340: 2032 3935 3c2f 7370 616e 3e20 2020 2020 295 │ │ │ │ -0000d350: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ -0000d370: 743c 2f73 7061 6e3e 2042 203d 2062 7261 t B = bra │ │ │ │ -0000d380: 6e63 6865 735f 2e73 697a 6528 293b 3c2f nches_.size();.
    2 │ │ │ │ -0000d3e0: 3936 3c2f 7370 616e 3e20 2020 2020 203c 96 < │ │ │ │ -0000d3f0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000d400: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (s │ │ │ │ -0000d430: 697a 655f 743c 2f73 7061 6e3e 2069 203d ize_t i = │ │ │ │ -0000d440: 2030 3b20 6920 266c 743b 2042 3b20 692b 0; i < B; i+ │ │ │ │ -0000d450: 2b29 207b 3c2f 6469 763e 0a3c 6469 7620 +) {
    .
    │ │ │ │ -0000d4a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0000d4d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ -0000d530: 3239 373c 2f61 3e3c 2f73 7061 6e3e 2020 297 │ │ │ │ -0000d540: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ -0000d560: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t NodePtr │ │ │ │ -0000d5c0: 3c2f 613e 2661 6d70 3b20 6272 616e 6368 & branch │ │ │ │ -0000d5d0: 203d 2062 7261 6e63 6865 735f 5b69 5d3b = branches_[i]; │ │ │ │ -0000d5e0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000d630: 2032 3938 3c2f 7370 616e 3e20 3c2f 6469 298 .
    299 │ │ │ │ -0000d690: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ -0000d6a0: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -0000d6b0: 656e 7422 3e2f 2f20 4368 6563 6b20 6966 ent">// Check if │ │ │ │ -0000d6c0: 207a 6572 6f3c 2f73 7061 6e3e 3c2f 6469 zero.
    300 │ │ │ │ -0000d720: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ -0000d730: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000d740: 6f72 6466 6c6f 7722 3e69 663c 2f73 7061 ordflow">if (!showZero) { │ │ │ │ -0000d760: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000d7b0: 2033 3031 3c2f 7370 616e 3e20 2020 2020 301 │ │ │ │ -0000d7c0: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ -0000d7e0: 3c2f 7370 616e 3e20 4c65 6166 2a20 6c65 Leaf* le │ │ │ │ -0000d7f0: 6166 203d 203c 7370 616e 2063 6c61 7373 af = dynam │ │ │ │ -0000d810: 6963 5f63 6173 7426 6c74 3b3c 2f73 7061 ic_cast<const Leaf* │ │ │ │ -0000d860: 2667 743b 3c2f 7370 616e 3e28 6272 616e >(bran │ │ │ │ -0000d870: 6368 2e67 6574 2829 293b 3c2f 6469 763e ch.get());
    │ │ │ │ -0000d880: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0000d8b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 302 < │ │ │ │ -0000d8e0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000d8f0: 6f72 6466 6c6f 7722 3e69 663c 2f73 7061 ordflow">if (leaf &&a │ │ │ │ -0000d910: 6d70 3b20 7661 6c75 6546 6f72 6d61 7474 mp; valueFormatt │ │ │ │ -0000d920: 6572 286c 6561 662d 2667 743b 636f 6e73 er(leaf->cons │ │ │ │ -0000d930: 7461 6e74 2829 292e 636f 6d70 6172 6528 tant()).compare( │ │ │ │ -0000d940: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ -0000d960: 743b 3026 7175 6f74 3b3c 2f73 7061 6e3e t;0" │ │ │ │ -0000d970: 2929 203c 7370 616e 2063 6c61 7373 3d22 )) con │ │ │ │ -0000d990: 7469 6e75 653c 2f73 7061 6e3e 3b3c 2f64 tinue;.
    30 │ │ │ │ -0000d9f0: 333c 2f73 7061 6e3e 2020 2020 2020 2020 3 │ │ │ │ -0000da00: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -0000da50: 2020 3330 343c 2f73 7061 6e3e 203c 2f64 304 .
    30 │ │ │ │ -0000dab0: 353c 2f73 7061 6e3e 2020 2020 2020 2020 5 │ │ │ │ -0000dac0: 6f73 2026 6c74 3b26 6c74 3b20 3c73 7061 os << "\& │ │ │ │ -0000daf0: 7175 6f74 3b26 7175 6f74 3b3c 2f73 7061 quot;" << this │ │ │ │ -0000db10: 2d26 6774 3b69 6428 2920 266c 743b 266c ->id() <&l │ │ │ │ -0000db20: 743b 203c 7370 616e 2063 6c61 7373 3d22 t; & │ │ │ │ -0000db40: 7175 6f74 3b5c 2671 756f 743b 202d 2667 quot;\" -&g │ │ │ │ -0000db50: 743b 205c 2671 756f 743b 2671 756f 743b t; \"" │ │ │ │ -0000db60: 3c2f 7370 616e 3e20 266c 743b 266c 743b << │ │ │ │ -0000db70: 2062 7261 6e63 682d 2667 743b 6964 2829 branch->id() │ │ │ │ -0000db80: 2026 6c74 3b26 6c74 3b20 3c73 7061 6e20 << "\&qu │ │ │ │ -0000dbb0: 6f74 3b26 7175 6f74 3b3c 2f73 7061 6e3e ot;" │ │ │ │ -0000dbc0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0000dc10: 2020 3330 363c 2f73 7061 6e3e 2020 2020 306 │ │ │ │ -0000dc20: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ -0000dc40: 3c2f 7370 616e 3e20 2842 203d 3d20 3220 (B == 2 │ │ │ │ -0000dc50: 2661 6d70 3b26 616d 703b 2069 203d 3d20 && i == │ │ │ │ -0000dc60: 3029 206f 7320 266c 743b 266c 743b 203c 0) os << < │ │ │ │ -0000dc70: 7370 616e 2063 6c61 7373 3d22 7374 7269 span class="stri │ │ │ │ -0000dc80: 6e67 6c69 7465 7261 6c22 3e26 7175 6f74 ngliteral">" │ │ │ │ -0000dc90: 3b20 5b73 7479 6c65 3d64 6173 6865 645d ; [style=dashed] │ │ │ │ -0000dca0: 2671 756f 743b 3c2f 7370 616e 3e3b 3c2f ";.
    3 │ │ │ │ -0000dd00: 3037 3c2f 7370 616e 3e20 2020 2020 2020 07 │ │ │ │ -0000dd10: 206f 7320 266c 743b 266c 743b 2073 7464 os << std │ │ │ │ -0000dd20: 3a3a 656e 646c 3b3c 2f64 6976 3e0a 3c64 ::endl;
    . │ │ │ │ -0000dd40: 3c61 2069 643d 226c 3030 3330 3822 206e 308 branch │ │ │ │ -0000dd90: 2d26 6774 3b64 6f74 286f 732c 206c 6162 ->dot(os, lab │ │ │ │ -0000dda0: 656c 466f 726d 6174 7465 722c 2076 616c elFormatter, val │ │ │ │ -0000ddb0: 7565 466f 726d 6174 7465 722c 2073 686f ueFormatter, sho │ │ │ │ -0000ddc0: 775a 6572 6f29 3b3c 2f64 6976 3e0a 3c64 wZero);
    . │ │ │ │ -0000dde0: 3c61 2069 643d 226c 3030 3330 3922 206e 309 }
    . │ │ │ │ -0000de30: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ -0000de90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    .
    │ │ │ │ -0000dfc0: 2033 3133 3c2f 613e 3c2f 7370 616e 3e20 313 │ │ │ │ -0000dfd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 boo │ │ │ │ -0000dff0: 6c3c 2f73 7061 6e3e 203c 6120 636c 6173 l sameLe │ │ │ │ -0000e050: 6166 3c2f 613e 283c 7370 616e 2063 6c61 af(con │ │ │ │ -0000e070: 7374 3c2f 7370 616e 3e20 3c61 2063 6c61 st Leaf& │ │ │ │ -0000e0b0: 616d 703b 2071 293c 7370 616e 2063 6c61 amp; q) co │ │ │ │ -0000e0d0: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override {
    .
    .
    │ │ │ │ -0000e1c0: 2033 3134 3c2f 613e 3c2f 7370 616e 3e20 314 │ │ │ │ -0000e1d0: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -0000e1f0: 6574 7572 6e3c 2f73 7061 6e3e 203c 7370 eturn false; │ │ │ │ -0000e220: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000e270: 2033 3135 3c2f 7370 616e 3e20 2020 207d 315 } │ │ │ │ -0000e280: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0000e2d0: 2033 3136 3c2f 7370 616e 3e20 3c2f 6469 316 .
    .
    318< │ │ │ │ -0000e3c0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> bool │ │ │ │ -0000e3f0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 sameLeaf(const │ │ │ │ -0000e470: 203c 6120 636c 6173 733d 2263 6f64 6520 No │ │ │ │ -0000e4a0: 6465 3c2f 613e 2661 6d70 3b20 7129 3c73 de& q) const overr │ │ │ │ -0000e4d0: 6964 6520 3c2f 7370 616e 3e7b 3c2f 6469 ide {.
    319 │ │ │ │ -0000e530: 3c2f 7370 616e 3e20 2020 2020 203c 7370 return (q.isLeaf() │ │ │ │ -0000e570: 2026 616d 703b 2661 6d70 3b20 712e 7361 && q.sa │ │ │ │ -0000e580: 6d65 4c65 6166 282a 3c73 7061 6e20 636c meLeaf(*th │ │ │ │ -0000e5a0: 6973 3c2f 7370 616e 3e29 293b 3c2f 6469 is));.
    320 │ │ │ │ -0000e600: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
    .
    321 < │ │ │ │ -0000e670: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ -0000e6f0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000e700: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 323 bool equals(< │ │ │ │ -0000e7e0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000e7f0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const N │ │ │ │ -0000e830: 6f64 653c 2f61 3e26 616d 703b 2071 2c20 ode& q, │ │ │ │ -0000e840: 3c73 7061 6e20 636c 6173 733d 226b 6579 const CompareFunc&a │ │ │ │ -0000e870: 6d70 3b20 636f 6d70 6172 6529 3c73 7061 mp; compare) const overrid │ │ │ │ -0000e8a0: 6520 3c2f 7370 616e 3e7b 3c2f 6469 763e e {
    │ │ │ │ -0000e8b0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0000e8e0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 324 const Choic │ │ │ │ -0000e960: 653c 2f61 3e2a 206f 7468 6572 203d 203c e* other = < │ │ │ │ -0000e970: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000e980: 6f72 6422 3e64 796e 616d 6963 5f63 6173 ord">dynamic_cas │ │ │ │ -0000e990: 7426 6c74 3b3c 2f73 7061 6e3e 3c73 7061 t<const < │ │ │ │ -0000e9c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0000e9d0: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -0000e9e0: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ -0000e9f0: 6365 3c2f 613e 2a3c 7370 616e 2063 6c61 ce*> │ │ │ │ -0000ea10: 3b3c 2f73 7061 6e3e 2826 616d 703b 7129 ;(&q) │ │ │ │ -0000ea20: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0000ea70: 2020 3332 353c 2f73 7061 6e3e 2020 2020 325 │ │ │ │ -0000ea80: 2020 3c73 7061 6e20 636c 6173 733d 226b if (!other) < │ │ │ │ -0000eab0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0000eac0: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ -0000ead0: 2f73 7061 6e3e 203c 7370 616e 2063 6c61 /span> fal │ │ │ │ -0000eaf0: 7365 3c2f 7370 616e 3e3b 3c2f 6469 763e se;
    │ │ │ │ -0000eb00: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0000eb30: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 326 if ( │ │ │ │ -0000eb80: 7468 6973 2d26 6774 3b6c 6162 656c 5f20 this->label_ │ │ │ │ -0000eb90: 213d 206f 7468 6572 2d26 6774 3b3c 6120 != other->la │ │ │ │ -0000ebf0: 6265 6c5f 3c2f 613e 2920 3c73 7061 6e20 bel_) return false;
    .
    < │ │ │ │ -0000ec80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000ec90: 6e6f 223e 2020 3332 373c 2f73 7061 6e3e no"> 327 │ │ │ │ -0000eca0: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ -0000ecc0: 6966 3c2f 7370 616e 3e20 2862 7261 6e63 if (branc │ │ │ │ -0000ecd0: 6865 735f 2e73 697a 6528 2920 213d 206f hes_.size() != o │ │ │ │ -0000ece0: 7468 6572 2d26 6774 3b3c 6120 636c 6173 ther->branch │ │ │ │ -0000ed40: 6573 5f3c 2f61 3e2e 7369 7a65 2829 2920 es_.size()) │ │ │ │ -0000ed50: 3c73 7061 6e20 636c 6173 733d 226b 6579 return │ │ │ │ -0000ed70: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c fa │ │ │ │ -0000ed90: 6c73 653c 2f73 7061 6e3e 3b3c 2f64 6976 lse;
    .
    328< │ │ │ │ -0000edf0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> // we don' │ │ │ │ -0000ee20: 7420 6361 7265 2061 626f 7574 2073 6861 t care about sha │ │ │ │ -0000ee30: 7265 6420 706f 696e 7465 7273 2062 6569 red pointers bei │ │ │ │ -0000ee40: 6e67 2065 7175 616c 2068 6572 653c 2f73 ng equal here
    .
    329 │ │ │ │ -0000eeb0: 2020 2020 203c 7370 616e 2063 6c61 7373 f │ │ │ │ -0000eed0: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (size_t i = 0; i < │ │ │ │ -0000ef10: 2062 7261 6e63 6865 735f 2e73 697a 6528 branches_.size( │ │ │ │ -0000ef20: 293b 2069 2b2b 293c 2f64 6976 3e0a 3c64 ); i++)
    . │ │ │ │ -0000ef40: 3c61 2069 643d 226c 3030 3333 3022 206e 330 if (! │ │ │ │ -0000efb0: 2862 7261 6e63 6865 735f 5b69 5d2d 2667 (branches_[i]-&g │ │ │ │ -0000efc0: 743b 3c61 2063 6c61 7373 3d22 636f 6465 t;e │ │ │ │ -0000eff0: 7175 616c 733c 2f61 3e28 2a28 6f74 6865 quals(*(othe │ │ │ │ -0000f000: 722d 2667 743b 3c61 2063 6c61 7373 3d22 r->branches_ │ │ │ │ -0000f060: 3c2f 613e 5b69 5d29 2c20 636f 6d70 6172 [i]), compar │ │ │ │ -0000f070: 6529 2929 3c2f 6469 763e 0a3c 6469 7620 e)))
    .
    331 │ │ │ │ -0000f0d0: 2020 2020 2020 2020 203c 7370 616e 2063 return │ │ │ │ -0000f100: 203c 7370 616e 2063 6c61 7373 3d22 6b65 false;
    .
    332 │ │ │ │ -0000f180: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -0000f1a0: 6574 7572 6e3c 2f73 7061 6e3e 203c 7370 eturn true;< │ │ │ │ -0000f1d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0000f220: 3333 333c 2f73 7061 6e3e 2020 2020 7d3c 333 }< │ │ │ │ -0000f230: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ -0000f250: 6120 6964 3d22 6c30 3033 3334 2220 6e61 a id="l00334" na │ │ │ │ -0000f260: 6d65 3d22 6c30 3033 3334 223e 3c2f 613e me="l00334"> │ │ │ │ -0000f270: 3c73 7061 6e20 636c 6173 733d 226c 696e 334
    .
    .< │ │ │ │ -0000f2e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000f2f0: 3e3c 6120 6964 3d22 6c30 3033 3336 2220 > 33 │ │ │ │ -0000f370: 363c 2f61 3e3c 2f73 7061 6e3e 2020 2020 6 │ │ │ │ -0000f380: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Y& opera │ │ │ │ -0000f400: 746f 7228 293c 2f61 3e28 3c73 7061 6e20 tor()( │ │ │ │ -0000f420: 636f 6e73 743c 2f73 7061 6e3e 203c 6120 const Assignm │ │ │ │ -0000f460: 656e 7426 6c74 3b4c 2667 743b 3c2f 613e ent<L> │ │ │ │ -0000f470: 2661 6d70 3b20 7829 3c73 7061 6e20 636c & x) c │ │ │ │ -0000f490: 6f6e 7374 206f 7665 7272 6964 6520 3c2f onst override {
    .< │ │ │ │ -0000f4c0: 6120 6964 3d22 6c30 3033 3337 2220 6e61 a id="l00337" na │ │ │ │ -0000f4d0: 6d65 3d22 6c30 3033 3337 223e 3c2f 613e me="l00337"> │ │ │ │ -0000f4e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 337#ifn │ │ │ │ -0000f520: 6465 6620 4e44 4542 5547 3c2f 7370 616e def NDEBUG
    .
    . │ │ │ │ -0000f590: 3c61 2069 643d 226c 3030 3333 3822 206e 338 │ │ │ │ -0000f610: 3c2f 613e 3c2f 7370 616e 3e20 2020 2020 │ │ │ │ -0000f620: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ -0000f640: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> Assignment< │ │ │ │ -0000f680: 3b4c 2667 743b 3a3a 636f 6e73 745f 6974 ;L>::const_it │ │ │ │ -0000f690: 6572 6174 6f72 3c2f 613e 2069 7420 3d20 erator it = │ │ │ │ -0000f6a0: 782e 6669 6e64 286c 6162 656c 5f29 3b3c x.find(label_);< │ │ │ │ -0000f6b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0000f700: 3333 393c 2f73 7061 6e3e 2020 2020 2020 339 │ │ │ │ -0000f710: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (it == x.end │ │ │ │ -0000f740: 2829 2920 7b3c 2f64 6976 3e0a 3c64 6976 ()) {
    .
    < │ │ │ │ -0000f780: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000f790: 6e6f 223e 2020 3334 303c 2f73 7061 6e3e no"> 340 │ │ │ │ -0000f7a0: 2020 2020 2020 2020 7374 643a 3a63 6f75 std::cou │ │ │ │ -0000f7b0: 7420 266c 743b 266c 743b 203c 7370 616e t << "Try │ │ │ │ -0000f7e0: 696e 6720 746f 2066 696e 6420 7661 6c75 ing to find valu │ │ │ │ -0000f7f0: 6520 666f 7220 2671 756f 743b 3c2f 7370 e for " << lab │ │ │ │ -0000f810: 656c 5f20 266c 743b 266c 743b 2073 7464 el_ << std │ │ │ │ -0000f820: 3a3a 656e 646c 3b3c 2f64 6976 3e0a 3c64 ::endl;
    ..
    341 throw │ │ │ │ -0000f940: 7374 643a 3a69 6e76 616c 6964 5f61 7267 std::invalid_arg │ │ │ │ -0000f950: 756d 656e 7428 3c2f 6469 763e 0a3c 6469 ument(
    .< │ │ │ │ -0000f970: 6120 6964 3d22 6c30 3033 3432 2220 6e61 a id="l00342" na │ │ │ │ -0000f980: 6d65 3d22 6c30 3033 3432 223e 3c2f 613e me="l00342"> │ │ │ │ -0000f990: 3c73 7061 6e20 636c 6173 733d 226c 696e 342 "D │ │ │ │ -0000f9e0: 6563 6973 696f 6e54 7265 653a 3a6f 7065 ecisionTree::ope │ │ │ │ -0000f9f0: 7261 746f 7228 293a 2076 616c 7565 2075 rator(): value u │ │ │ │ -0000fa00: 6e64 6566 696e 6564 2066 6f72 2061 206c ndefined for a l │ │ │ │ -0000fa10: 6162 656c 2671 756f 743b 3c2f 7370 616e abel"
    );
    .
    343 │ │ │ │ -0000fa80: 2020 2020 7d3c 2f64 6976 3e0a 3c64 6976 }
    .
    < │ │ │ │ -0000fac0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0000fad0: 6e6f 223e 2020 3334 343c 2f73 7061 6e3e no"> 344 │ │ │ │ -0000fae0: 3c73 7061 6e20 636c 6173 733d 2270 7265 #endi │ │ │ │ -0000fb00: 663c 2f73 7061 6e3e 3c2f 6469 763e 0a3c f
    .< │ │ │ │ -0000fb10: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0000fb20: 3e3c 6120 6964 3d22 6c30 3033 3435 2220 > 345 size_t │ │ │ │ -0000fb90: 2069 6e64 6578 203d 2078 2e61 7428 6c61 index = x.at(la │ │ │ │ -0000fba0: 6265 6c5f 293b 3c2f 6469 763e 0a3c 6469 bel_);
    .< │ │ │ │ -0000fbc0: 6120 6964 3d22 6c30 3033 3436 2220 6e61 a id="l00346" na │ │ │ │ -0000fbd0: 6d65 3d22 6c30 3033 3436 223e 3c2f 613e me="l00346"> │ │ │ │ -0000fbe0: 3c73 7061 6e20 636c 6173 733d 226c 696e 346 NodePtr child = branc │ │ │ │ -0000fc70: 6865 735f 5b69 6e64 6578 5d3b 3c2f 6469 hes_[index];.
    347 │ │ │ │ -0000fcd0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 return (*child)(x) │ │ │ │ -0000fd10: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0000fd60: 2020 3334 383c 2f73 7061 6e3e 2020 2020 348 │ │ │ │ -0000fd70: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -0000fdc0: 2020 3334 393c 2f73 7061 6e3e 203c 2f64 349 .
    .
    351 │ │ │ │ -0000feb0: 3c2f 7370 616e 3e20 2020 2043 686f 6963 Choic │ │ │ │ -0000fec0: 6528 3c73 7061 6e20 636c 6173 733d 226b e(const L& labe │ │ │ │ -0000fef0: 6c2c 203c 7370 616e 2063 6c61 7373 3d22 l, const Choice& │ │ │ │ -0000ff20: 3b20 662c 203c 7370 616e 2063 6c61 7373 ; f, const │ │ │ │ -0000ff40: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Unary& op) :.
    352 │ │ │ │ -00010000: 3c2f 7370 616e 3e20 2020 2020 206c 6162 lab │ │ │ │ -00010010: 656c 5f28 6c61 6265 6c29 2c20 616c 6c53 el_(label), allS │ │ │ │ -00010020: 616d 655f 2874 7275 6529 207b 3c2f 6469 ame_(true) {.
    353 │ │ │ │ -00010080: 3c2f 7370 616e 3e20 2020 2020 2062 7261 bra │ │ │ │ -00010090: 6e63 6865 735f 2e72 6573 6572 7665 2866 nches_.reserve(f │ │ │ │ -000100a0: 2e62 7261 6e63 6865 735f 2e73 697a 6528 .branches_.size( │ │ │ │ -000100b0: 2929 3b20 203c 7370 616e 2063 6c61 7373 )); // re │ │ │ │ -000100d0: 7365 7276 6520 7370 6163 653c 2f73 7061 serve space
    .
    .< │ │ │ │ -00010130: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00010140: 3e3c 6120 6964 3d22 6c30 3033 3534 2220 > 35 │ │ │ │ -000101c0: 343c 2f61 3e3c 2f73 7061 6e3e 2020 2020 4 │ │ │ │ -000101d0: 2020 3c73 7061 6e20 636c 6173 733d 226b for< │ │ │ │ -000101f0: 2f73 7061 6e3e 2028 3c73 7061 6e20 636c /span> (co │ │ │ │ -00010210: 6e73 743c 2f73 7061 6e3e 203c 6120 636c nst NodeP │ │ │ │ -00010270: 7472 3c2f 613e 2661 6d70 3b20 6272 616e tr& bran │ │ │ │ -00010280: 6368 203a 2066 2e62 7261 6e63 6865 735f ch : f.branches_ │ │ │ │ -00010290: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    355 │ │ │ │ -000102f0: 2020 2020 2020 7075 7368 5f62 6163 6b28 push_back( │ │ │ │ -00010300: 6272 616e 6368 2d26 6774 3b61 7070 6c79 branch->apply │ │ │ │ -00010310: 286f 7029 293b 3c2f 6469 763e 0a3c 6469 (op));
    .< │ │ │ │ -00010330: 6120 6964 3d22 6c30 3033 3536 2220 6e61 a id="l00356" na │ │ │ │ -00010340: 6d65 3d22 6c30 3033 3536 223e 3c2f 613e me="l00356"> │ │ │ │ -00010350: 3c73 7061 6e20 636c 6173 733d 226c 696e 356 }
    .< │ │ │ │ -00010380: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00010390: 3e3c 6120 6964 3d22 6c30 3033 3537 2220 > 357 }
    .< │ │ │ │ -000103e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000103f0: 3e3c 6120 6964 3d22 6c30 3033 3538 2220 > 358
    .
    │ │ │ │ -00010480: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000104b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ -00010510: 3336 393c 2f61 3e3c 2f73 7061 6e3e 2020 369 │ │ │ │ -00010520: 2020 3c61 2063 6c61 7373 3d22 636f 6465 Choice(const │ │ │ │ -000105a0: 204c 2661 6d70 3b20 6c61 6265 6c2c 203c L& label, < │ │ │ │ -000105b0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000105c0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const C │ │ │ │ -00010600: 686f 6963 653c 2f61 3e26 616d 703b 2066 hoice& f │ │ │ │ -00010610: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const UnaryAssign │ │ │ │ -00010640: 6d65 6e74 2661 6d70 3b20 6f70 2c3c 2f64 ment& op,.
    37 │ │ │ │ -000106a0: 303c 2f73 7061 6e3e 2020 2020 2020 2020 0 │ │ │ │ -000106b0: 2020 203c 7370 616e 2063 6c61 7373 3d22 const Assignment< │ │ │ │ -00010710: 4c26 6774 3b3c 2f61 3e26 616d 703b 2061 L>& a │ │ │ │ -00010720: 7373 6967 6e6d 656e 7429 3c2f 6469 763e ssignment)
    │ │ │ │ -00010730: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00010760: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 371 : l │ │ │ │ -00010790: 6162 656c 5f28 6c61 6265 6c29 2c20 616c abel_(label), al │ │ │ │ -000107a0: 6c53 616d 655f 2874 7275 6529 207b 3c2f lSame_(true) {.
    3 │ │ │ │ -00010800: 3732 3c2f 7370 616e 3e20 2020 2020 2062 72 b │ │ │ │ -00010810: 7261 6e63 6865 735f 2e72 6573 6572 7665 ranches_.reserve │ │ │ │ -00010820: 2866 2e3c 6120 636c 6173 733d 2263 6f64 (f.branches_.size()); // reserve spa │ │ │ │ -000108b0: 6365 3c2f 7370 616e 3e3c 2f64 6976 3e0a ce
    . │ │ │ │ -000108c0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    ..
    374 │ │ │ │ -00010a00: 2020 2020 203c 6120 636c 6173 733d 2263 Assignment<L │ │ │ │ -00010a40: 2667 743b 3c2f 613e 2061 7373 6967 6e6d > assignm │ │ │ │ -00010a50: 656e 745f 203d 2061 7373 6967 6e6d 656e ent_ = assignmen │ │ │ │ -00010a60: 743b 3c2f 6469 763e 0a3c 6469 7620 636c t;
    .
    375 .
    3 │ │ │ │ -00010b10: 3736 3c2f 7370 616e 3e20 2020 2020 203c 76 < │ │ │ │ -00010b20: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00010b30: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (s │ │ │ │ -00010b60: 697a 655f 743c 2f73 7061 6e3e 2069 203d ize_t i = │ │ │ │ -00010b70: 2030 3b20 6920 266c 743b 2066 2e3c 6120 0; i < f.br │ │ │ │ -00010bd0: 616e 6368 6573 5f3c 2f61 3e2e 7369 7a65 anches_.size │ │ │ │ -00010be0: 2829 3b20 692b 2b29 207b 3c2f 6469 763e (); i++) {
    │ │ │ │ -00010bf0: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -00010c80: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00010c90: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -00010ca0: 2361 6364 6366 3732 3439 3966 3230 6437 #acdcf72499f20d7 │ │ │ │ -00010cb0: 3935 3835 6134 3332 3239 3834 3862 3138 9585a43229848b18 │ │ │ │ -00010cc0: 3433 223e 2020 3337 373c 2f61 3e3c 2f73 43"> 377 assi │ │ │ │ -00010ce0: 676e 6d65 6e74 5f5b 6c61 6265 6c5f 5d20 gnment_[label_] │ │ │ │ -00010cf0: 3d20 693b 2020 3c73 7061 6e20 636c 6173 = i; // S │ │ │ │ -00010d10: 6574 2061 7373 6967 6e6d 656e 7420 666f et assignment fo │ │ │ │ -00010d20: 7220 6c61 6265 6c20 746f 2069 3c2f 7370 r label to i
    .
    378 < │ │ │ │ -00010d90: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00010de0: 3337 393c 2f73 7061 6e3e 2020 2020 2020 379 │ │ │ │ -00010df0: 2020 3c73 7061 6e20 636c 6173 733d 226b const NodePtr │ │ │ │ -00010e70: 2062 7261 6e63 6820 3d20 662e 3c61 2063 branch = f.bra │ │ │ │ -00010ed0: 6e63 6865 735f 3c2f 613e 5b69 5d3b 3c2f nches_[i];.
    3 │ │ │ │ -00010f30: 3830 3c2f 7370 616e 3e20 2020 2020 2020 80 │ │ │ │ -00010f40: 2070 7573 685f 6261 636b 2862 7261 6e63 push_back(branc │ │ │ │ -00010f50: 682d 2667 743b 6170 706c 7928 6f70 2c20 h->apply(op, │ │ │ │ -00010f60: 6173 7369 676e 6d65 6e74 5f29 293b 3c2f assignment_));..< │ │ │ │ -000111d0: 6120 6964 3d22 6c30 3033 3834 2220 6e61 a id="l00384" na │ │ │ │ -000111e0: 6d65 3d22 6c30 3033 3834 223e 3c2f 613e me="l00384"> │ │ │ │ -000111f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 384 assignm │ │ │ │ -00011220: 656e 745f 2e65 7261 7365 2861 7373 6967 ent_.erase(assig │ │ │ │ -00011230: 6e6d 656e 745f 6974 293b 3c2f 6469 763e nment_it);
    │ │ │ │ -00011240: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00011270: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 385 }.
    386 │ │ │ │ -000112f0: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
    387 │ │ │ │ -00011350: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ -00011360: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ -00011370: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ -00011380: 6e30 3033 3839 2220 6461 7461 2d73 7461 n00389" data-sta │ │ │ │ -00011390: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ -000113a0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    389 NodePtr apply(const │ │ │ │ -00011520: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ -00011570: 556e 6172 793c 2f61 3e26 616d 703b 206f Unary& o │ │ │ │ -00011580: 7029 3c73 7061 6e20 636c 6173 733d 226b p) const o │ │ │ │ -000115a0: 7665 7272 6964 6520 3c2f 7370 616e 3e7b verride { │ │ │ │ -000115b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00011600: 2033 3930 3c2f 7370 616e 3e20 2020 2020 390 │ │ │ │ -00011610: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto r = boost::ma │ │ │ │ -00011640: 6b65 5f73 6861 7265 6426 6c74 3b43 686f ke_shared<Cho │ │ │ │ -00011650: 6963 6526 6774 3b28 6c61 6265 6c5f 2c20 ice>(label_, │ │ │ │ -00011660: 2a3c 7370 616e 2063 6c61 7373 3d22 6b65 *this, op);
    .< │ │ │ │ -00011690: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000116a0: 3e3c 6120 6964 3d22 6c30 3033 3931 2220 > 391 return │ │ │ │ -00011710: 2055 6e69 7175 6528 7229 3b3c 2f64 6976 Unique(r);
    .
    392< │ │ │ │ -00011770: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    .
    393 .
    .
    395 Node │ │ │ │ -00011920: 5074 723c 2f61 3e20 3c61 2063 6c61 7373 Ptr apply(const UnaryAssig │ │ │ │ -000119b0: 6e6d 656e 7426 616d 703b 206f 702c 3c2f nment& op,.
    3 │ │ │ │ -00011a10: 3936 3c2f 7370 616e 3e20 2020 2020 2020 96 │ │ │ │ -00011a20: 2020 2020 2020 2020 2020 203c 7370 616e const Assign │ │ │ │ -00011a80: 6d65 6e74 266c 743b 4c26 6774 3b3c 2f61 ment<L>& assignmen │ │ │ │ -00011aa0: 7429 3c73 7061 6e20 636c 6173 733d 226b t) const o │ │ │ │ -00011ac0: 7665 7272 6964 6520 3c2f 7370 616e 3e7b verride { │ │ │ │ -00011ad0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00011b20: 2033 3937 3c2f 7370 616e 3e20 2020 2020 397 │ │ │ │ -00011b30: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto r = boost::ma │ │ │ │ -00011b60: 6b65 5f73 6861 7265 6426 6c74 3b43 686f ke_shared<Cho │ │ │ │ -00011b70: 6963 6526 6774 3b28 6c61 6265 6c5f 2c20 ice>(label_, │ │ │ │ -00011b80: 2a3c 7370 616e 2063 6c61 7373 3d22 6b65 *this, op, assignme │ │ │ │ -00011bb0: 6e74 293b 3c2f 6469 763e 0a3c 6469 7620 nt);
    .
    398 │ │ │ │ -00011c10: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -00011c30: 6574 7572 6e3c 2f73 7061 6e3e 2055 6e69 eturn Uni │ │ │ │ -00011c40: 7175 6528 7229 3b3c 2f64 6976 3e0a 3c64 que(r);
    . │ │ │ │ -00011c60: 3c61 2069 643d 226c 3030 3339 3922 206e 399 }
    ..
    4 │ │ │ │ -00011d00: 3030 3c2f 7370 616e 3e20 3c2f 6469 763e 00
    │ │ │ │ -00011d10: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00011d40: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 401 / │ │ │ │ -00011d80: 2f20 4170 706c 7920 6269 6e61 7279 206f / Apply binary o │ │ │ │ -00011d90: 7065 7261 746f 7220 2671 756f 743b 6820 perator "h │ │ │ │ -00011da0: 3d20 6620 6f70 2067 2671 756f 743b 206f = f op g" o │ │ │ │ -00011db0: 6e20 4368 6f69 6365 206e 6f64 653c 2f73 n Choice node
    .
    402 │ │ │ │ -00011e20: 2020 203c 7370 616e 2063 6c61 7373 3d22 // Note │ │ │ │ -00011e40: 206f 7020 6973 206e 6f74 2061 7373 756d op is not assum │ │ │ │ -00011e50: 6564 2063 6f6d 6d75 7461 7469 7665 2073 ed commutative s │ │ │ │ -00011e60: 6f20 7765 206e 6565 6420 746f 206b 6565 o we need to kee │ │ │ │ -00011e70: 7020 7472 6163 6b20 6f66 206f 7264 6572 p track of order │ │ │ │ -00011e80: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -00011ea0: 3c61 2069 643d 226c 3030 3430 3322 206e 403 // S │ │ │ │ -00011f00: 696d 706c 7920 6361 6c6c 7320 6170 706c imply calls appl │ │ │ │ -00011f10: 7920 6f6e 2061 7267 756d 656e 7420 746f y on argument to │ │ │ │ -00011f20: 2063 616c 6c20 636f 7272 6563 7420 7669 call correct vi │ │ │ │ -00011f30: 7274 7561 6c20 6d65 7468 6f64 3a3c 2f73 rtual method:
    .
    404 │ │ │ │ -00011fa0: 2020 203c 7370 616e 2063 6c61 7373 3d22 // fC.a │ │ │ │ -00011fc0: 7070 6c79 5f66 5f6f 705f 6728 674c 2920 pply_f_op_g(gL) │ │ │ │ -00011fd0: 2d26 6774 3b20 674c 2e61 7070 6c79 5f67 -> gL.apply_g │ │ │ │ -00011fe0: 5f6f 705f 6643 2866 4329 202d 2667 743b _op_fC(fC) -> │ │ │ │ -00011ff0: 2028 4c65 6166 293c 2f73 7061 6e3e 3c2f (Leaf).
    4 │ │ │ │ -00012050: 3035 3c2f 7370 616e 3e20 2020 203c 7370 05 // fC.apply_f │ │ │ │ -00012080: 5f6f 705f 6728 6743 2920 2d26 6774 3b20 _op_g(gC) -> │ │ │ │ -00012090: 6743 2e61 7070 6c79 5f67 5f6f 705f 6643 gC.apply_g_op_fC │ │ │ │ -000120a0: 2866 4329 202d 2667 743b 2028 6265 6c6f (fC) -> (belo │ │ │ │ -000120b0: 7729 3c2f 7370 616e 3e3c 2f64 6976 3e0a w)
    . │ │ │ │ -000120c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -000120f0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 406 NodePtr< │ │ │ │ -00012170: 2f61 3e20 6170 706c 795f 665f 6f70 5f67 /a> apply_f_op_g │ │ │ │ -00012180: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const Node& g │ │ │ │ -000121e0: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const Binary& │ │ │ │ -00012210: 206f 7029 3c73 7061 6e20 636c 6173 733d op) const │ │ │ │ -00012230: 206f 7665 7272 6964 6520 3c2f 7370 616e override {
    .
    407 │ │ │ │ -000122a0: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ -000122c0: 7572 6e3c 2f73 7061 6e3e 2067 2e61 7070 urn g.app │ │ │ │ -000122d0: 6c79 5f67 5f6f 705f 6643 282a 3c73 7061 ly_g_op_fC(*this, o │ │ │ │ -00012300: 7029 3b3c 2f64 6976 3e0a 3c64 6976 2063 p);
    .
    408 │ │ │ │ -00012360: 2020 7d3c 2f64 6976 3e0a 3c64 6976 2063 }
    .
    409 < │ │ │ │ -000123c0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00012410: 3431 303c 2f73 7061 6e3e 2020 2020 3c73 410 // If second │ │ │ │ -00012440: 2061 7267 756d 656e 7420 6f66 2062 696e argument of bin │ │ │ │ -00012450: 6172 7920 6f70 2069 7320 4c65 6166 206e ary op is Leaf n │ │ │ │ -00012460: 6f64 652c 2072 6563 7572 7365 206f 6e20 ode, recurse on │ │ │ │ -00012470: 6272 616e 6368 6573 3c2f 7370 616e 3e3c branches< │ │ │ │ -00012480: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -000124d0: 3431 313c 2f73 7061 6e3e 2020 2020 3c61 411 No │ │ │ │ -00012530: 6465 5074 723c 2f61 3e20 6170 706c 795f dePtr apply_ │ │ │ │ -00012540: 675f 6f70 5f66 4c28 3c73 7061 6e20 636c g_op_fL(co │ │ │ │ -00012560: 6e73 743c 2f73 7061 6e3e 204c 6561 6626 nst Leaf& │ │ │ │ -00012570: 616d 703b 2066 4c2c 203c 7370 616e 2063 amp; fL, c │ │ │ │ -00012590: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ -000125a0: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const override │ │ │ │ -000125d0: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ -000125e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ -000126b0: 3c61 2069 643d 226c 3030 3431 3322 206e 413 for (auto& │ │ │ │ -00012740: 616d 703b 2661 6d70 3b20 6272 616e 6368 amp;& branch │ │ │ │ -00012750: 203a 2062 7261 6e63 6865 735f 293c 2f64 : branches_).
    41 │ │ │ │ -000127b0: 343c 2f73 7061 6e3e 2020 2020 2020 2020 4 │ │ │ │ -000127c0: 682d 2667 743b 7075 7368 5f62 6163 6b28 h->push_back( │ │ │ │ -000127d0: 664c 2e61 7070 6c79 5f66 5f6f 705f 6728 fL.apply_f_op_g( │ │ │ │ -000127e0: 2a62 7261 6e63 682c 206f 7029 293b 3c2f *branch, op));.
    4 │ │ │ │ -00012840: 3135 3c2f 7370 616e 3e20 2020 2020 203c 15 < │ │ │ │ -00012850: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00012860: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ -00012870: 2f73 7061 6e3e 2055 6e69 7175 6528 6829 /span> Unique(h) │ │ │ │ -00012880: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -000128d0: 2020 3431 363c 2f73 7061 6e3e 2020 2020 416 │ │ │ │ -000128e0: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -00012930: 2020 3431 373c 2f73 7061 6e3e 203c 2f64 417 .
    41 │ │ │ │ -00012990: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 // If second a │ │ │ │ -000129c0: 7267 756d 656e 7420 6f66 2062 696e 6172 rgument of binar │ │ │ │ -000129d0: 7920 6f70 2069 7320 4368 6f69 6365 2c20 y op is Choice, │ │ │ │ -000129e0: 6361 6c6c 2063 6f6e 7374 7275 6374 6f72 call constructor │ │ │ │ -000129f0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -00012a10: 3c61 2069 643d 226c 3030 3431 3922 206e 419 NodePtr apply_g_op_fC( │ │ │ │ -00012ac0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Choice& f │ │ │ │ -00012af0: 432c 203c 7370 616e 2063 6c61 7373 3d22 C, const Binary& │ │ │ │ -00012b20: 3b20 6f70 293c 7370 616e 2063 6c61 7373 ; op) cons │ │ │ │ -00012b40: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
    .
    420 │ │ │ │ -00012bb0: 2020 2020 3c73 7061 6e20 636c 6173 733d auto h = boost: │ │ │ │ -00012be0: 3a6d 616b 655f 7368 6172 6564 266c 743b :make_shared< │ │ │ │ -00012bf0: 4368 6f69 6365 2667 743b 2866 432c 202a Choice>(fC, * │ │ │ │ -00012c00: 3c73 7061 6e20 636c 6173 733d 226b 6579 this, op);
    . │ │ │ │ -00012c40: 3c61 2069 643d 226c 3030 3432 3122 206e 421 return │ │ │ │ -00012cb0: 556e 6971 7565 2868 293b 3c2f 6469 763e Unique(h);
    │ │ │ │ -00012cc0: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ -00012d20: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00012d90: 6120 6964 3d22 6c30 3034 3234 2220 6e61 a id="l00424" na │ │ │ │ -00012da0: 6d65 3d22 6c30 3034 3234 223e 3c2f 613e me="l00424"> │ │ │ │ -00012db0: 3c73 7061 6e20 636c 6173 733d 226c 696e 424 // If │ │ │ │ -00012df0: 2073 6563 6f6e 6420 6172 6775 6d65 6e74 second argument │ │ │ │ -00012e00: 206f 6620 6269 6e61 7279 206f 7020 6973 of binary op is │ │ │ │ -00012e10: 204c 6561 663c 2f73 7061 6e3e 3c2f 6469 Leaf.
    425 │ │ │ │ -00012e70: 3c2f 7370 616e 3e20 2020 203c 7370 616e template │ │ │ │ -00012ea0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ -00012ec0: 6d65 3c2f 7370 616e 3e20 4f50 2667 743b me OP> │ │ │ │ -00012ed0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00012f20: 2034 3236 3c2f 7370 616e 3e20 2020 203c 426 < │ │ │ │ -00012f30: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00012f40: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ -00012f50: 6130 3237 3936 2e68 746d 6c23 6133 3661 a02796.html#a36a │ │ │ │ -00012f60: 3938 6636 3162 3062 3764 6630 3432 3432 98f61b0b7df04242 │ │ │ │ -00012f70: 3762 3133 3262 3331 3364 3766 6422 3e4e 7b132b313d7fd">N │ │ │ │ -00012f80: 6f64 6550 7472 3c2f 613e 2061 7070 6c79 odePtr apply │ │ │ │ -00012f90: 5f66 435f 6f70 5f67 4c28 3c73 7061 6e20 _fC_op_gL( │ │ │ │ -00012fb0: 636f 6e73 743c 2f73 7061 6e3e 204c 6561 const Lea │ │ │ │ -00012fc0: 6626 616d 703b 2067 4c2c 204f 5020 6f70 f& gL, OP op │ │ │ │ -00012fd0: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ -00013010: 6120 6964 3d22 6c30 3034 3237 2220 6e61 a id="l00427" na │ │ │ │ -00013020: 6d65 3d22 6c30 3034 3237 223e 3c2f 613e me="l00427"> │ │ │ │ -00013030: 3c73 7061 6e20 636c 6173 733d 226c 696e 427 aut │ │ │ │ -00013070: 6f3c 2f73 7061 6e3e 2068 203d 2062 6f6f o h = boo │ │ │ │ -00013080: 7374 3a3a 6d61 6b65 5f73 6861 7265 6426 st::make_shared& │ │ │ │ -00013090: 6c74 3b43 686f 6963 6526 6774 3b28 6c61 lt;Choice>(la │ │ │ │ -000130a0: 6265 6c28 292c 206e 7243 686f 6963 6573 bel(), nrChoices │ │ │ │ -000130b0: 2829 293b 3c2f 6469 763e 0a3c 6469 7620 ());
    .
    428 │ │ │ │ -00013110: 2020 2020 203c 7370 616e 2063 6c61 7373 f │ │ │ │ -00013130: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (auto& │ │ │ │ -00013160: 3b26 616d 703b 2062 7261 6e63 6820 3a20 ;& branch : │ │ │ │ -00013170: 6272 616e 6368 6573 5f29 3c2f 6469 763e branches_)
    │ │ │ │ -00013180: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000131b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 429 h-& │ │ │ │ -000131e0: 6774 3b70 7573 685f 6261 636b 2862 7261 gt;push_back(bra │ │ │ │ -000131f0: 6e63 682d 2667 743b 6170 706c 795f 665f nch->apply_f_ │ │ │ │ -00013200: 6f70 5f67 2867 4c2c 206f 7029 293b 3c2f op_g(gL, op));.
    4 │ │ │ │ -00013260: 3330 3c2f 7370 616e 3e20 2020 2020 203c 30 < │ │ │ │ -00013270: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00013280: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ -00013290: 2f73 7061 6e3e 2055 6e69 7175 6528 6829 /span> Unique(h) │ │ │ │ -000132a0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -000132f0: 2020 3433 313c 2f73 7061 6e3e 2020 2020 431 │ │ │ │ -00013300: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -00013350: 2020 3433 323c 2f73 7061 6e3e 203c 2f64 432 .
    ..
    │ │ │ │ -000135e0: 2034 3335 3c2f 7370 616e 3e20 2020 2020 435 │ │ │ │ -000135f0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 if (label_ == │ │ │ │ -00013620: 6c61 6265 6c29 203c 7370 616e 2063 6c61 label) return b │ │ │ │ -00013650: 7261 6e63 6865 735f 5b69 6e64 6578 5d3b ranches_[index]; │ │ │ │ -00013660: 2020 3c73 7061 6e20 636c 6173 733d 2263 // choos │ │ │ │ -00013680: 6520 6272 616e 6368 3c2f 7370 616e 3e3c e branch< │ │ │ │ -00013690: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -000136e0: 3433 363c 2f73 7061 6e3e 203c 2f64 6976 436
    .
    437< │ │ │ │ -00013740: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> // second case │ │ │ │ -00013770: 2c20 6e6f 7420 6c61 6265 6c20 6f66 2069 , not label of i │ │ │ │ -00013780: 6e74 6572 6573 742c 206a 7573 7420 7265 nterest, just re │ │ │ │ -00013790: 6375 7273 653c 2f73 7061 6e3e 3c2f 6469 curse.
    438 │ │ │ │ -000137f0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 auto r │ │ │ │ -00013820: 203d 2062 6f6f 7374 3a3a 6d61 6b65 5f73 = boost::make_s │ │ │ │ -00013830: 6861 7265 6426 6c74 3b43 686f 6963 6526 hared<Choice& │ │ │ │ -00013840: 6774 3b28 6c61 6265 6c5f 2c20 6272 616e gt;(label_, bran │ │ │ │ -00013850: 6368 6573 5f2e 7369 7a65 2829 293b 3c2f ches_.size());.
    4 │ │ │ │ -000138b0: 3339 3c2f 7370 616e 3e20 2020 2020 203c 39 < │ │ │ │ -000138c0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000138d0: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (auto< │ │ │ │ -00013900: 2f73 7061 6e3e 2661 6d70 3b26 616d 703b /span>&& │ │ │ │ -00013910: 2062 7261 6e63 6820 3a20 6272 616e 6368 branch : branch │ │ │ │ -00013920: 6573 5f29 3c2f 6469 763e 0a3c 6469 7620 es_)
    .
    440 │ │ │ │ -00013980: 2020 2020 2020 2072 2d26 6774 3b70 7573 r->pus │ │ │ │ -00013990: 685f 6261 636b 2862 7261 6e63 682d 2667 h_back(branch-&g │ │ │ │ -000139a0: 743b 6368 6f6f 7365 286c 6162 656c 2c20 t;choose(label, │ │ │ │ -000139b0: 696e 6465 7829 293b 3c2f 6469 763e 0a3c index));
    .< │ │ │ │ -000139c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000139d0: 3e3c 6120 6964 3d22 6c30 3034 3431 2220 > 441
    return │ │ │ │ -00013a40: 2055 6e69 7175 6528 7229 3b3c 2f64 6976 Unique(r);
    .
    442< │ │ │ │ -00013aa0: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    .
    443 .
    4 │ │ │ │ -00013b60: 3434 3c2f 7370 616e 3e20 2020 3c73 7061 44 private │ │ │ │ -00013b90: 3a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 :
    .
    │ │ │ │ -00013be0: 2020 3434 353c 2f73 7061 6e3e 2020 2020 445 │ │ │ │ -00013bf0: 3c73 7061 6e20 636c 6173 733d 226b 6579 using Base = DecisionT │ │ │ │ -00013c50: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ -00013c60: 3a4e 6f64 653c 2f61 3e3b 3c2f 6469 763e :Node;
    │ │ │ │ -00013c70: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00013ce0: 6120 6964 3d22 6c30 3034 3438 2220 6e61 a id="l00448" na │ │ │ │ -00013cf0: 6d65 3d22 6c30 3034 3438 223e 3c2f 613e me="l00448"> │ │ │ │ -00013d00: 3c73 7061 6e20 636c 6173 733d 226c 696e 448 frien │ │ │ │ -00013d40: 643c 2f73 7061 6e3e 203c 7370 616e 2063 d c │ │ │ │ -00013d60: 6c61 7373 203c 2f73 7061 6e3e 3c61 2063 lass boost │ │ │ │ -00013dc0: 3a3a 7365 7269 616c 697a 6174 696f 6e3a ::serialization: │ │ │ │ -00013dd0: 3a61 6363 6573 733c 2f61 3e3b 3c2f 6469 :access;.
    449 │ │ │ │ -00013e30: 3c2f 7370 616e 3e20 2020 203c 7370 616e template │ │ │ │ -00013e60: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <class │ │ │ │ -00013e80: 3c2f 7370 616e 3e20 4152 4348 4956 4526 ARCHIVE& │ │ │ │ -00013e90: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    450 │ │ │ │ -00013ef0: 2020 3c73 7061 6e20 636c 6173 733d 226b void │ │ │ │ -00013f10: 3c2f 7370 616e 3e20 7365 7269 616c 697a serializ │ │ │ │ -00013f20: 6528 4152 4348 4956 4526 616d 703b 2061 e(ARCHIVE& a │ │ │ │ -00013f30: 722c 203c 7370 616e 2063 6c61 7373 3d22 r, const │ │ │ │ -00013f70: 756e 7369 676e 6564 3c2f 7370 616e 3e20 unsigned │ │ │ │ -00013f80: 3c73 7061 6e20 636c 6173 733d 226b 6579 int /*ver │ │ │ │ -00013fc0: 7369 6f6e 2a2f 3c2f 7370 616e 3e29 207b sion*/) { │ │ │ │ -00013fd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00014020: 2034 3531 3c2f 7370 616e 3e20 2020 2020 451 │ │ │ │ -00014030: 2061 7220 2661 6d70 3b20 424f 4f53 545f ar & BOOST_ │ │ │ │ -00014040: 5345 5249 414c 495a 4154 494f 4e5f 4241 SERIALIZATION_BA │ │ │ │ -00014050: 5345 5f4f 424a 4543 545f 4e56 5028 4261 SE_OBJECT_NVP(Ba │ │ │ │ -00014060: 7365 293b 3c2f 6469 763e 0a3c 6469 7620 se);
    .
    452 │ │ │ │ -000140c0: 2020 2020 2061 7226 616d 703b 2042 4f4f ar& BOO │ │ │ │ -000140d0: 5354 5f53 4552 4941 4c49 5a41 5449 4f4e ST_SERIALIZATION │ │ │ │ -000140e0: 5f4e 5650 286c 6162 656c 5f29 3b3c 2f64 _NVP(label_);.
    45 │ │ │ │ -00014140: 333c 2f73 7061 6e3e 2020 2020 2020 6172 3 ar │ │ │ │ -00014150: 2661 6d70 3b20 424f 4f53 545f 5345 5249 & BOOST_SERI │ │ │ │ -00014160: 414c 495a 4154 494f 4e5f 4e56 5028 6272 ALIZATION_NVP(br │ │ │ │ -00014170: 616e 6368 6573 5f29 3b3c 2f64 6976 3e0a anches_);
    . │ │ │ │ -00014180: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    455 │ │ │ │ -00014260: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    456 │ │ │ │ -000142c0: 207d 3b20 203c 7370 616e 2063 6c61 7373 }; // Ch │ │ │ │ -000142e0: 6f69 6365 3c2f 7370 616e 3e3c 2f64 6976 oice
    .
    457< │ │ │ │ -00014340: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ -00014360: 3c61 2069 643d 226c 3030 3435 3822 206e 458 /***** │ │ │ │ -000143c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000143d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000143e0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000143f0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014400: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ -00014410: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00014460: 3435 393c 2f73 7061 6e3e 2020 3c73 7061 459 // DecisionTre │ │ │ │ -00014490: 653c 2f73 7061 6e3e 3c2f 6469 763e 0a3c e
    .< │ │ │ │ -000144a0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000144b0: 3e3c 6120 6964 3d22 6c30 3034 3630 2220 > 460 /**** │ │ │ │ -00014510: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014520: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014530: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014540: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014550: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ -00014560: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -000145b0: 2034 3631 3c2f 7370 616e 3e20 203c 7370 461 template<type │ │ │ │ -00014600: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ -00014610: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00014620: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ -00014640: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -000146d0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -000146e0: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -000146f0: 2361 6632 3930 6664 6232 3962 6232 3534 #af290fdb29bb254 │ │ │ │ -00014700: 6535 3162 6661 3239 3532 3038 3635 3562 e51bfa295208655b │ │ │ │ -00014710: 6366 223e 2020 3436 323c 2f61 3e3c 2f73 cf"> 462 DecisionT │ │ │ │ -00014780: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ -00014790: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree() {
    .
    < │ │ │ │ -000147e0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000147f0: 6e6f 223e 2020 3436 333c 2f73 7061 6e3e no"> 463 │ │ │ │ -00014800: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -00014810: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00014880: 6120 6964 3d22 6c30 3034 3635 2220 6e61 a id="l00465" na │ │ │ │ -00014890: 6d65 3d22 6c30 3034 3635 223e 3c2f 613e me="l00465"> │ │ │ │ -000148a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 465 templat │ │ │ │ -000148e0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ -00014930: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ -00014940: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00014990: 2034 3636 3c2f 7370 616e 3e20 203c 6120 466 De │ │ │ │ -000149f0: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00014a00: 2059 2667 743b 3a3a 4465 6369 7369 6f6e Y>::Decision │ │ │ │ -00014a10: 5472 6565 3c2f 613e 283c 7370 616e 2063 Tree(c │ │ │ │ -00014a30: 6f6e 7374 3c2f 7370 616e 3e20 4e6f 6465 onst Node │ │ │ │ -00014a40: 5074 7226 616d 703b 2072 6f6f 7429 203a Ptr& root) : │ │ │ │ -00014a50: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00014aa0: 2034 3637 3c2f 7370 616e 3e20 2020 2072 467 r │ │ │ │ -00014ab0: 6f6f 745f 2872 6f6f 7429 207b 3c2f 6469 oot_(root) {.
    468 │ │ │ │ -00014b10: 3c2f 7370 616e 3e20 207d 3c2f 6469 763e }
    │ │ │ │ -00014b20: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00014b90: 6120 6964 3d22 6c30 3034 3730 2220 6e61 a id="l00470" na │ │ │ │ -00014ba0: 6d65 3d22 6c30 3034 3730 223e 3c2f 613e me="l00470"> │ │ │ │ -00014bb0: 3c73 7061 6e20 636c 6173 733d 226c 696e 470 /****** │ │ │ │ -00014bf0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014c00: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014c10: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014c20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00014c30: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    4 │ │ │ │ -00014c90: 3731 3c2f 7370 616e 3e20 203c 7370 616e 71 template │ │ │ │ -00014cc0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ -00014ce0: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ -00014d20: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ -00014d30: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ -00014d40: 6e30 3034 3732 2220 6461 7461 2d73 7461 n00472" data-sta │ │ │ │ -00014d50: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ -00014d60: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    472 DecisionTre │ │ │ │ -00014e60: 6526 6c74 3b4c 2c20 5926 6774 3b3a 3a44 e<L, Y>::D │ │ │ │ -00014e70: 6563 6973 696f 6e54 7265 653c 2f61 3e28 ecisionTree( │ │ │ │ -00014e80: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Y& y) {< │ │ │ │ -00014eb0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00014f00: 3437 333c 2f73 7061 6e3e 2020 2020 726f 473 ro │ │ │ │ -00014f10: 6f74 5f20 3d20 3c61 2063 6c61 7373 3d22 ot_ = NodePtr(new │ │ │ │ -00014fc0: 4c65 6166 3c2f 613e 2879 2929 3b3c 2f64 Leaf(y));.
    47 │ │ │ │ -00015020: 343c 2f73 7061 6e3e 2020 7d3c 2f64 6976 4 }
    .
    .
    475 .
    4 │ │ │ │ -000150e0: 3736 3c2f 7370 616e 3e3c 7370 616e 2063 76 │ │ │ │ -00015100: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ -00015110: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015120: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015130: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015140: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ -00015150: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -00015170: 6120 6964 3d22 6c30 3034 3737 2220 6e61 a id="l00477" na │ │ │ │ -00015180: 6d65 3d22 6c30 3034 3737 223e 3c2f 613e me="l00477"> │ │ │ │ -00015190: 3c73 7061 6e20 636c 6173 733d 226c 696e 477 templat │ │ │ │ -000151d0: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename
    L, type │ │ │ │ -00015220: 6e61 6d65 3c2f 7370 616e 3e20 5926 6774 name Y> │ │ │ │ -00015230: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    . │ │ │ │ -00015290: 3c61 2069 643d 226c 3030 3437 3822 206e 478 │ │ │ │ -00015310: 3c2f 613e 3c2f 7370 616e 3e20 203c 6120 De │ │ │ │ -00015370: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00015380: 2059 2667 743b 3a3a 4465 6369 7369 6f6e Y>::Decision │ │ │ │ -00015390: 5472 6565 3c2f 613e 283c 7370 616e 2063 Tree(c │ │ │ │ -000153b0: 6f6e 7374 3c2f 7370 616e 3e20 4c26 616d onst L&am │ │ │ │ -000153c0: 703b 206c 6162 656c 2c20 3c73 7061 6e20 p; label, │ │ │ │ -000153e0: 636f 6e73 743c 2f73 7061 6e3e 2059 2661 const Y&a │ │ │ │ -000153f0: 6d70 3b20 7931 2c20 3c73 7061 6e20 636c mp; y1, co │ │ │ │ -00015410: 6e73 743c 2f73 7061 6e3e 2059 2661 6d70 nst Y& │ │ │ │ -00015420: 3b20 7932 2920 7b3c 2f64 6976 3e0a 3c64 ; y2) {
    . │ │ │ │ -00015440: 3c61 2069 643d 226c 3030 3437 3922 206e 479 auto │ │ │ │ -000154a0: 3c2f 7370 616e 3e20 6120 3d20 626f 6f73 a = boos │ │ │ │ -000154b0: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ -000154c0: 743b 4368 6f69 6365 2667 743b 286c 6162 t;Choice>(lab │ │ │ │ -000154d0: 656c 2c20 3229 3b3c 2f64 6976 3e0a 3c64 el, 2);
    . │ │ │ │ -000154f0: 3c61 2069 643d 226c 3030 3438 3022 206e 480 NodePtr l1(new Leaf(y1)) │ │ │ │ -000155f0: 2c20 6c32 283c 7370 616e 2063 6c61 7373 , l2(new Leaf(y2)) │ │ │ │ -00015650: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -000156a0: 2020 3438 313c 2f73 7061 6e3e 2020 2020 481 │ │ │ │ -000156b0: 612d 2667 743b 7075 7368 5f62 6163 6b28 a->push_back( │ │ │ │ -000156c0: 6c31 293b 3c2f 6469 763e 0a3c 6469 7620 l1);
    .
    482 │ │ │ │ -00015720: 2020 2061 2d26 6774 3b70 7573 685f 6261 a->push_ba │ │ │ │ -00015730: 636b 286c 3229 3b3c 2f64 6976 3e0a 3c64 ck(l2);
    . │ │ │ │ -00015750: 3c61 2069 643d 226c 3030 3438 3322 206e 483 root_ = Ch │ │ │ │ -000157a0: 6f69 6365 3a3a 556e 6971 7565 2861 293b oice::Unique(a); │ │ │ │ -000157b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00015800: 2034 3834 3c2f 7370 616e 3e20 207d 3c2f 484 }.
    .
    < │ │ │ │ -00015850: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00015860: 6e6f 223e 2020 3438 353c 2f73 7061 6e3e no"> 485 │ │ │ │ -00015870: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    │ │ │ │ -000158c0: 2020 3438 363c 2f73 7061 6e3e 3c73 7061 486 /*********** │ │ │ │ -000158f0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015900: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015910: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015920: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00015930: 2a2f 3c2f 7370 616e 3e3c 2f64 6976 3e0a */
    . │ │ │ │ -00015940: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -00015a60: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00015a90: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ -00015af0: 3438 383c 2f61 3e3c 2f73 7061 6e3e 2020 488 │ │ │ │ -00015b00: 3c61 2063 6c61 7373 3d22 636f 6465 2068 DecisionTree< │ │ │ │ -00015b60: 3b4c 2c20 5926 6774 3b3a 3a44 6563 6973 ;L, Y>::Decis │ │ │ │ -00015b70: 696f 6e54 7265 653c 2f61 3e28 3c73 7061 ionTree(const < │ │ │ │ -00015ba0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00015bb0: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ -00015bc0: 6130 3237 3936 2e68 746d 6c23 6164 6364 a02796.html#adcd │ │ │ │ -00015bd0: 6438 3061 6231 6166 3431 3564 3231 3233 d80ab1af415d2123 │ │ │ │ -00015be0: 3937 6132 3636 3633 3230 6533 3522 3e4c 97a2666320e35">L │ │ │ │ -00015bf0: 6162 656c 433c 2f61 3e26 616d 703b 206c abelC& l │ │ │ │ -00015c00: 6162 656c 432c 203c 7370 616e 2063 6c61 abelC, con │ │ │ │ -00015c20: 7374 3c2f 7370 616e 3e20 5926 616d 703b st Y& │ │ │ │ -00015c30: 2079 312c 3c2f 6469 763e 0a3c 6469 7620 y1,
    .
    489 │ │ │ │ -00015c90: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00015ca0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00015cb0: 2020 3c73 7061 6e20 636c 6173 733d 226b const Y& y2) │ │ │ │ -00015ce0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -00015d30: 2020 3439 303c 2f73 7061 6e3e 2020 2020 490 │ │ │ │ -00015d40: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (labelC.seco │ │ │ │ -00015d70: 6e64 2021 3d20 3229 203c 7370 616e 2063 nd != 2) throw │ │ │ │ -00015da0: 7374 643a 3a69 6e76 616c 6964 5f61 7267 std::invalid_arg │ │ │ │ -00015db0: 756d 656e 7428 3c2f 6469 763e 0a3c 6469 ument(
    .< │ │ │ │ -00015dd0: 6120 6964 3d22 6c30 3034 3931 2220 6e61 a id="l00491" na │ │ │ │ -00015de0: 6d65 3d22 6c30 3034 3931 223e 3c2f 613e me="l00491"> │ │ │ │ -00015df0: 3c73 7061 6e20 636c 6173 733d 226c 696e 491 "Decis │ │ │ │ -00015e40: 696f 6e54 7265 653a 2062 696e 6172 7920 ionTree: binary │ │ │ │ -00015e50: 636f 6e73 7472 7563 746f 7220 6361 6c6c constructor call │ │ │ │ -00015e60: 6564 2077 6974 6820 6e6f 6e2d 6269 6e61 ed with non-bina │ │ │ │ -00015e70: 7279 206c 6162 656c 2671 756f 743b 3c2f ry label");
    . │ │ │ │ -00015ea0: 3c61 2069 643d 226c 3030 3439 3222 206e 492 auto │ │ │ │ -00015f00: 3c2f 7370 616e 3e20 6120 3d20 626f 6f73 a = boos │ │ │ │ -00015f10: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ -00015f20: 743b 4368 6f69 6365 2667 743b 286c 6162 t;Choice>(lab │ │ │ │ -00015f30: 656c 432e 6669 7273 742c 2032 293b 3c2f elC.first, 2);.
    4 │ │ │ │ -00015f90: 3933 3c2f 7370 616e 3e20 2020 203c 6120 93 Nod │ │ │ │ -00015ff0: 6550 7472 3c2f 613e 206c 3128 3c73 7061 ePtr l1(new Leaf(y1)), l2(new Leaf(y2));
    .< │ │ │ │ -000160c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000160d0: 3e3c 6120 6964 3d22 6c30 3034 3934 2220 > 494 a->pus │ │ │ │ -00016120: 685f 6261 636b 286c 3129 3b3c 2f64 6976 h_back(l1);
    .
    495< │ │ │ │ -00016180: 2f73 7061 6e3e 2020 2020 612d 2667 743b /span> a-> │ │ │ │ -00016190: 7075 7368 5f62 6163 6b28 6c32 293b 3c2f push_back(l2);.
    4 │ │ │ │ -000161f0: 3936 3c2f 7370 616e 3e20 2020 2072 6f6f 96 roo │ │ │ │ -00016200: 745f 203d 2043 686f 6963 653a 3a55 6e69 t_ = Choice::Uni │ │ │ │ -00016210: 7175 6528 6129 3b3c 2f64 6976 3e0a 3c64 que(a);
    . │ │ │ │ -00016230: 3c61 2069 643d 226c 3030 3439 3722 206e 497 }
    ..
    498 │ │ │ │ -000162d0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ -000162e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000162f0: 3e3c 6120 6964 3d22 6c30 3034 3939 2220 > 499 /**** │ │ │ │ -00016350: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016360: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016370: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016380: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016390: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ -000163a0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -000163f0: 2035 3030 3c2f 7370 616e 3e20 203c 7370 500 template<type │ │ │ │ -00016440: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ -00016450: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00016460: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ -00016480: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -00016510: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00016520: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -00016530: 2361 6437 3534 6438 3639 3963 6232 3035 #ad754d8699cb205 │ │ │ │ -00016540: 3738 3866 3838 3966 3031 3237 3831 6264 788f889f012781bd │ │ │ │ -00016550: 6332 223e 2020 3530 313c 2f61 3e3c 2f73 c2"> 501 DecisionT │ │ │ │ -000165c0: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ -000165d0: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree(const std::vector │ │ │ │ -00016610: 266c 743b 4c61 6265 6c43 2667 743b 2661 <LabelC>&a │ │ │ │ -00016620: 6d70 3b20 6c61 6265 6c43 732c 3c2f 6469 mp; labelCs,.
    502 │ │ │ │ -00016680: 3c2f 7370 616e 3e20 2020 2020 203c 7370 const │ │ │ │ -000166b0: 7374 643a 3a76 6563 746f 7226 6c74 3b59 std::vector<Y │ │ │ │ -000166c0: 2667 743b 2661 6d70 3b20 7973 2920 7b3c >& ys) {< │ │ │ │ -000166d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00016720: 3530 333c 2f73 7061 6e3e 2020 2020 3c73 503 // call recu │ │ │ │ -00016750: 7273 6976 6520 4372 6561 7465 3c2f 7370 rsive Create
    .
    504 │ │ │ │ -000167c0: 2020 726f 6f74 5f20 3d20 6372 6561 7465 root_ = create │ │ │ │ -000167d0: 286c 6162 656c 4373 2e62 6567 696e 2829 (labelCs.begin() │ │ │ │ -000167e0: 2c20 6c61 6265 6c43 732e 656e 6428 292c , labelCs.end(), │ │ │ │ -000167f0: 2079 732e 6265 6769 6e28 292c 2079 732e ys.begin(), ys. │ │ │ │ -00016800: 656e 6428 2929 3b3c 2f64 6976 3e0a 3c64 end());
    . │ │ │ │ -00016820: 3c61 2069 643d 226c 3030 3530 3522 206e 505 }
    ..
    506 │ │ │ │ -000168c0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ -000168d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000168e0: 3e3c 6120 6964 3d22 6c30 3035 3037 2220 > 507 /**** │ │ │ │ -00016940: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016950: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016960: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016970: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00016980: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ -00016990: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -000169e0: 2035 3038 3c2f 7370 616e 3e20 203c 7370 508 template<type │ │ │ │ -00016a30: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ -00016a40: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00016a50: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ -00016a70: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ -00016b00: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00016b10: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ -00016b20: 2361 3361 3961 3462 3432 3039 3831 3266 #a3a9a4b4209812f │ │ │ │ -00016b30: 3037 3163 3132 3235 6439 3534 3266 6134 071c1225d9542fa4 │ │ │ │ -00016b40: 3433 223e 2020 3530 393c 2f61 3e3c 2f73 43"> 509 DecisionT │ │ │ │ -00016bb0: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ -00016bc0: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree(const std::vector │ │ │ │ -00016c00: 266c 743b 4c61 6265 6c43 2667 743b 2661 <LabelC>&a │ │ │ │ -00016c10: 6d70 3b20 6c61 6265 6c43 732c 3c2f 6469 mp; labelCs,.
    510 │ │ │ │ -00016c70: 3c2f 7370 616e 3e20 2020 2020 203c 7370 const │ │ │ │ -00016ca0: 7374 643a 3a73 7472 696e 6726 616d 703b std::string& │ │ │ │ -00016cb0: 2074 6162 6c65 2920 7b3c 2f64 6976 3e0a table) {
    . │ │ │ │ -00016cc0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00016cf0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 511 // │ │ │ │ -00016d30: 2043 6f6e 7665 7274 2073 7464 3a3a 7374 Convert std::st │ │ │ │ -00016d40: 7269 6e67 2074 6f20 7661 6c75 6573 206f ring to values o │ │ │ │ -00016d50: 6620 7479 7065 2059 3c2f 7370 616e 3e3c f type Y< │ │ │ │ -00016d60: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00016db0: 3531 323c 2f73 7061 6e3e 2020 2020 7374 512 st │ │ │ │ -00016dc0: 643a 3a76 6563 746f 7226 6c74 3b59 2667 d::vector<Y&g │ │ │ │ -00016dd0: 743b 2079 733b 3c2f 6469 763e 0a3c 6469 t; ys;
    .< │ │ │ │ -00016df0: 6120 6964 3d22 6c30 3035 3133 2220 6e61 a id="l00513" na │ │ │ │ -00016e00: 6d65 3d22 6c30 3035 3133 223e 3c2f 613e me="l00513"> │ │ │ │ -00016e10: 3c73 7061 6e20 636c 6173 733d 226c 696e 513 std::istrin │ │ │ │ -00016e40: 6773 7472 6561 6d20 6973 7328 7461 626c gstream iss(tabl │ │ │ │ -00016e50: 6529 3b3c 2f64 6976 3e0a 3c64 6976 2063 e);
    .
    514 │ │ │ │ -00016eb0: 2020 636f 7079 2873 7464 3a3a 6973 7472 copy(std::istr │ │ │ │ -00016ec0: 6561 6d5f 6974 6572 6174 6f72 266c 743b eam_iterator< │ │ │ │ -00016ed0: 5926 6774 3b28 6973 7329 2c20 7374 643a Y>(iss), std: │ │ │ │ -00016ee0: 3a69 7374 7265 616d 5f69 7465 7261 746f :istream_iterato │ │ │ │ -00016ef0: 7226 6c74 3b59 2667 743b 2829 2c3c 2f64 r<Y>(),.
    51 │ │ │ │ -00016f50: 353c 2f73 7061 6e3e 2020 2020 2020 2020 5 │ │ │ │ -00016f60: 2062 6163 6b5f 696e 7365 7274 6572 2879 back_inserter(y │ │ │ │ -00016f70: 7329 293b 3c2f 6469 763e 0a3c 6469 7620 s));
    .
    516 │ │ │ │ -00016fd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00017020: 2035 3137 3c2f 7370 616e 3e20 2020 203c 517 < │ │ │ │ -00017030: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00017040: 656e 7422 3e2f 2f20 6e6f 7720 6361 6c6c ent">// now call │ │ │ │ -00017050: 2072 6563 7572 7369 7665 2043 7265 6174 recursive Creat │ │ │ │ -00017060: 653c 2f73 7061 6e3e 3c2f 6469 763e 0a3c e
    .< │ │ │ │ -00017070: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00017080: 3e3c 6120 6964 3d22 6c30 3035 3138 2220 > 518
    root_ = c │ │ │ │ -000170d0: 7265 6174 6528 6c61 6265 6c43 732e 6265 reate(labelCs.be │ │ │ │ -000170e0: 6769 6e28 292c 206c 6162 656c 4373 2e65 gin(), labelCs.e │ │ │ │ -000170f0: 6e64 2829 2c20 7973 2e62 6567 696e 2829 nd(), ys.begin() │ │ │ │ -00017100: 2c20 7973 2e65 6e64 2829 293b 3c2f 6469 , ys.end());.
    519 │ │ │ │ -00017160: 3c2f 7370 616e 3e20 207d 3c2f 6469 763e }
    │ │ │ │ -00017170: 0a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 .
    .
    │ │ │ │ -000171c0: 2020 3532 303c 2f73 7061 6e3e 203c 2f64 520 .
    52 │ │ │ │ -00017220: 313c 2f73 7061 6e3e 3c73 7061 6e20 636c 1 │ │ │ │ -00017240: 2f2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /*************** │ │ │ │ -00017250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017260: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f *************/
    .
    < │ │ │ │ -000172d0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000172e0: 6e6f 223e 2020 3532 323c 2f73 7061 6e3e no"> 522 │ │ │ │ -000172f0: 2020 3c73 7061 6e20 636c 6173 733d 226b template │ │ │ │ -00017310: 3c2f 7370 616e 3e26 6c74 3b3c 7370 616e <typename │ │ │ │ -00017340: 204c 2c20 3c73 7061 6e20 636c 6173 733d L, typena │ │ │ │ -00017360: 6d65 3c2f 7370 616e 3e20 5926 6774 3b3c me Y>< │ │ │ │ -00017370: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    ..
    524 │ │ │ │ -00017590: 2020 2020 4974 6572 6174 6f72 2062 6567 Iterator beg │ │ │ │ -000175a0: 696e 2c20 4974 6572 6174 6f72 2065 6e64 in, Iterator end │ │ │ │ -000175b0: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const L& labe │ │ │ │ -000175e0: 6c29 207b 3c2f 6469 763e 0a3c 6469 7620 l) {
    .
    525 │ │ │ │ -00017640: 2020 2072 6f6f 745f 203d 2063 6f6d 706f root_ = compo │ │ │ │ -00017650: 7365 2862 6567 696e 2c20 656e 642c 206c se(begin, end, l │ │ │ │ -00017660: 6162 656c 293b 3c2f 6469 763e 0a3c 6469 abel);
    .< │ │ │ │ -00017680: 6120 6964 3d22 6c30 3035 3236 2220 6e61 a id="l00526" na │ │ │ │ -00017690: 6d65 3d22 6c30 3035 3236 223e 3c2f 613e me="l00526"> │ │ │ │ -000176a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 526 }
    .
    .
    527< │ │ │ │ -00017720: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ -00017740: 3c61 2069 643d 226c 3030 3532 3822 206e 528 /***** │ │ │ │ -000177a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000177b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000177c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000177d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000177e0: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ -000177f0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00017840: 3532 393c 2f73 7061 6e3e 2020 3c73 7061 529 template<typen │ │ │ │ -00017890: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ -000178d0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    530 DecisionTr │ │ │ │ -00017a10: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ -00017a20: 4465 6369 7369 6f6e 5472 6565 3c2f 613e DecisionTree │ │ │ │ -00017a30: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const L& label │ │ │ │ -00017a60: 2c3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ,
    .
    │ │ │ │ -00017ab0: 2020 3533 313c 2f73 7061 6e3e 2020 2020 531 │ │ │ │ -00017ac0: 2020 3c73 7061 6e20 636c 6173 733d 226b const DecisionTree& f0, const Decisi │ │ │ │ -00017b80: 6f6e 5472 6565 3c2f 613e 2661 6d70 3b20 onTree& │ │ │ │ -00017b90: 6631 2920 207b 3c2f 6469 763e 0a3c 6469 f1) {
    .< │ │ │ │ -00017bb0: 6120 6964 3d22 6c30 3035 3332 2220 6e61 a id="l00532" na │ │ │ │ -00017bc0: 6d65 3d22 6c30 3035 3332 223e 3c2f 613e me="l00532"> │ │ │ │ -00017bd0: 3c73 7061 6e20 636c 6173 733d 226c 696e 532 const │ │ │ │ -00017c10: 3c2f 7370 616e 3e20 7374 643a 3a76 6563 std::vec │ │ │ │ -00017c20: 746f 7226 6c74 3b44 6563 6973 696f 6e54 tor<DecisionT │ │ │ │ -00017c30: 7265 6526 6774 3b20 6675 6e63 7469 6f6e ree> function │ │ │ │ -00017c40: 737b 6630 2c20 6631 7d3b 3c2f 6469 763e s{f0, f1};
    │ │ │ │ -00017c50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00017c80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 533 root_ = │ │ │ │ -00017cb0: 2063 6f6d 706f 7365 2866 756e 6374 696f compose(functio │ │ │ │ -00017cc0: 6e73 2e62 6567 696e 2829 2c20 6675 6e63 ns.begin(), func │ │ │ │ -00017cd0: 7469 6f6e 732e 656e 6428 292c 206c 6162 tions.end(), lab │ │ │ │ -00017ce0: 656c 293b 3c2f 6469 763e 0a3c 6469 7620 el);
    .
    534 │ │ │ │ -00017d40: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ -00017d50: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ -00017de0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00017df0: 6e6f 223e 2020 3533 363c 2f73 7061 6e3e no"> 536 │ │ │ │ -00017e00: 3c73 7061 6e20 636c 6173 733d 2263 6f6d /******* │ │ │ │ -00017e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017e40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017e50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00017e60: 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c 2f64 *****/.
    53 │ │ │ │ -00017ec0: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ -00017ee0: 7465 6d70 6c61 7465 3c2f 7370 616e 3e20 template │ │ │ │ -00017ef0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ -00017f10: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ -00017f50: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00017f60: 3e3c 6120 6964 3d22 6c30 3035 3338 2220 > 538 templ │ │ │ │ -00017fc0: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ -00017fd0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00017fe0: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    X, ty │ │ │ │ -00018010: 7065 6e61 6d65 3c2f 7370 616e 3e20 4675 pename Fu │ │ │ │ -00018020: 6e63 2667 743b 3c2f 6469 763e 0a3c 6469 nc>
    ...
    < │ │ │ │ -00018240: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00018250: 6e6f 223e 2020 3534 303c 2f73 7061 6e3e no"> 540 │ │ │ │ -00018260: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00018270: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00018280: 2020 2046 756e 6320 595f 6f66 5f58 2920 Func Y_of_X) │ │ │ │ -00018290: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ -000182e0: 2020 3534 313c 2f73 7061 6e3e 2020 2020 541 │ │ │ │ -000182f0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // Define │ │ │ │ -00018310: 6675 6e63 746f 7220 666f 7220 6964 656e functor for iden │ │ │ │ -00018320: 7469 7479 206d 6170 7069 6e67 206f 6620 tity mapping of │ │ │ │ -00018330: 6e6f 6465 206c 6162 656c 2e3c 2f73 7061 node label.
    .
    542 │ │ │ │ -000183a0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto L_of_L = [](< │ │ │ │ -000183d0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000183e0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const L& label) │ │ │ │ -00018400: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ -00018420: 726e 3c2f 7370 616e 3e20 6c61 6265 6c3b rn label; │ │ │ │ -00018430: 207d 3b3c 2f64 6976 3e0a 3c64 6976 2063 };
    .
    543 │ │ │ │ -00018490: 2020 726f 6f74 5f20 3d20 636f 6e76 6572 root_ = conver │ │ │ │ -000184a0: 7446 726f 6d26 6c74 3b4c 2c20 5826 6774 tFrom<L, X> │ │ │ │ -000184b0: 3b28 6f74 6865 722e 3c61 2063 6c61 7373 ;(other.root_, L_of_L, Y_of │ │ │ │ -00018520: 5f58 293b 3c2f 6469 763e 0a3c 6469 7620 _X);
    .
    544 │ │ │ │ -00018580: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ -00018590: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ -00018620: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00018630: 6e6f 223e 2020 3534 363c 2f73 7061 6e3e no"> 546 │ │ │ │ -00018640: 3c73 7061 6e20 636c 6173 733d 2263 6f6d /******* │ │ │ │ -00018660: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018670: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018680: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018690: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000186a0: 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c 2f64 *****/.
    54 │ │ │ │ -00018700: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ -00018720: 7465 6d70 6c61 7465 3c2f 7370 616e 3e20 template │ │ │ │ -00018730: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ -00018750: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ -00018790: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000187a0: 3e3c 6120 6964 3d22 6c30 3035 3438 2220 > 548 templ │ │ │ │ -00018800: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ -00018810: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00018820: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    M, ty │ │ │ │ -00018850: 7065 6e61 6d65 3c2f 7370 616e 3e20 582c pename X, │ │ │ │ -00018860: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ -00018880: 2f73 7061 6e3e 2046 756e 6326 6774 3b3c /span> Func>< │ │ │ │ -00018890: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ -00018910: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00018920: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 549 Deci │ │ │ │ -000189d0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ -000189e0: 2667 743b 3a3a 4465 6369 7369 6f6e 5472 >::DecisionTr │ │ │ │ -000189f0: 6565 3c2f 613e 283c 7370 616e 2063 6c61 ee(con │ │ │ │ -00018a10: 7374 3c2f 7370 616e 3e20 3c61 2063 6c61 st DecisionTr │ │ │ │ -00018a50: 6565 266c 743b 4d2c 2058 2667 743b 3c2f ee<M, X>& other,.
    5 │ │ │ │ -00018ac0: 3530 3c2f 7370 616e 3e20 2020 2020 2020 50 │ │ │ │ -00018ad0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00018ae0: 2020 2020 2020 2020 2020 2020 3c73 7061 const s │ │ │ │ -00018b10: 7464 3a3a 6d61 7026 6c74 3b4d 2c20 4c26 td::map<M, L& │ │ │ │ -00018b20: 6774 3b26 616d 703b 206d 6170 2c20 4675 gt;& map, Fu │ │ │ │ -00018b30: 6e63 2059 5f6f 665f 5829 207b 3c2f 6469 nc Y_of_X) {.
    551 │ │ │ │ -00018b90: 3c2f 7370 616e 3e20 2020 203c 7370 616e auto L_o │ │ │ │ -00018bc0: 665f 4d20 3d20 5b26 616d 703b 6d61 705d f_M = [&map] │ │ │ │ -00018bd0: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const M& label │ │ │ │ -00018c00: 2920 2d26 6774 3b20 4c20 7b20 3c73 7061 ) -> L { return map.at(label │ │ │ │ -00018c40: 293b 207d 3b3c 2f64 6976 3e0a 3c64 6976 ); };
    .
    < │ │ │ │ -00018c80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00018c90: 6e6f 223e 2020 3535 323c 2f73 7061 6e3e no"> 552 │ │ │ │ -00018ca0: 2020 2020 726f 6f74 5f20 3d20 636f 6e76 root_ = conv │ │ │ │ -00018cb0: 6572 7446 726f 6d26 6c74 3b4d 2c20 5826 ertFrom<M, X& │ │ │ │ -00018cc0: 6774 3b28 6f74 6865 722e 3c61 2063 6c61 gt;(other.root_ │ │ │ │ -00018d20: 3c2f 613e 2c20 4c5f 6f66 5f4d 2c20 595f , L_of_M, Y_ │ │ │ │ -00018d30: 6f66 5f58 293b 3c2f 6469 763e 0a3c 6469 of_X);
    .< │ │ │ │ -00018d50: 6120 6964 3d22 6c30 3035 3533 2220 6e61 a id="l00553" na │ │ │ │ -00018d60: 6d65 3d22 6c30 3035 3533 223e 3c2f 613e me="l00553"> │ │ │ │ -00018d70: 3c73 7061 6e20 636c 6173 733d 226c 696e 553 }
    .
    .
    554< │ │ │ │ -00018df0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ -00018e10: 3c61 2069 643d 226c 3030 3535 3522 206e 555 /***** │ │ │ │ -00018e70: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018e80: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018e90: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018ea0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00018eb0: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ -00018ec0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00018f10: 3535 363c 2f73 7061 6e3e 2020 3c73 7061 556 // Called by t │ │ │ │ -00018f40: 776f 2063 6f6e 7374 7275 6374 6f72 7320 wo constructors │ │ │ │ -00018f50: 6162 6f76 652e 3c2f 7370 616e 3e3c 2f64 above..
    55 │ │ │ │ -00018fb0: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ -00018fd0: 2f2f 2054 616b 6573 2061 206c 6162 656c // Takes a label │ │ │ │ -00018fe0: 2061 6e64 2061 2063 6f72 7265 7370 6f6e and a correspon │ │ │ │ -00018ff0: 6469 6e67 2072 616e 6765 206f 6620 6465 ding range of de │ │ │ │ -00019000: 6369 7369 6f6e 2074 7265 6573 2c20 616e cision trees, an │ │ │ │ -00019010: 6420 6372 6561 7465 7320 613c 2f73 7061 d creates a
    .
    558 < │ │ │ │ -00019080: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00019090: 656e 7422 3e2f 2f20 6e65 7720 6465 6369 ent">// new deci │ │ │ │ -000190a0: 7369 6f6e 2074 7265 652e 2048 6f77 6576 sion tree. Howev │ │ │ │ -000190b0: 6572 2c20 7468 6520 6f72 6465 7220 6f66 er, the order of │ │ │ │ -000190c0: 2074 6865 206c 6162 656c 7320 6e65 6564 the labels need │ │ │ │ -000190d0: 7320 746f 2062 6520 7265 7370 6563 7465 s to be respecte │ │ │ │ -000190e0: 642c 3c2f 7370 616e 3e3c 2f64 6976 3e0a d,
    . │ │ │ │ -000190f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00019120: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 559 // s │ │ │ │ -00019160: 6f20 7765 2063 616e 6e6f 7420 6a75 7374 o we cannot just │ │ │ │ -00019170: 2063 7265 6174 6520 6120 726f 6f74 2043 create a root C │ │ │ │ -00019180: 686f 6963 6520 6e6f 6465 206f 6e20 7468 hoice node on th │ │ │ │ -00019190: 6520 6c61 6265 6c3a 2069 6620 7468 6520 e label: if the │ │ │ │ -000191a0: 6c61 6265 6c20 6973 3c2f 7370 616e 3e3c label is< │ │ │ │ -000191b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00019200: 3536 303c 2f73 7061 6e3e 2020 3c73 7061 560 // not the hig │ │ │ │ -00019230: 6865 7374 206c 6162 656c 2c20 7765 206e hest label, we n │ │ │ │ -00019240: 6565 6420 6120 636f 6d70 6c69 6361 7465 eed a complicate │ │ │ │ -00019250: 642f 2065 7870 656e 7369 7665 2072 6563 d/ expensive rec │ │ │ │ -00019260: 7572 7369 7665 2063 616c 6c2e 3c2f 7370 ursive call.
    .
    561 │ │ │ │ -000192d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 template < │ │ │ │ -00019310: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -00019320: 4c2c 203c 7370 616e 2063 6c61 7373 3d22 L, typenam │ │ │ │ -00019340: 653c 2f73 7061 6e3e 2059 2667 743b 3c2f e Y>.
    5 │ │ │ │ -000193a0: 3632 3c2f 7370 616e 3e20 203c 7370 616e 62 template │ │ │ │ -000193d0: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ -000193f0: 616d 653c 2f73 7061 6e3e 2049 7465 7261 ame Itera │ │ │ │ -00019400: 746f 7226 6774 3b3c 2f64 6976 3e0a 3c64 tor>
    . │ │ │ │ -00019420: 3c61 2069 643d 226c 3030 3536 3322 206e 563 typena │ │ │ │ -00019480: 6d65 3c2f 7370 616e 3e20 3c61 2063 6c61 me Decisi │ │ │ │ -000194e0: 6f6e 5472 6565 266c 743b 4c2c 2059 2667 onTree<L, Y&g │ │ │ │ -000194f0: 743b 3a3a 4e6f 6465 5074 723c 2f61 3e20 t;::NodePtr │ │ │ │ -00019500: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Deci │ │ │ │ -00019530: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ -00019540: 2667 743b 3a3a 636f 6d70 6f73 653c 2f61 >::compose(
    .
    564 │ │ │ │ -000195b0: 2020 2049 7465 7261 746f 7220 6265 6769 Iterator begi │ │ │ │ -000195c0: 6e2c 2049 7465 7261 746f 7220 656e 642c n, Iterator end, │ │ │ │ -000195d0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const L& label │ │ │ │ -00019600: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ -00019640: 6120 6964 3d22 6c30 3035 3635 2220 6e61 a id="l00565" na │ │ │ │ -00019650: 6d65 3d22 6c30 3035 3635 223e 3c2f 613e me="l00565"> │ │ │ │ -00019660: 3c73 7061 6e20 636c 6173 733d 226c 696e 565 // fi │ │ │ │ -000196a0: 6e64 2068 6967 6865 7374 206c 6162 656c nd highest label │ │ │ │ -000196b0: 2061 6d6f 6e67 2062 7261 6e63 6865 733c among branches< │ │ │ │ -000196c0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -000196e0: 6120 6964 3d22 6c30 3035 3636 2220 6e61 a id="l00566" na │ │ │ │ -000196f0: 6d65 3d22 6c30 3035 3636 223e 3c2f 613e me="l00566"> │ │ │ │ -00019700: 3c73 7061 6e20 636c 6173 733d 226c 696e 566 boost::opti │ │ │ │ -00019730: 6f6e 616c 266c 743b 4c26 6774 3b20 6869 onal<L> hi │ │ │ │ -00019740: 6768 6573 744c 6162 656c 3b3c 2f64 6976 ghestLabel;
    .
    567< │ │ │ │ -000197a0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> size_t nrChoices = 0; │ │ │ │ -000197e0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00019830: 2035 3638 3c2f 7370 616e 3e20 2020 203c 568 < │ │ │ │ -00019840: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00019850: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (Iterator it │ │ │ │ -00019870: 203d 2062 6567 696e 3b20 6974 2021 3d20 = begin; it != │ │ │ │ -00019880: 656e 643b 2069 742b 2b29 207b 3c2f 6469 end; it++) {.
    569 │ │ │ │ -000198e0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 if │ │ │ │ -00019910: 2028 6974 2d26 6774 3b72 6f6f 745f 2d26 (it->root_-& │ │ │ │ -00019920: 6774 3b69 734c 6561 6628 2929 3c2f 6469 gt;isLeaf()).
    570 │ │ │ │ -00019980: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ -00019990: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000199a0: 6f72 6466 6c6f 7722 3e63 6f6e 7469 6e75 ordflow">continu │ │ │ │ -000199b0: 653c 2f73 7061 6e3e 3b3c 2f64 6976 3e0a e;
    . │ │ │ │ -000199c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    572 │ │ │ │ -00019aa0: 2020 2020 2020 2020 626f 6f73 743a 3a64 boost::d │ │ │ │ -00019ab0: 796e 616d 6963 5f70 6f69 6e74 6572 5f63 ynamic_pointer_c │ │ │ │ -00019ac0: 6173 7426 6c74 3b63 6f6e 7374 2043 686f ast<const Cho │ │ │ │ -00019ad0: 6963 6526 6774 3b28 6974 2d26 6774 3b72 ice>(it->r │ │ │ │ -00019ae0: 6f6f 745f 293b 3c2f 6469 763e 0a3c 6469 oot_);
    .< │ │ │ │ -00019b00: 6120 6964 3d22 6c30 3035 3733 2220 6e61 a id="l00573" na │ │ │ │ -00019b10: 6d65 3d22 6c30 3035 3733 223e 3c2f 613e me="l00573"> │ │ │ │ -00019b20: 3c73 7061 6e20 636c 6173 733d 226c 696e 573 if (!hig │ │ │ │ -00019b70: 6865 7374 4c61 6265 6c20 7c7c 2063 2d26 hestLabel || c-& │ │ │ │ -00019b80: 6774 3b6c 6162 656c 2829 2026 6774 3b20 gt;label() > │ │ │ │ -00019b90: 2a68 6967 6865 7374 4c61 6265 6c29 207b *highestLabel) { │ │ │ │ -00019ba0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00019bf0: 2035 3734 3c2f 7370 616e 3e20 2020 2020 574 │ │ │ │ -00019c00: 2020 2068 6967 6865 7374 4c61 6265 6c2e highestLabel. │ │ │ │ -00019c10: 7265 7365 7428 632d 2667 743b 6c61 6265 reset(c->labe │ │ │ │ -00019c20: 6c28 2929 3b3c 2f64 6976 3e0a 3c64 6976 l());
    .
    < │ │ │ │ -00019c60: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00019c70: 6e6f 223e 2020 3537 353c 2f73 7061 6e3e no"> 575 │ │ │ │ -00019c80: 2020 2020 2020 2020 6e72 4368 6f69 6365 nrChoice │ │ │ │ -00019c90: 7320 3d20 632d 2667 743b 6e72 4368 6f69 s = c->nrChoi │ │ │ │ -00019ca0: 6365 7328 293b 3c2f 6469 763e 0a3c 6469 ces();
    .< │ │ │ │ -00019cc0: 6120 6964 3d22 6c30 3035 3736 2220 6e61 a id="l00576" na │ │ │ │ -00019cd0: 6d65 3d22 6c30 3035 3736 223e 3c2f 613e me="l00576"> │ │ │ │ -00019ce0: 3c73 7061 6e20 636c 6173 733d 226c 696e 576 }
    .< │ │ │ │ -00019d10: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00019d20: 3e3c 6120 6964 3d22 6c30 3035 3737 2220 > 577 }
    .< │ │ │ │ -00019d70: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00019d80: 3e3c 6120 6964 3d22 6c30 3035 3738 2220 > 578
    .
    579 │ │ │ │ -00019e20: 2020 203c 7370 616e 2063 6c61 7373 3d22 // if l │ │ │ │ -00019e40: 6162 656c 2069 7320 616c 7265 6164 7920 abel is already │ │ │ │ -00019e50: 696e 2063 6f72 7265 6374 206f 7264 6572 in correct order │ │ │ │ -00019e60: 2c20 6a75 7374 2070 7574 2074 6f67 6574 , just put toget │ │ │ │ -00019e70: 6865 7220 6120 6368 6f69 6365 206f 6e20 her a choice on │ │ │ │ -00019e80: 6c61 6265 6c3c 2f73 7061 6e3e 3c2f 6469 label.
    580 │ │ │ │ -00019ee0: 3c2f 7370 616e 3e20 2020 203c 7370 616e if ( │ │ │ │ -00019f10: 216e 7243 686f 6963 6573 207c 7c20 2168 !nrChoices || !h │ │ │ │ -00019f20: 6967 6865 7374 4c61 6265 6c20 7c7c 206c ighestLabel || l │ │ │ │ -00019f30: 6162 656c 2026 6774 3b20 2a68 6967 6865 abel > *highe │ │ │ │ -00019f40: 7374 4c61 6265 6c29 207b 3c2f 6469 763e stLabel) {
    │ │ │ │ -00019f50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00019f80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 581 auto cho │ │ │ │ -00019fd0: 6963 654f 6e4c 6162 656c 203d 2062 6f6f iceOnLabel = boo │ │ │ │ -00019fe0: 7374 3a3a 6d61 6b65 5f73 6861 7265 6426 st::make_shared& │ │ │ │ -00019ff0: 6c74 3b43 686f 6963 6526 6774 3b28 6c61 lt;Choice>(la │ │ │ │ -0001a000: 6265 6c2c 2065 6e64 202d 2062 6567 696e bel, end - begin │ │ │ │ -0001a010: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    582 │ │ │ │ -0001a070: 2020 203c 7370 616e 2063 6c61 7373 3d22 for │ │ │ │ -0001a090: 3c2f 7370 616e 3e20 2849 7465 7261 746f (Iterato │ │ │ │ -0001a0a0: 7220 6974 203d 2062 6567 696e 3b20 6974 r it = begin; it │ │ │ │ -0001a0b0: 2021 3d20 656e 643b 2069 742b 2b29 3c2f != end; it++).
    5 │ │ │ │ -0001a110: 3833 3c2f 7370 616e 3e20 2020 2020 2020 83 │ │ │ │ -0001a120: 2063 686f 6963 654f 6e4c 6162 656c 2d26 choiceOnLabel-& │ │ │ │ -0001a130: 6774 3b70 7573 685f 6261 636b 2869 742d gt;push_back(it- │ │ │ │ -0001a140: 2667 743b 726f 6f74 5f29 3b3c 2f64 6976 >root_);
    .
    584< │ │ │ │ -0001a1a0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return Choice::Uniq │ │ │ │ -0001a1e0: 7565 2863 686f 6963 654f 6e4c 6162 656c ue(choiceOnLabel │ │ │ │ -0001a1f0: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    585 │ │ │ │ -0001a250: 207d 203c 7370 616e 2063 6c61 7373 3d22 } els │ │ │ │ -0001a270: 653c 2f73 7061 6e3e 207b 3c2f 6469 763e e {
    │ │ │ │ -0001a280: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001a2b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 586 // Set up a new │ │ │ │ -0001a300: 2063 686f 6963 6520 6f6e 2074 6865 2068 choice on the h │ │ │ │ -0001a310: 6967 6865 7374 206c 6162 656c 3c2f 7370 ighest label
    .
    587 │ │ │ │ -0001a380: 2020 2020 3c73 7061 6e20 636c 6173 733d auto choiceOnHi │ │ │ │ -0001a3b0: 6768 6573 744c 6162 656c 203d 3c2f 6469 ghestLabel =.
    588 │ │ │ │ -0001a410: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ -0001a420: 2062 6f6f 7374 3a3a 6d61 6b65 5f73 6861 boost::make_sha │ │ │ │ -0001a430: 7265 6426 6c74 3b43 686f 6963 6526 6774 red<Choice> │ │ │ │ -0001a440: 3b28 2a68 6967 6865 7374 4c61 6265 6c2c ;(*highestLabel, │ │ │ │ -0001a450: 206e 7243 686f 6963 6573 293b 3c2f 6469 nrChoices);.
    589 │ │ │ │ -0001a4b0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 // now, for a │ │ │ │ -0001a4e0: 6c6c 2070 6f73 7369 626c 6520 7661 6c75 ll possible valu │ │ │ │ -0001a4f0: 6573 206f 6620 6869 6768 6573 744c 6162 es of highestLab │ │ │ │ -0001a500: 656c 3c2f 7370 616e 3e3c 2f64 6976 3e0a el
    . │ │ │ │ -0001a510: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    591 │ │ │ │ -0001a640: 2020 2020 2020 3c73 7061 6e20 636c 6173 // m │ │ │ │ -0001a660: 616b 6520 6120 6e65 7720 7365 7420 6f66 ake a new set of │ │ │ │ -0001a670: 2066 756e 6374 696f 6e73 2066 6f72 2063 functions for c │ │ │ │ -0001a680: 6f6d 706f 7369 6e67 2062 7920 6974 6572 omposing by iter │ │ │ │ -0001a690: 6174 696e 6720 6f76 6572 2074 6865 2067 ating over the g │ │ │ │ -0001a6a0: 6976 656e 3c2f 7370 616e 3e3c 2f64 6976 iven
    .
    592< │ │ │ │ -0001a700: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> // functions │ │ │ │ -0001a730: 2c20 616e 6420 7365 6c65 6374 696e 6720 , and selecting │ │ │ │ -0001a740: 7468 6520 6170 7072 6f70 7269 6174 6520 the appropriate │ │ │ │ -0001a750: 6272 616e 6368 2e3c 2f73 7061 6e3e 3c2f branch..
    5 │ │ │ │ -0001a7b0: 3933 3c2f 7370 616e 3e20 2020 2020 2020 93 │ │ │ │ -0001a7c0: 2073 7464 3a3a 7665 6374 6f72 266c 743b std::vector< │ │ │ │ -0001a7d0: 4465 6369 7369 6f6e 5472 6565 2667 743b DecisionTree> │ │ │ │ -0001a7e0: 2066 756e 6374 696f 6e73 3b3c 2f64 6976 functions;
    .
    594< │ │ │ │ -0001a840: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> for (Iterator it │ │ │ │ -0001a880: 3d20 6265 6769 6e3b 2069 7420 213d 2065 = begin; it != e │ │ │ │ -0001a890: 6e64 3b20 6974 2b2b 2920 7b3c 2f64 6976 nd; it++) {
    .
    595< │ │ │ │ -0001a8f0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -0001a900: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // by rest │ │ │ │ -0001a920: 7269 6374 696e 6720 7468 6520 696e 7075 ricting the inpu │ │ │ │ -0001a930: 7420 6675 6e63 7469 6f6e 7320 746f 2076 t functions to v │ │ │ │ -0001a940: 616c 7565 2069 2066 6f72 206c 6162 656c alue i for label │ │ │ │ -0001a950: 4265 6c6f 773c 2f73 7061 6e3e 3c2f 6469 Below.
    596 │ │ │ │ -0001a9b0: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ -0001a9c0: 2044 6563 6973 696f 6e54 7265 6520 6368 DecisionTree ch │ │ │ │ -0001a9d0: 6f73 656e 203d 2069 742d 2667 743b 6368 osen = it->ch │ │ │ │ -0001a9e0: 6f6f 7365 282a 6869 6768 6573 744c 6162 oose(*highestLab │ │ │ │ -0001a9f0: 656c 2c20 696e 6465 7829 3b3c 2f64 6976 el, index);
    .
    597< │ │ │ │ -0001aa50: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -0001aa60: 6675 6e63 7469 6f6e 732e 7075 7368 5f62 functions.push_b │ │ │ │ -0001aa70: 6163 6b28 6368 6f73 656e 293b 3c2f 6469 ack(chosen);.
    598 │ │ │ │ -0001aad0: 3c2f 7370 616e 3e20 2020 2020 2020 207d } │ │ │ │ -0001aae0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001ab30: 2035 3939 3c2f 7370 616e 3e20 2020 2020 599 │ │ │ │ -0001ab40: 2020 203c 7370 616e 2063 6c61 7373 3d22 // We t │ │ │ │ -0001ab60: 6865 6e20 7265 6375 7273 652c 2066 6f72 hen recurse, for │ │ │ │ -0001ab70: 2061 6c6c 2076 616c 7565 7320 6f66 2074 all values of t │ │ │ │ -0001ab80: 6865 2068 6967 6865 7374 206c 6162 656c he highest label │ │ │ │ -0001ab90: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -0001abb0: 3c61 2069 643d 226c 3030 3630 3022 206e 600 NodePt │ │ │ │ -0001ac00: 7220 6669 203d 2063 6f6d 706f 7365 2866 r fi = compose(f │ │ │ │ -0001ac10: 756e 6374 696f 6e73 2e62 6567 696e 2829 unctions.begin() │ │ │ │ -0001ac20: 2c20 6675 6e63 7469 6f6e 732e 656e 6428 , functions.end( │ │ │ │ -0001ac30: 292c 206c 6162 656c 293b 3c2f 6469 763e ), label);
    │ │ │ │ -0001ac40: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001ac70: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 601 cho │ │ │ │ -0001aca0: 6963 654f 6e48 6967 6865 7374 4c61 6265 iceOnHighestLabe │ │ │ │ -0001acb0: 6c2d 2667 743b 7075 7368 5f62 6163 6b28 l->push_back( │ │ │ │ -0001acc0: 6669 293b 3c2f 6469 763e 0a3c 6469 7620 fi);
    .
    602 │ │ │ │ -0001ad20: 2020 2020 207d 3c2f 6469 763e 0a3c 6469 }
    .< │ │ │ │ -0001ad40: 6120 6964 3d22 6c30 3036 3033 2220 6e61 a id="l00603" na │ │ │ │ -0001ad50: 6d65 3d22 6c30 3036 3033 223e 3c2f 613e me="l00603"> │ │ │ │ -0001ad60: 3c73 7061 6e20 636c 6173 733d 226c 696e 603 return C │ │ │ │ -0001adb0: 686f 6963 653a 3a55 6e69 7175 6528 6368 hoice::Unique(ch │ │ │ │ -0001adc0: 6f69 6365 4f6e 4869 6768 6573 744c 6162 oiceOnHighestLab │ │ │ │ -0001add0: 656c 293b 3c2f 6469 763e 0a3c 6469 7620 el);
    .
    604 │ │ │ │ -0001ae30: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    605 │ │ │ │ -0001ae90: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    606 .
    6 │ │ │ │ -0001af40: 3037 3c2f 7370 616e 3e3c 7370 616e 2063 07 │ │ │ │ -0001af60: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ -0001af70: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001af80: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001af90: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001afa0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ -0001afb0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -0001afd0: 6120 6964 3d22 6c30 3036 3038 2220 6e61 a id="l00608" na │ │ │ │ -0001afe0: 6d65 3d22 6c30 3036 3038 223e 3c2f 613e me="l00608"> │ │ │ │ -0001aff0: 3c73 7061 6e20 636c 6173 733d 226c 696e 608 // &quo │ │ │ │ -0001b030: 743b 6372 6561 7465 2671 756f 743b 2069 t;create" i │ │ │ │ -0001b040: 7320 6120 6269 7420 6f66 2061 2063 6f6d s a bit of a com │ │ │ │ -0001b050: 706c 6963 6174 6564 2074 6869 6e67 2c20 plicated thing, │ │ │ │ -0001b060: 6275 7420 7665 7279 2075 7365 6675 6c2e but very useful. │ │ │ │ -0001b070: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -0001b090: 3c61 2069 643d 226c 3030 3630 3922 206e 609 // It │ │ │ │ -0001b0f0: 7461 6b65 7320 6120 7261 6e67 6520 6f66 takes a range of │ │ │ │ -0001b100: 206c 6162 656c 7320 616e 6420 6120 636f labels and a co │ │ │ │ -0001b110: 7272 6573 706f 6e64 696e 6720 7261 6e67 rresponding rang │ │ │ │ -0001b120: 6520 6f66 2076 616c 7565 732c 3c2f 7370 e of values,
    .
    610 │ │ │ │ -0001b190: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // and cre │ │ │ │ -0001b1b0: 6174 6573 2061 2064 6563 6973 696f 6e20 ates a decision │ │ │ │ -0001b1c0: 7472 6565 2c20 6173 2066 6f6c 6c6f 7773 tree, as follows │ │ │ │ -0001b1d0: 3a3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c :
    .< │ │ │ │ -0001b1e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001b1f0: 3e3c 6120 6964 3d22 6c30 3036 3131 2220 > 611 // - │ │ │ │ -0001b250: 6966 2074 6865 7265 2069 7320 6f6e 6c79 if there is only │ │ │ │ -0001b260: 206f 6e65 206c 6162 656c 2c20 6372 6561 one label, crea │ │ │ │ -0001b270: 7465 7320 6120 6368 6f69 6365 206e 6f64 tes a choice nod │ │ │ │ -0001b280: 6520 7769 7468 2076 616c 7565 7320 696e e with values in │ │ │ │ -0001b290: 206c 6561 7665 733c 2f73 7061 6e3e 3c2f leaves.
    6 │ │ │ │ -0001b2f0: 3132 3c2f 7370 616e 3e20 203c 7370 616e 12 // - otherwise, │ │ │ │ -0001b320: 2069 7420 6576 656e 6c79 2073 706c 6974 it evenly split │ │ │ │ -0001b330: 7320 7570 2074 6865 2072 616e 6765 206f s up the range o │ │ │ │ -0001b340: 6620 7661 6c75 6573 2061 6e64 2063 7265 f values and cre │ │ │ │ -0001b350: 6174 6573 2061 2074 7265 6520 666f 723c ates a tree for< │ │ │ │ -0001b360: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -0001b380: 6120 6964 3d22 6c30 3036 3133 2220 6e61 a id="l00613" na │ │ │ │ -0001b390: 6d65 3d22 6c30 3036 3133 223e 3c2f 613e me="l00613"> │ │ │ │ -0001b3a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 613 // ea │ │ │ │ -0001b3e0: 6368 2073 7562 2d72 616e 6765 2c20 616e ch sub-range, an │ │ │ │ -0001b3f0: 6420 6173 7369 676e 7320 7468 6174 2074 d assigns that t │ │ │ │ -0001b400: 7265 6520 746f 2066 6972 7374 206c 6162 ree to first lab │ │ │ │ -0001b410: 656c 2623 3339 3b73 2063 686f 6963 6573 el's choices │ │ │ │ -0001b420: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -0001b440: 3c61 2069 643d 226c 3030 3631 3422 206e 614 // Exa │ │ │ │ -0001b4a0: 6d70 6c65 3a3c 2f73 7061 6e3e 3c2f 6469 mple:.
    615 │ │ │ │ -0001b500: 3c2f 7370 616e 3e20 203c 7370 616e 2063 / │ │ │ │ -0001b520: 2f20 6372 6561 7465 285b 4220 415d 2c5b / create([B A],[ │ │ │ │ -0001b530: 3120 3220 3320 345d 2920 776f 756c 6420 1 2 3 4]) would │ │ │ │ -0001b540: 6361 6c6c 3c2f 7370 616e 3e3c 2f64 6976 call
    .
    616< │ │ │ │ -0001b5a0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ -0001b5c0: 2020 2063 7265 6174 6528 5b41 5d2c 5b31 create([A],[1 │ │ │ │ -0001b5d0: 2032 5d29 3c2f 7370 616e 3e3c 2f64 6976 2])
    .
    617< │ │ │ │ -0001b630: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ -0001b650: 2020 2063 7265 6174 6528 5b41 5d2c 5b33 create([A],[3 │ │ │ │ -0001b660: 2034 5d29 3c2f 7370 616e 3e3c 2f64 6976 4])
    .
    618< │ │ │ │ -0001b6c0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ -0001b6e0: 2061 6e64 2070 726f 6475 6365 3c2f 7370 and produce
    .
    619 │ │ │ │ -0001b750: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // B=0
    .
    620 │ │ │ │ -0001b7d0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // A=0: 1 │ │ │ │ -0001b7f0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -0001b810: 3c61 2069 643d 226c 3030 3632 3122 206e 621 // A= │ │ │ │ -0001b870: 313a 2032 3c2f 7370 616e 3e3c 2f64 6976 1: 2
    .
    622< │ │ │ │ -0001b8d0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ -0001b8f0: 2042 3d31 3c2f 7370 616e 3e3c 2f64 6976 B=1
    .
    623< │ │ │ │ -0001b950: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ -0001b970: 2020 413d 303a 2033 3c2f 7370 616e 3e3c A=0: 3< │ │ │ │ -0001b980: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0001b9d0: 3632 343c 2f73 7061 6e3e 2020 3c73 7061 624 // A=1: 4
    .
    625 │ │ │ │ -0001ba60: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // Note, t │ │ │ │ -0001ba80: 6872 6f75 6768 2074 6865 206d 6167 6963 hrough the magic │ │ │ │ -0001ba90: 206f 6620 2671 756f 743b 636f 6d70 6f73 of "compos │ │ │ │ -0001baa0: 6526 7175 6f74 3b2c 2063 7265 6174 6528 e", create( │ │ │ │ -0001bab0: 5b41 2042 5d2c 5b31 2033 2032 2034 5d29 [A B],[1 3 2 4]) │ │ │ │ -0001bac0: 2077 696c 6c20 7072 6f64 7563 653c 2f73 will produce
    .
    626 │ │ │ │ -0001bb30: 203c 7370 616e 2063 6c61 7373 3d22 636f // exactl │ │ │ │ -0001bb50: 7920 7468 6520 7361 6d65 2074 7265 6520 y the same tree │ │ │ │ -0001bb60: 6173 2061 626f 7665 3a20 7468 6520 6869 as above: the hi │ │ │ │ -0001bb70: 6768 6573 7420 6c61 6265 6c20 6973 2061 ghest label is a │ │ │ │ -0001bb80: 6c77 6179 7320 7468 6520 726f 6f74 2e3c lways the root.< │ │ │ │ -0001bb90: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -0001bbb0: 6120 6964 3d22 6c30 3036 3237 2220 6e61 a id="l00627" na │ │ │ │ -0001bbc0: 6d65 3d22 6c30 3036 3237 223e 3c2f 613e me="l00627"> │ │ │ │ -0001bbd0: 3c73 7061 6e20 636c 6173 733d 226c 696e 627 // Howe │ │ │ │ -0001bc10: 7665 722c 2069 7420 7769 6c6c 2062 6520 ver, it will be │ │ │ │ -0001bc20: 2a77 6179 2a20 6661 7374 6572 2069 6620 *way* faster if │ │ │ │ -0001bc30: 6c61 6265 6c73 2061 7265 2067 6976 656e labels are given │ │ │ │ -0001bc40: 2068 6967 6865 7374 2074 6f20 6c6f 7765 highest to lowe │ │ │ │ -0001bc50: 7374 2e3c 2f73 7061 6e3e 3c2f 6469 763e st.
    │ │ │ │ -0001bc60: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001bc90: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 628 tem │ │ │ │ -0001bcd0: 706c 6174 653c 2f73 7061 6e3e 266c 743b plate< │ │ │ │ -0001bce0: 3c73 7061 6e20 636c 6173 733d 226b 6579 typename L, t │ │ │ │ -0001bd20: 7970 656e 616d 653c 2f73 7061 6e3e 2059 ypename Y │ │ │ │ -0001bd30: 2667 743b 3c2f 6469 763e 0a3c 6469 7620 >
    .
    629 │ │ │ │ -0001bd90: 203c 7370 616e 2063 6c61 7373 3d22 6b65 template< │ │ │ │ -0001bdb0: 2f73 7061 6e3e 266c 743b 3c73 7061 6e20 /span>< │ │ │ │ -0001bdd0: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -0001bde0: 4974 2c20 3c73 7061 6e20 636c 6173 733d It, typena │ │ │ │ -0001be00: 6d65 3c2f 7370 616e 3e20 5661 6c75 6549 me ValueI │ │ │ │ -0001be10: 7426 6774 3b3c 2f64 6976 3e0a 3c64 6976 t>
    .
    ..
    631 │ │ │ │ -0001c070: 2020 2020 4974 2062 6567 696e 2c20 4974 It begin, It │ │ │ │ -0001c080: 2065 6e64 2c20 5661 6c75 6549 7420 6265 end, ValueIt be │ │ │ │ -0001c090: 6769 6e59 2c20 5661 6c75 6549 7420 656e ginY, ValueIt en │ │ │ │ -0001c0a0: 6459 293c 7370 616e 2063 6c61 7373 3d22 dY) const │ │ │ │ -0001c0c0: 3c2f 7370 616e 3e7b 3c2f 6469 763e 0a3c {
    .< │ │ │ │ -0001c0d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001c0e0: 3e3c 6120 6964 3d22 6c30 3036 3332 2220 > 632 // │ │ │ │ -0001c140: 6765 7420 6372 7563 6961 6c20 636f 756e get crucial coun │ │ │ │ -0001c150: 7473 3c2f 7370 616e 3e3c 2f64 6976 3e0a ts
    . │ │ │ │ -0001c160: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -0001c190: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 633 size_t │ │ │ │ -0001c1e0: 6e72 4368 6f69 6365 7320 3d20 6265 6769 nrChoices = begi │ │ │ │ -0001c1f0: 6e2d 2667 743b 7365 636f 6e64 3b3c 2f64 n->second;.
    63 │ │ │ │ -0001c250: 343c 2f73 7061 6e3e 2020 2020 3c73 7061 4 size_t size = endY │ │ │ │ -0001c290: 2d20 6265 6769 6e59 3b3c 2f64 6976 3e0a - beginY;
    . │ │ │ │ -0001c2a0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ -0001c330: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0001c340: 6e6f 223e 2020 3633 363c 2f73 7061 6e3e no"> 636 │ │ │ │ -0001c350: 2020 2020 3c73 7061 6e20 636c 6173 733d // Fin │ │ │ │ -0001c370: 6420 7468 6520 6e65 7874 206b 6579 2074 d the next key t │ │ │ │ -0001c380: 6f20 776f 726b 206f 6e3c 2f73 7061 6e3e o work on │ │ │ │ -0001c390: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001c3e0: 2036 3337 3c2f 7370 616e 3e20 2020 2049 637 I │ │ │ │ -0001c3f0: 7420 6c61 6265 6c43 203d 2062 6567 696e t labelC = begin │ │ │ │ -0001c400: 202b 2031 3b3c 2f64 6976 3e0a 3c64 6976 + 1;
    .
    < │ │ │ │ -0001c440: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0001c450: 6e6f 223e 2020 3633 383c 2f73 7061 6e3e no"> 638 │ │ │ │ -0001c460: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ -0001c480: 3c2f 7370 616e 3e20 286c 6162 656c 4320 (labelC │ │ │ │ -0001c490: 3d3d 2065 6e64 2920 7b3c 2f64 6976 3e0a == end) {
    . │ │ │ │ -0001c4a0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .< │ │ │ │ -0001c550: 6120 6964 3d22 6c30 3036 3430 2220 6e61 a id="l00640" na │ │ │ │ -0001c560: 6d65 3d22 6c30 3036 3430 223e 3c2f 613e me="l00640"> │ │ │ │ -0001c570: 3c73 7061 6e20 636c 6173 733d 226c 696e 640 // │ │ │ │ -0001c5b0: 4372 6561 7465 2061 2073 696d 706c 6520 Create a simple │ │ │ │ -0001c5c0: 6368 6f69 6365 206e 6f64 6520 7769 7468 choice node with │ │ │ │ -0001c5d0: 2076 616c 7565 7320 6173 206c 6561 7665 values as leave │ │ │ │ -0001c5e0: 732e 3c2f 7370 616e 3e3c 2f64 6976 3e0a s.
    . │ │ │ │ -0001c5f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    642 │ │ │ │ -0001c6e0: 2020 2020 2020 7374 643a 3a63 6f75 7420 std::cout │ │ │ │ -0001c6f0: 266c 743b 266c 743b 203c 7370 616e 2063 << "Tryin │ │ │ │ -0001c720: 6720 746f 2063 7265 6174 6520 4444 206f g to create DD o │ │ │ │ -0001c730: 6e20 2671 756f 743b 3c2f 7370 616e 3e20 n " │ │ │ │ -0001c740: 266c 743b 266c 743b 2062 6567 696e 2d26 << begin-& │ │ │ │ -0001c750: 6774 3b66 6972 7374 2026 6c74 3b26 6c74 gt;first << │ │ │ │ -0001c760: 3b20 7374 643a 3a65 6e64 6c3b 3c2f 6469 ; std::endl;.
    643 │ │ │ │ -0001c7c0: 3c2f 7370 616e 3e20 2020 2020 2020 2073 s │ │ │ │ -0001c7d0: 7464 3a3a 636f 7574 2026 6c74 3b26 6c74 td::cout << │ │ │ │ -0001c7e0: 3b20 626f 6f73 743a 3a66 6f72 6d61 7428 ; boost::format( │ │ │ │ -0001c7f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001c840: 2036 3434 3c2f 7370 616e 3e20 2020 2020 644 │ │ │ │ -0001c850: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0001c860: 2020 2020 3c73 7061 6e20 636c 6173 733d │ │ │ │ -0001c880: 2671 756f 743b 4465 6369 7369 6f6e 5472 "DecisionTr │ │ │ │ -0001c890: 6565 3a3a 6372 6561 7465 3a20 6578 7065 ee::create: expe │ │ │ │ -0001c8a0: 6374 6564 2025 6420 7661 6c75 6573 2062 cted %d values b │ │ │ │ -0001c8b0: 7574 2067 6f74 2025 6420 2671 756f 743b ut got %d " │ │ │ │ -0001c8c0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -0001c8e0: 3c61 2069 643d 226c 3030 3634 3522 206e 645 │ │ │ │ -0001c930: 2020 2020 2020 2020 2020 203c 7370 616e "ins │ │ │ │ -0001c960: 7465 6164 2671 756f 743b 3c2f 7370 616e tead") %
    .
    646 │ │ │ │ -0001c9d0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0001c9e0: 2020 2020 2020 2020 6e72 4368 6f69 6365 nrChoice │ │ │ │ -0001c9f0: 7320 2520 7369 7a65 3c2f 6469 763e 0a3c s % size
    .< │ │ │ │ -0001ca00: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001ca10: 3e3c 6120 6964 3d22 6c30 3036 3437 2220 > 647 │ │ │ │ -0001ca60: 2020 2020 2026 6c74 3b26 6c74 3b20 7374 << st │ │ │ │ -0001ca70: 643a 3a65 6e64 6c3b 3c2f 6469 763e 0a3c d::endl;
    .< │ │ │ │ -0001ca80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001ca90: 3e3c 6120 6964 3d22 6c30 3036 3438 2220 > 648 throw std::invalid_a │ │ │ │ -0001cb10: 7267 756d 656e 7428 3c73 7061 6e20 636c rgument("Decisi │ │ │ │ -0001cb40: 6f6e 5472 6565 3a3a 6372 6561 7465 2069 onTree::create i │ │ │ │ -0001cb50: 6e76 616c 6964 2061 7267 756d 656e 7426 nvalid argument& │ │ │ │ -0001cb60: 7175 6f74 3b3c 2f73 7061 6e3e 293b 3c2f quot;);.
    6 │ │ │ │ -0001cbc0: 3439 3c2f 7370 616e 3e20 2020 2020 207d 49 } │ │ │ │ -0001cbd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001cc20: 2036 3530 3c2f 7370 616e 3e20 2020 2020 650 │ │ │ │ -0001cc30: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto choice = boos │ │ │ │ -0001cc60: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ -0001cc70: 743b 4368 6f69 6365 2667 743b 2862 6567 t;Choice>(beg │ │ │ │ -0001cc80: 696e 2d26 6774 3b66 6972 7374 2c20 656e in->first, en │ │ │ │ -0001cc90: 6459 202d 2062 6567 696e 5929 3b3c 2f64 dY - beginY);.
    65 │ │ │ │ -0001ccf0: 313c 2f73 7061 6e3e 2020 2020 2020 3c73 1 for (ValueIt y = │ │ │ │ -0001cd30: 6265 6769 6e59 3b20 7920 213d 2065 6e64 beginY; y != end │ │ │ │ -0001cd40: 593b 2079 2b2b 293c 2f64 6976 3e0a 3c64 Y; y++)
    . │ │ │ │ -0001cd60: 3c61 2069 643d 226c 3030 3635 3222 206e 652 choice │ │ │ │ -0001cdb0: 2d26 6774 3b70 7573 685f 6261 636b 283c ->push_back(< │ │ │ │ -0001cdc0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0001cdd0: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ -0001cde0: 6130 3237 3936 2e68 746d 6c23 6133 3661 a02796.html#a36a │ │ │ │ -0001cdf0: 3938 6636 3162 3062 3764 6630 3432 3432 98f61b0b7df04242 │ │ │ │ -0001ce00: 3762 3133 3262 3331 3364 3766 6422 3e4e 7b132b313d7fd">N │ │ │ │ -0001ce10: 6f64 6550 7472 3c2f 613e 283c 7370 616e odePtr(new Leaf(*y)));
    .< │ │ │ │ -0001ce80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001ce90: 3e3c 6120 6964 3d22 6c30 3036 3533 2220 > 653 return │ │ │ │ -0001cf00: 2043 686f 6963 653a 3a55 6e69 7175 6528 Choice::Unique( │ │ │ │ -0001cf10: 6368 6f69 6365 293b 3c2f 6469 763e 0a3c choice);
    .< │ │ │ │ -0001cf20: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001cf30: 3e3c 6120 6964 3d22 6c30 3036 3534 2220 > 654 }
    .< │ │ │ │ -0001cf80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001cf90: 3e3c 6120 6964 3d22 6c30 3036 3535 2220 > 655
    .
    656 │ │ │ │ -0001d030: 2020 203c 7370 616e 2063 6c61 7373 3d22 // Recu │ │ │ │ -0001d050: 7273 6976 6520 6361 7365 3a20 7065 7266 rsive case: perf │ │ │ │ -0001d060: 6f72 6d20 2671 756f 743b 5368 616e 6e6f orm "Shanno │ │ │ │ -0001d070: 6e20 6578 7061 6e73 696f 6e26 7175 6f74 n expansion" │ │ │ │ -0001d080: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
    .< │ │ │ │ -0001d090: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001d0a0: 3e3c 6120 6964 3d22 6c30 3036 3537 2220 > 657 // │ │ │ │ -0001d100: 4372 6561 7465 7320 6f6e 6520 7472 6565 Creates one tree │ │ │ │ -0001d110: 2028 692e 652e 2c66 756e 6374 696f 6e29 (i.e.,function) │ │ │ │ -0001d120: 2066 6f72 2065 6163 6820 6368 6f69 6365 for each choice │ │ │ │ -0001d130: 206f 6620 6375 7272 656e 7420 6b65 793c of current key< │ │ │ │ -0001d140: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -0001d160: 6120 6964 3d22 6c30 3036 3538 2220 6e61 a id="l00658" na │ │ │ │ -0001d170: 6d65 3d22 6c30 3036 3538 223e 3c2f 613e me="l00658"> │ │ │ │ -0001d180: 3c73 7061 6e20 636c 6173 733d 226c 696e 658 // by │ │ │ │ -0001d1c0: 2063 616c 6c69 6e67 2063 7265 6174 6520 calling create │ │ │ │ -0001d1d0: 7265 6375 7273 6976 656c 792c 2061 6e64 recursively, and │ │ │ │ -0001d1e0: 2074 6865 6e20 7075 7473 2074 6865 6d20 then puts them │ │ │ │ -0001d1f0: 616c 6c20 746f 6765 7468 6572 2e3c 2f73 all together.
    .
    659 │ │ │ │ -0001d260: 2020 2073 7464 3a3a 7665 6374 6f72 266c std::vector&l │ │ │ │ -0001d270: 743b 4465 6369 7369 6f6e 5472 6565 2667 t;DecisionTree&g │ │ │ │ -0001d280: 743b 2066 756e 6374 696f 6e73 3b3c 2f64 t; functions;.
    66 │ │ │ │ -0001d2e0: 303c 2f73 7061 6e3e 2020 2020 3c73 7061 0 size_t split = │ │ │ │ -0001d370: 2073 697a 6520 2f20 6e72 4368 6f69 6365 size / nrChoice │ │ │ │ -0001d380: 733b 3c2f 6469 763e 0a3c 6469 7620 636c s;
    .
    661 │ │ │ │ -0001d3e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 for (size_t i │ │ │ │ -0001d430: 203d 2030 3b20 6920 266c 743b 206e 7243 = 0; i < nrC │ │ │ │ -0001d440: 686f 6963 6573 3b20 692b 2b2c 2062 6567 hoices; i++, beg │ │ │ │ -0001d450: 696e 5920 2b3d 203c 6120 636c 6173 733d inY += split) {
    .
    662 │ │ │ │ -0001d510: 2020 2020 203c 6120 636c 6173 733d 2263 NodePtr │ │ │ │ -0001d570: 2066 203d 2063 7265 6174 6526 6c74 3b49 f = create<I │ │ │ │ -0001d580: 742c 2056 616c 7565 4974 2667 743b 286c t, ValueIt>(l │ │ │ │ -0001d590: 6162 656c 432c 2065 6e64 2c20 6265 6769 abelC, end, begi │ │ │ │ -0001d5a0: 6e59 2c20 6265 6769 6e59 202b 203c 6120 nY, beginY + sp │ │ │ │ -0001d600: 6c69 743c 2f61 3e29 3b3c 2f64 6976 3e0a lit);
    . │ │ │ │ -0001d610: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    664 │ │ │ │ -0001d6e0: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    665 │ │ │ │ -0001d740: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ -0001d760: 7572 6e3c 2f73 7061 6e3e 2063 6f6d 706f urn compo │ │ │ │ -0001d770: 7365 2866 756e 6374 696f 6e73 2e62 6567 se(functions.beg │ │ │ │ -0001d780: 696e 2829 2c20 6675 6e63 7469 6f6e 732e in(), functions. │ │ │ │ -0001d790: 656e 6428 292c 2062 6567 696e 2d26 6774 end(), begin-> │ │ │ │ -0001d7a0: 3b66 6972 7374 293b 3c2f 6469 763e 0a3c ;first);
    .< │ │ │ │ -0001d7b0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001d7c0: 3e3c 6120 6964 3d22 6c30 3036 3636 2220 > 666 }
    ..
    66 │ │ │ │ -0001d860: 373c 2f73 7061 6e3e 203c 2f64 6976 3e0a 7
    . │ │ │ │ -0001d870: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -0001d980: 2020 3636 393c 2f73 7061 6e3e 2020 3c73 669 template <ty │ │ │ │ -0001d9d0: 7065 6e61 6d65 3c2f 7370 616e 3e20 4c2c pename L, │ │ │ │ -0001d9e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ -0001da00: 2f73 7061 6e3e 2059 2667 743b 3c2f 6469 /span> Y>.
    670 │ │ │ │ -0001da60: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ -0001da80: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ -0001da90: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ -0001dab0: 653c 2f73 7061 6e3e 204d 2c20 3c73 7061 e M, typename X>
    ..
    671 typenam │ │ │ │ -0001dbf0: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e Decisio │ │ │ │ -0001dc50: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ -0001dc60: 3b3a 3a4e 6f64 6550 7472 3c2f 613e 203c ;::NodePtr < │ │ │ │ -0001dc70: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0001dc80: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ -0001dc90: 2261 3032 3739 362e 6874 6d6c 2361 3134 "a02796.html#a14 │ │ │ │ -0001dca0: 3936 3661 3365 3231 6334 3239 3063 3430 966a3e21c4290c40 │ │ │ │ -0001dcb0: 6133 3338 3833 3133 3437 3862 6638 223e a3388313478bf8"> │ │ │ │ -0001dcc0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ -0001dcd0: 4c2c 2059 2667 743b 3a3a 636f 6e76 6572 L, Y>::conver │ │ │ │ -0001dce0: 7446 726f 6d3c 2f61 3e28 3c2f 6469 763e tFrom(
    │ │ │ │ -0001dcf0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    │ │ │ │ -0001de60: 2020 3637 333c 2f73 7061 6e3e 2020 2020 673 │ │ │ │ -0001de70: 2020 7374 643a 3a66 756e 6374 696f 6e26 std::function& │ │ │ │ -0001de80: 6c74 3b4c 283c 7370 616e 2063 6c61 7373 lt;L(const │ │ │ │ -0001dea0: 3c2f 7370 616e 3e20 4d26 616d 703b 2926 M&)& │ │ │ │ -0001deb0: 6774 3b20 4c5f 6f66 5f4d 2c3c 2f64 6976 gt; L_of_M,
    .
    674< │ │ │ │ -0001df10: 2f73 7061 6e3e 2020 2020 2020 7374 643a /span> std: │ │ │ │ -0001df20: 3a66 756e 6374 696f 6e26 6c74 3b59 283c :function<Y(< │ │ │ │ -0001df30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0001df40: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const X&)> Y_ │ │ │ │ -0001df60: 6f66 5f58 293c 7370 616e 2063 6c61 7373 of_X) cons │ │ │ │ -0001df80: 7420 3c2f 7370 616e 3e7b 3c2f 6469 763e t {
    │ │ │ │ -0001df90: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001dfc0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 675 u │ │ │ │ -0001e000: 7369 6e67 203c 2f73 7061 6e3e 4c59 203d sing LY = │ │ │ │ -0001e010: 203c 6120 636c 6173 733d 2263 6f64 6520 Dec │ │ │ │ -0001e040: 6973 696f 6e54 7265 6526 6c74 3b4c 2c20 isionTree<L, │ │ │ │ -0001e050: 5926 6774 3b3c 2f61 3e3b 3c2f 6469 763e Y>;
    │ │ │ │ -0001e060: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -0001e0d0: 6120 6964 3d22 6c30 3036 3737 2220 6e61 a id="l00677" na │ │ │ │ -0001e0e0: 6d65 3d22 6c30 3036 3737 223e 3c2f 613e me="l00677"> │ │ │ │ -0001e0f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 677 // Ug │ │ │ │ -0001e130: 6c69 6e65 7373 2062 656c 6f77 2062 6563 liness below bec │ │ │ │ -0001e140: 6175 7365 2061 7070 6172 656e 746c 7920 ause apparently │ │ │ │ -0001e150: 7765 2063 616e 2623 3339 3b74 2068 6176 we can't hav │ │ │ │ -0001e160: 6520 7465 6d70 6c61 7465 6420 7669 7274 e templated virt │ │ │ │ -0001e170: 7561 6c3c 2f73 7061 6e3e 3c2f 6469 763e ual
    │ │ │ │ -0001e180: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001e1b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 678 / │ │ │ │ -0001e1f0: 2f20 6675 6e63 7469 6f6e 732e 3c2f 7370 / functions.
    .
    679 │ │ │ │ -0001e260: 2020 3c73 7061 6e20 636c 6173 733d 2263 // If le │ │ │ │ -0001e280: 6166 2c20 6170 706c 7920 756e 6172 7920 af, apply unary │ │ │ │ -0001e290: 636f 6e76 6572 7369 6f6e 2026 7175 6f74 conversion " │ │ │ │ -0001e2a0: 3b6f 7026 7175 6f74 3b20 616e 6420 6372 ;op" and cr │ │ │ │ -0001e2b0: 6561 7465 2061 2075 6e69 7175 6520 6c65 eate a unique le │ │ │ │ -0001e2c0: 6166 2e3c 2f73 7061 6e3e 3c2f 6469 763e af.
    │ │ │ │ -0001e2d0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001e300: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 680 u │ │ │ │ -0001e340: 7369 6e67 203c 2f73 7061 6e3e 4d58 4c65 sing MXLe │ │ │ │ -0001e350: 6166 203d 203c 7370 616e 2063 6c61 7373 af = typen │ │ │ │ -0001e370: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decision │ │ │ │ -0001e3b0: 5472 6565 266c 743b 4d2c 2058 2667 743b Tree<M, X> │ │ │ │ -0001e3c0: 3a3a 4c65 6166 3c2f 613e 3b3c 2f64 6976 ::Leaf;
    .
    681< │ │ │ │ -0001e420: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> if (< │ │ │ │ -0001e450: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0001e460: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ -0001e470: 206c 6561 6620 3d20 626f 6f73 743a 3a64 leaf = boost::d │ │ │ │ -0001e480: 796e 616d 6963 5f70 6f69 6e74 6572 5f63 ynamic_pointer_c │ │ │ │ -0001e490: 6173 7426 6c74 3b63 6f6e 7374 204d 584c ast<const MXL │ │ │ │ -0001e4a0: 6561 6626 6774 3b28 6629 2920 7b3c 2f64 eaf>(f)) {.
    68 │ │ │ │ -0001e500: 323c 2f73 7061 6e3e 2020 2020 2020 3c73 2 return NodePtr(new │ │ │ │ -0001e5e0: 4c65 6166 3c2f 613e 2859 5f6f 665f 5828 Leaf(Y_of_X( │ │ │ │ -0001e5f0: 6c65 6166 2d26 6774 3b63 6f6e 7374 616e leaf->constan │ │ │ │ -0001e600: 7428 2929 2c20 6c65 6166 2d26 6774 3b6e t()), leaf->n │ │ │ │ -0001e610: 7241 7373 6967 6e6d 656e 7473 2829 2929 rAssignments())) │ │ │ │ -0001e620: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0001e670: 2020 3638 333c 2f73 7061 6e3e 2020 2020 683 │ │ │ │ -0001e680: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ -0001e6d0: 2020 3638 343c 2f73 7061 6e3e 203c 2f64 684 .
    68 │ │ │ │ -0001e730: 353c 2f73 7061 6e3e 2020 2020 3c73 7061 5 // Check if Ch │ │ │ │ -0001e760: 6f69 6365 3c2f 7370 616e 3e3c 2f64 6976 oice
    .
    686< │ │ │ │ -0001e7c0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> │ │ │ │ -0001e7e0: 7573 696e 6720 3c2f 7370 616e 3e4d 5843 using MXC │ │ │ │ -0001e7f0: 686f 6963 6520 3d20 3c73 7061 6e20 636c hoice = ty │ │ │ │ -0001e810: 7065 6e61 6d65 3c2f 7370 616e 3e20 3c61 pename Decis │ │ │ │ -0001e850: 696f 6e54 7265 6526 6c74 3b4d 2c20 5826 ionTree<M, X& │ │ │ │ -0001e860: 6774 3b3a 3a43 686f 6963 653c 2f61 3e3b gt;::Choice; │ │ │ │ -0001e870: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001e8c0: 2036 3837 3c2f 7370 616e 3e20 2020 203c 687 < │ │ │ │ -0001e8d0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0001e8e0: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ -0001e8f0: 2063 686f 6963 6520 3d20 626f 6f73 743a choice = boost: │ │ │ │ -0001e900: 3a64 796e 616d 6963 5f70 6f69 6e74 6572 :dynamic_pointer │ │ │ │ -0001e910: 5f63 6173 7426 6c74 3b63 6f6e 7374 204d _cast<const M │ │ │ │ -0001e920: 5843 686f 6963 6526 6774 3b28 6629 3b3c XChoice>(f);< │ │ │ │ -0001e930: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0001e980: 3638 383c 2f73 7061 6e3e 2020 2020 3c73 688 if (!choice) throw std::invalid_ │ │ │ │ -0001e9f0: 6172 6775 6d65 6e74 283c 2f64 6976 3e0a argument(
    . │ │ │ │ -0001ea00: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 . │ │ │ │ -0001ead0: 3c61 2069 643d 226c 3030 3639 3022 206e 690
    .
    691 │ │ │ │ -0001eb70: 2020 3c73 7061 6e20 636c 6173 733d 2263 // get n │ │ │ │ -0001eb90: 6577 206c 6162 656c 3c2f 7370 616e 3e3c ew label< │ │ │ │ -0001eba0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0001ebf0: 3639 323c 2f73 7061 6e3e 2020 2020 3c73 692 const │ │ │ │ -0001ec20: 204d 206f 6c64 4c61 6265 6c20 3d20 6368 M oldLabel = ch │ │ │ │ -0001ec30: 6f69 6365 2d26 6774 3b6c 6162 656c 2829 oice->label() │ │ │ │ -0001ec40: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0001ec90: 2020 3639 333c 2f73 7061 6e3e 2020 2020 693 │ │ │ │ -0001eca0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const L newLabel = │ │ │ │ -0001ecd0: 4c5f 6f66 5f4d 286f 6c64 4c61 6265 6c29 L_of_M(oldLabel) │ │ │ │ -0001ece0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -0001ed30: 2020 3639 343c 2f73 7061 6e3e 203c 2f64 694 .
    69 │ │ │ │ -0001ed90: 353c 2f73 7061 6e3e 2020 2020 3c73 7061 5 // put togethe │ │ │ │ -0001edc0: 7220 7669 6120 5368 616e 6e6f 6e20 6578 r via Shannon ex │ │ │ │ -0001edd0: 7061 6e73 696f 6e20 6f74 6865 7277 6973 pansion otherwis │ │ │ │ -0001ede0: 6520 6e6f 7420 736f 7274 6564 2e3c 2f73 e not sorted.
    .
    696 │ │ │ │ -0001ee50: 2020 2073 7464 3a3a 7665 6374 6f72 266c std::vector&l │ │ │ │ -0001ee60: 743b 4c59 2667 743b 2066 756e 6374 696f t;LY> functio │ │ │ │ -0001ee70: 6e73 3b3c 2f64 6976 3e0a 3c64 6976 2063 ns;
    .
    697 │ │ │ │ -0001eed0: 2020 3c73 7061 6e20 636c 6173 733d 226b for< │ │ │ │ -0001eef0: 2f73 7061 6e3e 2028 3c73 7061 6e20 636c /span> (au │ │ │ │ -0001ef10: 746f 3c2f 7370 616e 3e26 616d 703b 2661 to&&a │ │ │ │ -0001ef20: 6d70 3b20 6272 616e 6368 203a 2063 686f mp; branch : cho │ │ │ │ -0001ef30: 6963 652d 2667 743b 6272 616e 6368 6573 ice->branches │ │ │ │ -0001ef40: 2829 2920 7b3c 2f64 6976 3e0a 3c64 6976 ()) {
    .
    < │ │ │ │ -0001ef80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0001ef90: 6e6f 223e 2020 3639 383c 2f73 7061 6e3e no"> 698 │ │ │ │ -0001efa0: 2020 2020 2020 6675 6e63 7469 6f6e 732e functions. │ │ │ │ -0001efb0: 656d 706c 6163 655f 6261 636b 2863 6f6e emplace_back(con │ │ │ │ -0001efc0: 7665 7274 4672 6f6d 266c 743b 4d2c 2058 vertFrom<M, X │ │ │ │ -0001efd0: 2667 743b 2862 7261 6e63 682c 204c 5f6f >(branch, L_o │ │ │ │ -0001efe0: 665f 4d2c 2059 5f6f 665f 5829 293b 3c2f f_M, Y_of_X));.
    6 │ │ │ │ -0001f040: 3939 3c2f 7370 616e 3e20 2020 207d 3c2f 99 }.
    7 │ │ │ │ -0001f0a0: 3030 3c2f 7370 616e 3e20 2020 203c 7370 00 return LY::compose │ │ │ │ -0001f0e0: 2866 756e 6374 696f 6e73 2e62 6567 696e (functions.begin │ │ │ │ -0001f0f0: 2829 2c20 6675 6e63 7469 6f6e 732e 656e (), functions.en │ │ │ │ -0001f100: 6428 292c 206e 6577 4c61 6265 6c29 3b3c d(), newLabel);< │ │ │ │ -0001f110: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0001f160: 3730 313c 2f73 7061 6e3e 2020 7d3c 2f64 701 }.
    .
    702 │ │ │ │ -0001f1d0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001f220: 2037 3033 3c2f 7370 616e 3e3c 7370 616e 703 /************ │ │ │ │ -0001f250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001f260: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001f270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001f280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0001f290: 2f3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c /
    .< │ │ │ │ -0001f2a0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0001f2b0: 3e3c 6120 6964 3d22 6c30 3037 3134 2220 > 714 templ │ │ │ │ -0001f310: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ -0001f320: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0001f330: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    L, ty │ │ │ │ -0001f360: 7065 6e61 6d65 3c2f 7370 616e 3e20 5926 pename Y& │ │ │ │ -0001f370: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    │ │ │ │ -0001f3c0: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ -0001f4a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0001f4d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 716 u │ │ │ │ -0001f510: 7369 6e67 203c 2f73 7061 6e3e 4620 3d20 sing F = │ │ │ │ -0001f520: 7374 643a 3a66 756e 6374 696f 6e26 6c74 std::function< │ │ │ │ -0001f530: 3b76 6f69 6428 3c73 7061 6e20 636c 6173 ;void(cons │ │ │ │ -0001f550: 743c 2f73 7061 6e3e 2059 2661 6d70 3b29 t Y&) │ │ │ │ -0001f560: 2667 743b 3b3c 2f64 6976 3e0a 3c64 6976 >;
    .
    < │ │ │ │ -0001f5a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0001f5b0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 717 explicit Visit(F │ │ │ │ -0001f690: 3c61 2063 6c61 7373 3d22 636f 6465 2068 f) : f │ │ │ │ -0001f740: 283c 6120 636c 6173 733d 2263 6f64 6520 (f) {} .
    .
    718 F f │ │ │ │ -0001f8e0: 3c2f 613e 3b20 2020 2020 2020 2020 2020 ; │ │ │ │ -0001f8f0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0001f900: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0001f950: 2037 3139 3c2f 7370 616e 3e20 3c2f 6469 719 .
    .
    721< │ │ │ │ -0001fa40: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> void │ │ │ │ -0001fa70: 3c61 2063 6c61 7373 3d22 636f 6465 2068 operator()( │ │ │ │ -0001fad0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const typenam │ │ │ │ -0001fb10: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e Decisio │ │ │ │ -0001fb70: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ -0001fb80: 3b3a 3a4e 6f64 6550 7472 3c2f 613e 2661 ;::NodePtr&a │ │ │ │ -0001fb90: 6d70 3b20 6e6f 6465 293c 7370 616e 2063 mp; node) │ │ │ │ -0001fbb0: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    7 │ │ │ │ -0001fc10: 3232 3c2f 7370 616e 3e20 2020 2020 203c 22 < │ │ │ │ -0001fc20: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0001fc30: 6f72 6422 3e75 7369 6e67 203c 2f73 7061 ord">using Leaf = t │ │ │ │ -0001fc60: 7970 656e 616d 653c 2f73 7061 6e3e 203c ypename < │ │ │ │ -0001fc70: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -0001fc80: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ -0001fc90: 3032 3737 362e 6874 6d6c 223e 4465 6369 02776.html">Deci │ │ │ │ -0001fca0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ -0001fcb0: 2667 743b 3a3a 4c65 6166 3c2f 613e 3b3c >::Leaf;< │ │ │ │ -0001fcc0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0001fd10: 3732 333c 2f73 7061 6e3e 2020 2020 2020 723 │ │ │ │ -0001fd20: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (auto< │ │ │ │ -0001fd60: 2f73 7061 6e3e 206c 6561 6620 3d20 626f /span> leaf = bo │ │ │ │ -0001fd70: 6f73 743a 3a64 796e 616d 6963 5f70 6f69 ost::dynamic_poi │ │ │ │ -0001fd80: 6e74 6572 5f63 6173 7426 6c74 3b63 6f6e nter_cast<con │ │ │ │ -0001fd90: 7374 204c 6561 6626 6774 3b28 6e6f 6465 st Leaf>(node │ │ │ │ -0001fda0: 2929 3c2f 6469 763e 0a3c 6469 7620 636c ))
    .
    724 │ │ │ │ -0001fe00: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ -0001fe20: 6574 7572 6e3c 2f73 7061 6e3e 203c 6120 eturn f< │ │ │ │ -0001fe80: 2f61 3e28 6c65 6166 2d26 6774 3b63 6f6e /a>(leaf->con │ │ │ │ -0001fe90: 7374 616e 7428 2929 3b3c 2f64 6976 3e0a stant());
    . │ │ │ │ -0001fea0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ -0001ff30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -0001ff40: 6e6f 223e 2020 3732 363c 2f73 7061 6e3e no"> 726 │ │ │ │ -0001ff50: 2020 2020 2020 3c73 7061 6e20 636c 6173 usin │ │ │ │ -0001ff70: 6720 3c2f 7370 616e 3e43 686f 6963 6520 g Choice │ │ │ │ -0001ff80: 3d20 3c73 7061 6e20 636c 6173 733d 226b = typename │ │ │ │ -0001ffa0: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 DecisionTre │ │ │ │ -0001ffe0: 6526 6c74 3b4c 2c20 5926 6774 3b3a 3a43 e<L, Y>::C │ │ │ │ -0001fff0: 686f 6963 653c 2f61 3e3b 3c2f 6469 763e hoice;
    │ │ │ │ -00020000: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00020030: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 727 auto cho │ │ │ │ -00020080: 6963 6520 3d20 626f 6f73 743a 3a64 796e ice = boost::dyn │ │ │ │ -00020090: 616d 6963 5f70 6f69 6e74 6572 5f63 6173 amic_pointer_cas │ │ │ │ -000200a0: 7426 6c74 3b63 6f6e 7374 2043 686f 6963 t<const Choic │ │ │ │ -000200b0: 6526 6774 3b28 6e6f 6465 293b 3c2f 6469 e>(node);.
    728 │ │ │ │ -00020110: 3c2f 7370 616e 3e20 2020 2020 203c 7370 if │ │ │ │ -00020140: 2028 2163 686f 6963 6529 3c2f 6469 763e (!choice)
    │ │ │ │ -00020150: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00020180: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 729 throw std::invalid │ │ │ │ -000201e0: 5f61 7267 756d 656e 7428 3c73 7061 6e20 _argument("Deci │ │ │ │ -00020210: 7369 6f6e 5472 6565 3a3a 5669 7369 743a sionTree::Visit: │ │ │ │ -00020220: 2049 6e76 616c 6964 204e 6f64 6550 7472 Invalid NodePtr │ │ │ │ -00020230: 2671 756f 743b 3c2f 7370 616e 3e29 3b3c ");< │ │ │ │ -00020240: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00020290: 3733 303c 2f73 7061 6e3e 2020 2020 2020 730 │ │ │ │ -000202a0: 3c73 7061 6e20 636c 6173 733d 226b 6579 for (auto │ │ │ │ -000202e0: 3c2f 7370 616e 3e26 616d 703b 2661 6d70 && │ │ │ │ -000202f0: 3b20 6272 616e 6368 203a 2063 686f 6963 ; branch : choic │ │ │ │ -00020300: 652d 2667 743b 6272 616e 6368 6573 2829 e->branches() │ │ │ │ -00020310: 2920 282a 3c73 7061 6e20 636c 6173 733d ) (*this)(branch); │ │ │ │ -00020340: 203c 7370 616e 2063 6c61 7373 3d22 636f // recurs │ │ │ │ -00020360: 6521 3c2f 7370 616e 3e3c 2f64 6976 3e0a e!
    . │ │ │ │ -00020370: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ -000203d0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00020420: 2037 3332 3c2f 7370 616e 3e20 207d 3b3c 732 };< │ │ │ │ -00020430: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00020480: 3733 333c 2f73 7061 6e3e 203c 2f64 6976 733
    .
    734< │ │ │ │ -000204e0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ -00020500: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ -00020510: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ -00020530: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ -00020560: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    .< │ │ │ │ -00020580: 6120 6964 3d22 6c30 3037 3335 2220 6e61 a id="l00735" na │ │ │ │ -00020590: 6d65 3d22 6c30 3037 3335 223e 3c2f 613e me="l00735"> │ │ │ │ -000205a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 735 templat │ │ │ │ -000205e0: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename Func>
    .
    .
    │ │ │ │ -000206b0: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 736 void De │ │ │ │ -00020780: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00020790: 2059 2667 743b 3a3a 7669 7369 743c 2f61 Y>::visit(Func f) │ │ │ │ -000207c0: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    7 │ │ │ │ -00020820: 3337 3c2f 7370 616e 3e20 2020 203c 6120 37 Visit& │ │ │ │ -00020860: 6c74 3b4c 2c20 5926 6774 3b3c 2f61 3e20 lt;L, Y> │ │ │ │ -00020870: 7669 7369 7428 6629 3b3c 2f64 6976 3e0a visit(f);
    . │ │ │ │ -00020880: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ -00020920: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00020930: 6e6f 223e 2020 3733 393c 2f73 7061 6e3e no"> 739 │ │ │ │ -00020940: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -00020950: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -000209c0: 6120 6964 3d22 6c30 3037 3431 2220 6e61 a id="l00741" na │ │ │ │ -000209d0: 6d65 3d22 6c30 3037 3431 223e 3c2f 613e me="l00741"> │ │ │ │ -000209e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 741 /****** │ │ │ │ -00020a20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00020a30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00020a40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00020a50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00020a60: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    7 │ │ │ │ -00020ac0: 3531 3c2f 7370 616e 3e20 203c 7370 616e 51 template │ │ │ │ -00020af0: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ -00020b10: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ -00020b50: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ -00020be0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00020bf0: 7265 663d 2261 3032 3738 382e 6874 6d6c ref="a02788.html │ │ │ │ -00020c00: 223e 2020 3735 323c 2f61 3e3c 2f73 7061 "> 752 struct │ │ │ │ -00020c30: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 VisitLeaf {
    .
    753 │ │ │ │ -00020cd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 using < │ │ │ │ -00020cf0: 2f73 7061 6e3e 4620 3d20 7374 643a 3a66 /span>F = std::f │ │ │ │ -00020d00: 756e 6374 696f 6e26 6c74 3b76 6f69 6428 unction<void( │ │ │ │ -00020d10: 3c73 7061 6e20 636c 6173 733d 226b 6579 const typenam │ │ │ │ -00020d50: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e DecisionTr │ │ │ │ -00020d90: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ -00020da0: 4c65 6166 3c2f 613e 2661 6d70 3b29 2667 Leaf&)&g │ │ │ │ -00020db0: 743b 3b3c 2f64 6976 3e0a 3c64 6976 2063 t;;
    .
    754 │ │ │ │ -00020e50: 3c2f 7370 616e 3e20 2020 203c 7370 616e explicit │ │ │ │ -00020e80: 203c 6120 636c 6173 733d 2263 6f64 6520 VisitLeaf( │ │ │ │ -00020ee0: 4620 3c61 2063 6c61 7373 3d22 636f 6465 F f) : f(f) {} < │ │ │ │ -00020ff0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .< │ │ │ │ -00021050: 6120 6964 3d22 6c30 3037 3535 2220 6e61 a id="l00755" na │ │ │ │ -00021060: 6d65 3d22 6c30 3037 3535 223e 3c2f 613e me="l00755"> │ │ │ │ -00021070: 3c73 7061 6e20 636c 6173 733d 226c 696e 755< │ │ │ │ -000210d0: 2f61 3e3c 2f73 7061 6e3e 2020 2020 4620 /a> F │ │ │ │ -000210e0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 f; │ │ │ │ -00021140: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00021150: 2020 3c2f 6469 763e 0a3c 6469 7620 636c
    .
    756 .
    .
    758 void operator()(const typen │ │ │ │ -00021360: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decis │ │ │ │ -000213c0: 696f 6e54 7265 6526 6c74 3b4c 2c20 5926 ionTree<L, Y& │ │ │ │ -000213d0: 6774 3b3a 3a4e 6f64 6550 7472 3c2f 613e gt;::NodePtr │ │ │ │ -000213e0: 2661 6d70 3b20 6e6f 6465 293c 7370 616e & node) const { │ │ │ │ -00021410: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00021460: 2037 3539 3c2f 7370 616e 3e20 2020 2020 759 │ │ │ │ -00021470: 203c 7370 616e 2063 6c61 7373 3d22 6b65 using Leaf = typename │ │ │ │ -000214c0: 203c 6120 636c 6173 733d 2263 6f64 6520 De │ │ │ │ -000214f0: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00021500: 2059 2667 743b 3a3a 4c65 6166 3c2f 613e Y>::Leaf │ │ │ │ -00021510: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -00021560: 2020 3736 303c 2f73 7061 6e3e 2020 2020 760 │ │ │ │ -00021570: 2020 3c73 7061 6e20 636c 6173 733d 226b if (aut │ │ │ │ -000215b0: 6f3c 2f73 7061 6e3e 206c 6561 6620 3d20 o leaf = │ │ │ │ -000215c0: 626f 6f73 743a 3a64 796e 616d 6963 5f70 boost::dynamic_p │ │ │ │ -000215d0: 6f69 6e74 6572 5f63 6173 7426 6c74 3b63 ointer_cast<c │ │ │ │ -000215e0: 6f6e 7374 204c 6561 6626 6774 3b28 6e6f onst Leaf>(no │ │ │ │ -000215f0: 6465 2929 3c2f 6469 763e 0a3c 6469 7620 de))
    .
    761 │ │ │ │ -00021650: 2020 2020 2020 203c 7370 616e 2063 6c61 return < │ │ │ │ -00021680: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00021690: 5f76 6172 6961 626c 6522 2068 7265 663d _variable" href= │ │ │ │ -000216a0: 2261 3032 3738 382e 6874 6d6c 2361 6330 "a02788.html#ac0 │ │ │ │ -000216b0: 6231 6136 6637 3233 3434 6638 6363 3436 b1a6f72344f8cc46 │ │ │ │ -000216c0: 3038 6130 3564 6162 3433 3961 3461 223e 08a05dab439a4a"> │ │ │ │ -000216d0: 663c 2f61 3e28 2a6c 6561 6629 3b3c 2f64 f(*leaf);.
    76 │ │ │ │ -00021730: 323c 2f73 7061 6e3e 203c 2f64 6976 3e0a 2
    . │ │ │ │ -00021740: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00021770: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 763 │ │ │ │ -000217b0: 7573 696e 6720 3c2f 7370 616e 3e43 686f using Cho │ │ │ │ -000217c0: 6963 6520 3d20 3c73 7061 6e20 636c 6173 ice = type │ │ │ │ -000217e0: 6e61 6d65 3c2f 7370 616e 3e20 3c61 2063 name Decisio │ │ │ │ -00021820: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ -00021830: 3b3a 3a43 686f 6963 653c 2f61 3e3b 3c2f ;::Choice;.
    7 │ │ │ │ -00021890: 3634 3c2f 7370 616e 3e20 2020 2020 203c 64 < │ │ │ │ -000218a0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000218b0: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ -000218c0: 2063 686f 6963 6520 3d20 626f 6f73 743a choice = boost: │ │ │ │ -000218d0: 3a64 796e 616d 6963 5f70 6f69 6e74 6572 :dynamic_pointer │ │ │ │ -000218e0: 5f63 6173 7426 6c74 3b63 6f6e 7374 2043 _cast<const C │ │ │ │ -000218f0: 686f 6963 6526 6774 3b28 6e6f 6465 293b hoice>(node); │ │ │ │ -00021900: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00021950: 2037 3635 3c2f 7370 616e 3e20 2020 2020 765 │ │ │ │ -00021960: 203c 7370 616e 2063 6c61 7373 3d22 6b65 if (!choice).
    7 │ │ │ │ -000219e0: 3636 3c2f 7370 616e 3e20 2020 2020 2020 66 │ │ │ │ -000219f0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 throw │ │ │ │ -00021a10: 3c2f 7370 616e 3e20 7374 643a 3a69 6e76 std::inv │ │ │ │ -00021a20: 616c 6964 5f61 7267 756d 656e 7428 3c73 alid_argument(" │ │ │ │ -00021a50: 4465 6369 7369 6f6e 5472 6565 3a3a 5669 DecisionTree::Vi │ │ │ │ -00021a60: 7369 744c 6561 663a 2049 6e76 616c 6964 sitLeaf: Invalid │ │ │ │ -00021a70: 204e 6f64 6550 7472 2671 756f 743b 3c2f NodePtr");
    . │ │ │ │ -00021aa0: 3c61 2069 643d 226c 3030 3736 3722 206e 767 for (auto& │ │ │ │ -00021b30: 616d 703b 2661 6d70 3b20 6272 616e 6368 amp;& branch │ │ │ │ -00021b40: 203a 2063 686f 6963 652d 2667 743b 6272 : choice->br │ │ │ │ -00021b50: 616e 6368 6573 2829 2920 282a 3c73 7061 anches()) (*this)(b │ │ │ │ -00021b80: 7261 6e63 6829 3b20 203c 7370 616e 2063 ranch); / │ │ │ │ -00021ba0: 2f20 7265 6375 7273 6521 3c2f 7370 616e / recurse!
    .
    │ │ │ │ -00021c00: 2020 3736 383c 2f73 7061 6e3e 2020 2020 768 │ │ │ │ -00021c10: 7d3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c }
    .
    .< │ │ │ │ -00021c20: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00021c30: 3e3c 6120 6964 3d22 6c30 3037 3639 2220 > 769 };
    . │ │ │ │ -00021c90: 3c61 2069 643d 226c 3030 3737 3022 206e 770
    .
    771 │ │ │ │ -00021d30: 3c73 7061 6e20 636c 6173 733d 226b 6579 template < │ │ │ │ -00021d70: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -00021d80: 4c2c 203c 7370 616e 2063 6c61 7373 3d22 L, typenam │ │ │ │ -00021da0: 653c 2f73 7061 6e3e 2059 2667 743b 3c2f e Y>.
    7 │ │ │ │ -00021e00: 3732 3c2f 7370 616e 3e20 203c 7370 616e 72 template │ │ │ │ -00021e30: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ -00021e50: 616d 653c 2f73 7061 6e3e 2046 756e 6326 ame Func& │ │ │ │ -00021e60: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    . │ │ │ │ -00021eb0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ -00021ee0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 7 │ │ │ │ -00021f40: 3733 3c2f 613e 3c2f 7370 616e 3e20 203c 73 < │ │ │ │ -00021f50: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00021f60: 6f72 6474 7970 6522 3e76 6f69 643c 2f73 ordtype">void DecisionTr │ │ │ │ -00021fd0: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ -00021fe0: 7669 7369 744c 6561 663c 2f61 3e28 4675 visitLeaf(Fu │ │ │ │ -00021ff0: 6e63 2066 293c 7370 616e 2063 6c61 7373 nc f) cons │ │ │ │ -00022010: 7420 3c2f 7370 616e 3e7b 3c2f 6469 763e t {
    │ │ │ │ -00022020: 0a3c 6469 7620 636c 6173 733d 226c 696e .. │ │ │ │ -000220d0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ -00022170: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00022180: 6e6f 223e 2020 3737 363c 2f73 7061 6e3e no"> 776 │ │ │ │ -00022190: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -000221a0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00022210: 6120 6964 3d22 6c30 3037 3738 2220 6e61 a id="l00778" na │ │ │ │ -00022220: 6d65 3d22 6c30 3037 3738 223e 3c2f 613e me="l00778"> │ │ │ │ -00022230: 3c73 7061 6e20 636c 6173 733d 226c 696e 778 /****** │ │ │ │ -00022270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00022280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00022290: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000222a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000222b0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    7 │ │ │ │ -00022310: 3835 3c2f 7370 616e 3e20 203c 7370 616e 85 template │ │ │ │ -00022340: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ -00022360: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ -000223a0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ -00022430: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00022440: 7265 663d 2261 3032 3739 322e 6874 6d6c ref="a02792.html │ │ │ │ -00022450: 223e 2020 3738 363c 2f61 3e3c 2f73 7061 "> 786 struct │ │ │ │ -00022480: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 VisitWith {
    .
    787 │ │ │ │ -00022520: 2020 203c 7370 616e 2063 6c61 7373 3d22 using < │ │ │ │ -00022540: 2f73 7061 6e3e 4620 3d20 7374 643a 3a66 /span>F = std::f │ │ │ │ -00022550: 756e 6374 696f 6e26 6c74 3b76 6f69 6428 unction<void( │ │ │ │ -00022560: 3c73 7061 6e20 636c 6173 733d 226b 6579 const A │ │ │ │ -000225b0: 7373 6967 6e6d 656e 7426 6c74 3b4c 2667 ssignment<L&g │ │ │ │ -000225c0: 743b 3c2f 613e 2661 6d70 3b2c 203c 7370 t;&, const │ │ │ │ -000225f0: 5926 616d 703b 2926 6774 3b3b 3c2f 6469 Y&)>;.
    │ │ │ │ -00022690: 2020 3738 383c 2f61 3e3c 2f73 7061 6e3e 788 │ │ │ │ -000226a0: 2020 2020 3c73 7061 6e20 636c 6173 733d explic │ │ │ │ -000226c0: 6974 3c2f 7370 616e 3e20 3c61 2063 6c61 it Visit │ │ │ │ -00022720: 5769 7468 3c2f 613e 2846 203c 6120 636c With(F f) : f(f) {}
    .< │ │ │ │ -00022840: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ -00022850: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ -00022860: 6e30 3037 3839 2220 6461 7461 2d73 7461 n00789" data-sta │ │ │ │ -00022870: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ -00022880: 227d 3b22 3e0a 3c64 6976 2063 6c61 7373 "};">.
    789 Assignment< │ │ │ │ -00022960: 3b4c 2667 743b 3c2f 613e 203c 6120 636c ;L> assi │ │ │ │ -000229c0: 676e 6d65 6e74 3c2f 613e 3b20 203c 2f64 gnment; .
    790 F f; │ │ │ │ -00022ad0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00022ae0: 2020 2020 203c 2f64 6976 3e0a 3c64 6976
    .
    < │ │ │ │ -00022b20: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00022b30: 6e6f 223e 2020 3739 313c 2f73 7061 6e3e no"> 791 │ │ │ │ -00022b40: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    . │ │ │ │ -00022ba0: 3c61 2069 643d 226c 3030 3739 3322 206e 793 │ │ │ │ -00022c20: 3c2f 613e 3c2f 7370 616e 3e20 2020 203c < │ │ │ │ -00022c30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00022c40: 6f72 6474 7970 6522 3e76 6f69 643c 2f73 ordtype">void operator() │ │ │ │ -00022cb0: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ -00022cd0: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c ty │ │ │ │ -00022cf0: 7065 6e61 6d65 3c2f 7370 616e 3e20 3c61 pename De │ │ │ │ -00022d50: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00022d60: 2059 2667 743b 3a3a 4e6f 6465 5074 723c Y>::NodePtr< │ │ │ │ -00022d70: 2f61 3e26 616d 703b 206e 6f64 6529 207b /a>& node) { │ │ │ │ -00022d80: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00022dd0: 2037 3934 3c2f 7370 616e 3e20 2020 2020 794 │ │ │ │ -00022de0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 using Leaf = typename │ │ │ │ -00022e30: 203c 6120 636c 6173 733d 2263 6f64 6520 De │ │ │ │ -00022e60: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ -00022e70: 2059 2667 743b 3a3a 4c65 6166 3c2f 613e Y>::Leaf │ │ │ │ -00022e80: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -00022ed0: 2020 3739 353c 2f73 7061 6e3e 2020 2020 795 │ │ │ │ -00022ee0: 2020 3c73 7061 6e20 636c 6173 733d 226b if (aut │ │ │ │ -00022f20: 6f3c 2f73 7061 6e3e 206c 6561 6620 3d20 o leaf = │ │ │ │ -00022f30: 626f 6f73 743a 3a64 796e 616d 6963 5f70 boost::dynamic_p │ │ │ │ -00022f40: 6f69 6e74 6572 5f63 6173 7426 6c74 3b63 ointer_cast<c │ │ │ │ -00022f50: 6f6e 7374 204c 6561 6626 6774 3b28 6e6f onst Leaf>(no │ │ │ │ -00022f60: 6465 2929 3c2f 6469 763e 0a3c 6469 7620 de))
    .
    796 │ │ │ │ -00022fc0: 2020 2020 2020 203c 7370 616e 2063 6c61 return < │ │ │ │ -00022ff0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00023000: 5f76 6172 6961 626c 6522 2068 7265 663d _variable" href= │ │ │ │ -00023010: 2261 3032 3739 322e 6874 6d6c 2361 6463 "a02792.html#adc │ │ │ │ -00023020: 3463 6662 3532 6434 3430 3932 6364 6165 4cfb52d44092cdae │ │ │ │ -00023030: 3966 3136 6365 6461 3835 3066 3535 223e 9f16ceda850f55"> │ │ │ │ -00023040: 663c 2f61 3e28 3c61 2063 6c61 7373 3d22 f(assignmen │ │ │ │ -000230a0: 743c 2f61 3e2c 206c 6561 662d 2667 743b t, leaf-> │ │ │ │ -000230b0: 636f 6e73 7461 6e74 2829 293b 3c2f 6469 constant());.
    797 │ │ │ │ -00023110: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ -00023120: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00023130: 3e3c 6120 6964 3d22 6c30 3037 3938 2220 > 798 u │ │ │ │ -00023190: 7369 6e67 203c 2f73 7061 6e3e 4368 6f69 sing Choi │ │ │ │ -000231a0: 6365 203d 203c 7370 616e 2063 6c61 7373 ce = typen │ │ │ │ -000231c0: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decision │ │ │ │ -00023200: 5472 6565 266c 743b 4c2c 2059 2667 743b Tree<L, Y> │ │ │ │ -00023210: 3a3a 4368 6f69 6365 3c2f 613e 3b3c 2f64 ::Choice;.
    79 │ │ │ │ -00023270: 393c 2f73 7061 6e3e 2020 2020 2020 3c73 9 auto │ │ │ │ -000232a0: 6368 6f69 6365 203d 2062 6f6f 7374 3a3a choice = boost:: │ │ │ │ -000232b0: 6479 6e61 6d69 635f 706f 696e 7465 725f dynamic_pointer_ │ │ │ │ -000232c0: 6361 7374 266c 743b 636f 6e73 7420 4368 cast<const Ch │ │ │ │ -000232d0: 6f69 6365 2667 743b 286e 6f64 6529 3b3c oice>(node);< │ │ │ │ -000232e0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00023330: 3830 303c 2f73 7061 6e3e 2020 2020 2020 800 │ │ │ │ -00023340: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (!choice).
    80 │ │ │ │ -000233c0: 313c 2f73 7061 6e3e 2020 2020 2020 2020 1 │ │ │ │ -000233d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 throw< │ │ │ │ -000233f0: 2f73 7061 6e3e 2073 7464 3a3a 696e 7661 /span> std::inva │ │ │ │ -00023400: 6c69 645f 6172 6775 6d65 6e74 283c 7370 lid_argument("D │ │ │ │ -00023430: 6563 6973 696f 6e54 7265 653a 3a56 6973 ecisionTree::Vis │ │ │ │ -00023440: 6974 5769 7468 3a20 496e 7661 6c69 6420 itWith: Invalid │ │ │ │ -00023450: 4e6f 6465 5074 7226 7175 6f74 3b3c 2f73 NodePtr");
    .< │ │ │ │ -00023480: 6120 6964 3d22 6c30 3038 3032 2220 6e61 a id="l00802" na │ │ │ │ -00023490: 6d65 3d22 6c30 3038 3032 223e 3c2f 613e me="l00802"> │ │ │ │ -000234a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 802 for (size_t
    i = 0; i &l │ │ │ │ -00023520: 743b 2063 686f 6963 652d 2667 743b 6e72 t; choice->nr │ │ │ │ -00023530: 4368 6f69 6365 7328 293b 2069 2b2b 2920 Choices(); i++) │ │ │ │ -00023540: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    ..
    804< │ │ │ │ -000236b0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ -000236d0: 3c61 2069 643d 226c 3030 3830 3522 206e 805 (*this │ │ │ │ -00023720: 2928 6368 6f69 6365 2d26 6774 3b62 7261 )(choice->bra │ │ │ │ -00023730: 6e63 6865 7328 295b 695d 293b 2020 3c73 nches()[i]); // recurse!< │ │ │ │ -00023760: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -00023780: 6120 6964 3d22 6c30 3038 3036 2220 6e61 a id="l00806" na │ │ │ │ -00023790: 6d65 3d22 6c30 3038 3036 223e 3c2f 613e me="l00806"> │ │ │ │ -000237a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 806
    .
    807 │ │ │ │ -00023820: 2020 2020 203c 7370 616e 2063 6c61 7373 // Re │ │ │ │ -00023840: 6d6f 7665 2074 6865 2063 686f 6963 6520 move the choice │ │ │ │ -00023850: 736f 2077 6520 6172 6520 6261 636b 7472 so we are backtr │ │ │ │ -00023860: 6163 6b69 6e67 3c2f 7370 616e 3e3c 2f64 acking.
    80 │ │ │ │ -000238c0: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ -000238d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 auto choice_it = a │ │ │ │ -00023950: 7373 6967 6e6d 656e 743c 2f61 3e2e 6669 ssignment.fi │ │ │ │ -00023960: 6e64 2863 686f 6963 652d 2667 743b 6c61 nd(choice->la │ │ │ │ -00023970: 6265 6c28 2929 3b3c 2f64 6976 3e0a 3c64 bel());
    . │ │ │ │ -00023990: 3c61 2069 643d 226c 3030 3830 3922 206e 809
    assig │ │ │ │ -00023a30: 6e6d 656e 743c 2f61 3e2e 6572 6173 6528 nment.erase( │ │ │ │ -00023a40: 6368 6f69 6365 5f69 7429 3b3c 2f64 6976 choice_it);
    .
    810< │ │ │ │ -00023aa0: 2f73 7061 6e3e 2020 2020 2020 7d3c 2f64 /span> }.
    81 │ │ │ │ -00023b00: 313c 2f73 7061 6e3e 2020 2020 7d3c 2f64 1 }.
    .
    812 │ │ │ │ -00023b70: 207d 3b3c 2f64 6976 3e0a 3c64 6976 2063 };
    .
    813 < │ │ │ │ -00023bd0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00023c20: 3831 343c 2f73 7061 6e3e 2020 3c73 7061 814 template <type │ │ │ │ -00023c70: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ -00023c80: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00023c90: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ -00023cb0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00023ce0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 815 tem │ │ │ │ -00023d20: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ -00023d30: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ -00023d50: 2f73 7061 6e3e 2046 756e 6326 6774 3b3c /span> Func>< │ │ │ │ -00023d60: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ -00023de0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00023df0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 816 void │ │ │ │ -00023e70: 203c 6120 636c 6173 733d 2263 6f64 6520 DecisionTree&l │ │ │ │ -00023ed0: 743b 4c2c 2059 2667 743b 3a3a 7669 7369 t;L, Y>::visi │ │ │ │ -00023ee0: 7457 6974 683c 2f61 3e28 4675 6e63 2066 tWith(Func f │ │ │ │ -00023ef0: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ -00023f30: 6120 6964 3d22 6c30 3038 3137 2220 6e61 a id="l00817" na │ │ │ │ -00023f40: 6d65 3d22 6c30 3038 3137 223e 3c2f 613e me="l00817"> │ │ │ │ -00023f50: 3c73 7061 6e20 636c 6173 733d 226c 696e 817 VisitWith<L │ │ │ │ -00023fb0: 2c20 5926 6774 3b3c 2f61 3e20 7669 7369 , Y> visi │ │ │ │ -00023fc0: 7428 6629 3b3c 2f64 6976 3e0a 3c64 6976 t(f);
    .
    < │ │ │ │ -00024000: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00024010: 6e6f 223e 2020 3831 383c 2f73 7061 6e3e no"> 818 │ │ │ │ -00024020: 2020 2020 7669 7369 7428 726f 6f74 5f29 visit(root_) │ │ │ │ -00024030: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -00024080: 2020 3831 393c 2f73 7061 6e3e 2020 7d3c 819 }< │ │ │ │ -00024090: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ -000240b0: 6120 6964 3d22 6c30 3038 3230 2220 6e61 a id="l00820" na │ │ │ │ -000240c0: 6d65 3d22 6c30 3038 3230 223e 3c2f 613e me="l00820"> │ │ │ │ -000240d0: 3c73 7061 6e20 636c 6173 733d 226c 696e 820
    .
    821 /********** │ │ │ │ -00024170: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024180: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024190: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000241a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000241b0: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
    │ │ │ │ -000241c0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000241f0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 822 tem │ │ │ │ -00024230: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ -00024240: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ -00024260: 2f73 7061 6e3e 204c 2c20 3c73 7061 6e20 /span> L, │ │ │ │ -00024280: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -00024290: 5926 6774 3b3c 2f64 6976 3e0a 3c64 6976 Y>
    .
    .
    │ │ │ │ -00024370: 2038 3233 3c2f 613e 3c2f 7370 616e 3e20 823 │ │ │ │ -00024380: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ -000243a0: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t Decisi │ │ │ │ -00024400: 6f6e 5472 6565 266c 743b 4c2c 2059 2667 onTree<L, Y&g │ │ │ │ -00024410: 743b 3a3a 6e72 4c65 6176 6573 3c2f 613e t;::nrLeaves │ │ │ │ -00024420: 2829 3c73 7061 6e20 636c 6173 733d 226b () const < │ │ │ │ -00024440: 2f73 7061 6e3e 7b3c 2f64 6976 3e0a 3c64 /span>{
    . │ │ │ │ -00024460: 3c61 2069 643d 226c 3030 3832 3422 206e 824 │ │ │ │ -000244c0: 7369 7a65 5f74 3c2f 7370 616e 3e20 746f size_t to │ │ │ │ -000244d0: 7461 6c20 3d20 303b 3c2f 6469 763e 0a3c tal = 0;
    .< │ │ │ │ -000244e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000244f0: 3e3c 6120 6964 3d22 6c30 3038 3235 2220 > 825 visit([&a │ │ │ │ -00024540: 6d70 3b74 6f74 616c 5d28 3c73 7061 6e20 mp;total]( │ │ │ │ -00024560: 636f 6e73 743c 2f73 7061 6e3e 2059 2661 const Y&a │ │ │ │ -00024570: 6d70 3b20 6e6f 6465 2920 7b20 746f 7461 mp; node) { tota │ │ │ │ -00024580: 6c20 2b3d 2031 3b20 7d29 3b3c 2f64 6976 l += 1; });
    .
    826< │ │ │ │ -000245e0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> return total;
    .< │ │ │ │ -00024620: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00024630: 3e3c 6120 6964 3d22 6c30 3038 3237 2220 > 827 }
    ..
    82 │ │ │ │ -000246d0: 383c 2f73 7061 6e3e 203c 2f64 6976 3e0a 8
    . │ │ │ │ -000246e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -000247f0: 2020 3833 303c 2f73 7061 6e3e 2020 3c73 830 // fold is j │ │ │ │ -00024820: 7573 7420 646f 6e65 2077 6974 6820 6120 ust done with a │ │ │ │ -00024830: 7669 7369 743c 2f73 7061 6e3e 3c2f 6469 visit.
    831 │ │ │ │ -00024890: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ -000248b0: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ -000248c0: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ -000248e0: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    . │ │ │ │ -00024930: 3c61 2069 643d 226c 3030 3833 3222 206e 832 templa │ │ │ │ -00024990: 7465 3c2f 7370 616e 3e20 266c 743b 3c73 te <typename Func, │ │ │ │ -000249e0: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -000249f0: 5826 6774 3b3c 2f64 6976 3e0a 3c64 6976 X>
    ..
    │ │ │ │ -00024d10: 2020 3833 363c 2f73 7061 6e3e 2020 7d3c 836 }< │ │ │ │ -00024d20: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ -00024d40: 6120 6964 3d22 6c30 3038 3337 2220 6e61 a id="l00837" na │ │ │ │ -00024d50: 6d65 3d22 6c30 3038 3337 223e 3c2f 613e me="l00837"> │ │ │ │ -00024d60: 3c73 7061 6e20 636c 6173 733d 226c 696e 837
    .
    838 /********** │ │ │ │ -00024e00: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024e10: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00024e40: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
    │ │ │ │ -00024e50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00024e80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 852 tem │ │ │ │ -00024ec0: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ -00024ed0: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ -00024ef0: 2f73 7061 6e3e 204c 2c20 3c73 7061 6e20 /span> L, │ │ │ │ -00024f10: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ -00024f20: 5926 6774 3b3c 2f64 6976 3e0a 3c64 6976 Y>
    .
    .
    │ │ │ │ -00025000: 2038 3533 3c2f 613e 3c2f 7370 616e 3e20 853 │ │ │ │ -00025010: 2073 7464 3a3a 7365 7426 6c74 3b4c 2667 std::set<L&g │ │ │ │ -00025020: 743b 203c 6120 636c 6173 733d 2263 6f64 t; DecisionTree │ │ │ │ -00025080: 266c 743b 4c2c 2059 2667 743b 3a3a 6c61 <L, Y>::la │ │ │ │ -00025090: 6265 6c73 3c2f 613e 2829 3c73 7061 6e20 bels() │ │ │ │ -000250b0: 2063 6f6e 7374 203c 2f73 7061 6e3e 7b3c const {< │ │ │ │ -000250c0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00025110: 3835 343c 2f73 7061 6e3e 2020 2020 7374 854 st │ │ │ │ -00025120: 643a 3a73 6574 266c 743b 4c26 6774 3b20 d::set<L> │ │ │ │ -00025130: 756e 6971 7565 3b3c 2f64 6976 3e0a 3c64 unique;
    . │ │ │ │ -00025150: 3c61 2069 643d 226c 3030 3835 3522 206e 855 auto │ │ │ │ -000251b0: 3c2f 7370 616e 3e20 6620 3d20 5b26 616d f = [&am │ │ │ │ -000251c0: 703b 5d28 3c73 7061 6e20 636c 6173 733d p;](const< │ │ │ │ -000251e0: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> Assignment< │ │ │ │ -00025220: 3b4c 2667 743b 3c2f 613e 2661 6d70 3b20 ;L>& │ │ │ │ -00025230: 6173 7369 676e 6d65 6e74 2c20 3c73 7061 assignment, const Y │ │ │ │ -00025260: 2661 6d70 3b29 207b 3c2f 6469 763e 0a3c &) {
    .< │ │ │ │ -00025270: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00025280: 3e3c 6120 6964 3d22 6c30 3038 3536 2220 > 856 for (< │ │ │ │ -000252f0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00025300: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ -00025310: 2661 6d70 3b26 616d 703b 206b 7620 3a20 && kv : │ │ │ │ -00025320: 6173 7369 676e 6d65 6e74 2920 7b3c 2f64 assignment) {.
    85 │ │ │ │ -00025380: 373c 2f73 7061 6e3e 2020 2020 2020 2020 7 │ │ │ │ -00025390: 756e 6971 7565 2e69 6e73 6572 7428 6b76 unique.insert(kv │ │ │ │ -000253a0: 2e66 6972 7374 293b 3c2f 6469 763e 0a3c .first);
    .< │ │ │ │ -000253b0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -000253c0: 3e3c 6120 6964 3d22 6c30 3038 3538 2220 > 858 }
    │ │ │ │ -00025410: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    860< │ │ │ │ -000254c0: 2f73 7061 6e3e 2020 2020 7669 7369 7457 /span> visitW │ │ │ │ -000254d0: 6974 6828 6629 3b3c 2f64 6976 3e0a 3c64 ith(f);
    . │ │ │ │ -000254f0: 3c61 2069 643d 226c 3030 3836 3122 206e 861 │ │ │ │ -00025550: 7265 7475 726e 3c2f 7370 616e 3e20 756e return un │ │ │ │ -00025560: 6971 7565 3b3c 2f64 6976 3e0a 3c64 6976 ique;
    .
    < │ │ │ │ -000255a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000255b0: 6e6f 223e 2020 3836 323c 2f73 7061 6e3e no"> 862 │ │ │ │ -000255c0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -000255d0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00025640: 6120 6964 3d22 6c30 3038 3634 2220 6e61 a id="l00864" na │ │ │ │ -00025650: 6d65 3d22 6c30 3038 3634 223e 3c2f 613e me="l00864"> │ │ │ │ -00025660: 3c73 7061 6e20 636c 6173 733d 226c 696e 864/******** │ │ │ │ -000256a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000256b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000256c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000256d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000256e0: 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 ****/.
    865 │ │ │ │ -00025740: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ -00025760: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ -00025770: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ -00025790: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    . │ │ │ │ -000257e0: 3c61 2069 643d 226c 3030 3836 3622 206e 866 bo │ │ │ │ -00025840: 6f6c 3c2f 7370 616e 3e20 3c61 2063 6c61 ol DecisionTr │ │ │ │ -00025880: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ -00025890: 6571 7561 6c73 3c2f 613e 283c 7370 616e equals(const Decisi │ │ │ │ -000258f0: 6f6e 5472 6565 3c2f 613e 2661 6d70 3b20 onTree& │ │ │ │ -00025900: 6f74 6865 722c 3c2f 6469 763e 0a3c 6469 other,
    .< │ │ │ │ -00025920: 6120 6964 3d22 6c30 3038 3637 2220 6e61 a id="l00867" na │ │ │ │ -00025930: 6d65 3d22 6c30 3038 3637 223e 3c2f 613e me="l00867"> │ │ │ │ -00025940: 3c73 7061 6e20 636c 6173 733d 226c 696e 867 │ │ │ │ -00025970: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00025980: 2020 203c 7370 616e 2063 6c61 7373 3d22 const CompareFun │ │ │ │ -000259b0: 6326 616d 703b 2063 6f6d 7061 7265 293c c& compare)< │ │ │ │ -000259c0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -000259d0: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    868 │ │ │ │ -00025a40: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ -00025a60: 7572 6e3c 2f73 7061 6e3e 2072 6f6f 745f urn root_ │ │ │ │ -00025a70: 2d26 6774 3b65 7175 616c 7328 2a6f 7468 ->equals(*oth │ │ │ │ -00025a80: 6572 2e3c 6120 636c 6173 733d 2263 6f64 er.root_, c │ │ │ │ -00025ae0: 6f6d 7061 7265 293b 3c2f 6469 763e 0a3c ompare);
    .< │ │ │ │ -00025af0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00025b00: 3e3c 6120 6964 3d22 6c30 3038 3639 2220 > 869 }
    .< │ │ │ │ -00025b60: 6120 6964 3d22 6c30 3038 3730 2220 6e61 a id="l00870" na │ │ │ │ -00025b70: 6d65 3d22 6c30 3038 3730 223e 3c2f 613e me="l00870"> │ │ │ │ -00025b80: 3c73 7061 6e20 636c 6173 733d 226c 696e 870
    .
    871 < │ │ │ │ -00025c00: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00025c10: 6f72 6422 3e74 656d 706c 6174 653c 2f73 ord">template <t │ │ │ │ -00025c40: 7970 656e 616d 653c 2f73 7061 6e3e 204c ypename L │ │ │ │ -00025c50: 2c20 3c73 7061 6e20 636c 6173 733d 226b , typename │ │ │ │ -00025c70: 3c2f 7370 616e 3e20 5926 6774 3b3c 2f64 Y>.
    .
    872 │ │ │ │ -00025d60: 3c2f 7370 616e 3e20 203c 7370 616e 2063 void < │ │ │ │ -00025d90: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00025da0: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ -00025db0: 2261 3032 3739 362e 6874 6d6c 2361 3930 "a02796.html#a90 │ │ │ │ -00025dc0: 3835 3439 3336 3135 3938 6562 6364 3661 8549361598ebcd6a │ │ │ │ -00025dd0: 6434 3061 3565 3565 6562 6337 3363 223e d40a5e5eebc73c"> │ │ │ │ -00025de0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ -00025df0: 4c2c 2059 2667 743b 3a3a 7072 696e 743c L, Y>::print< │ │ │ │ -00025e00: 2f61 3e28 3c73 7061 6e20 636c 6173 733d /a>(const< │ │ │ │ -00025e20: 2f73 7061 6e3e 2073 7464 3a3a 7374 7269 /span> std::stri │ │ │ │ -00025e30: 6e67 2661 6d70 3b20 732c 3c2f 6469 763e ng& s,
    │ │ │ │ -00025e40: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00025e70: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 873 │ │ │ │ -00025ea0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00025eb0: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ -00025ed0: 743c 2f73 7061 6e3e 204c 6162 656c 466f t LabelFo │ │ │ │ -00025ee0: 726d 6174 7465 7226 616d 703b 206c 6162 rmatter& lab │ │ │ │ -00025ef0: 656c 466f 726d 6174 7465 722c 3c2f 6469 elFormatter,.
    874 │ │ │ │ -00025f50: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ -00025f60: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00025f70: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ -00025f90: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ -00025fa0: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ -00025fb0: 616c 7565 466f 726d 6174 7465 7229 3c73 alueFormatter) const {
    .
    875 │ │ │ │ -00026040: 2020 726f 6f74 5f2d 2667 743b 7072 696e root_->prin │ │ │ │ -00026050: 7428 732c 206c 6162 656c 466f 726d 6174 t(s, labelFormat │ │ │ │ -00026060: 7465 722c 2076 616c 7565 466f 726d 6174 ter, valueFormat │ │ │ │ -00026070: 7465 7229 3b3c 2f64 6976 3e0a 3c64 6976 ter);
    .
    < │ │ │ │ -000260b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000260c0: 6e6f 223e 2020 3837 363c 2f73 7061 6e3e no"> 876 │ │ │ │ -000260d0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -000260e0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00026150: 6120 6964 3d22 6c30 3038 3738 2220 6e61 a id="l00878" na │ │ │ │ -00026160: 6d65 3d22 6c30 3038 3738 223e 3c2f 613e me="l00878"> │ │ │ │ -00026170: 3c73 7061 6e20 636c 6173 733d 226c 696e 878 templat │ │ │ │ -000261b0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ -00026200: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ -00026210: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ -00026270: 6120 6964 3d22 6c30 3038 3739 2220 6e61 a id="l00879" na │ │ │ │ -00026280: 6d65 3d22 6c30 3038 3739 223e 3c2f 613e me="l00879"> │ │ │ │ -00026290: 3c73 7061 6e20 636c 6173 733d 226c 696e 879< │ │ │ │ -000262f0: 2f61 3e3c 2f73 7061 6e3e 2020 3c73 7061 /a> bool DecisionTree& │ │ │ │ -00026380: 6c74 3b4c 2c20 5926 6774 3b3a 3a6f 7065 lt;L, Y>::ope │ │ │ │ -00026390: 7261 746f 723d 3d3c 2f61 3e28 3c73 7061 rator==(const < │ │ │ │ -000263c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -000263d0: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ -000263e0: 3237 3936 2e68 746d 6c22 3e44 6563 6973 2796.html">Decis │ │ │ │ -000263f0: 696f 6e54 7265 653c 2f61 3e26 616d 703b ionTree& │ │ │ │ -00026400: 206f 7468 6572 293c 7370 616e 2063 6c61 other) co │ │ │ │ -00026420: 6e73 7420 3c2f 7370 616e 3e7b 3c2f 6469 nst {.
    880 │ │ │ │ -00026480: 3c2f 7370 616e 3e20 2020 203c 7370 616e return root_->equ │ │ │ │ -000264c0: 616c 7328 2a6f 7468 6572 2e3c 6120 636c als(*other.root │ │ │ │ -00026520: 5f3c 2f61 3e29 3b3c 2f64 6976 3e0a 3c64 _);
    . │ │ │ │ -00026540: 3c61 2069 643d 226c 3030 3838 3122 206e 881 }
    ..
    882 │ │ │ │ -000265e0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ -000265f0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -00026600: 3e3c 6120 6964 3d22 6c30 3038 3833 2220 > 883 templ │ │ │ │ -00026660: 6174 653c 2f73 7061 6e3e 266c 743b 3c73 ate<typename L, typ │ │ │ │ -000266b0: 656e 616d 653c 2f73 7061 6e3e 2059 2667 ename Y&g │ │ │ │ -000266c0: 743b 3c2f 6469 763e 0a3c 6469 7620 636c t;
    ..
    < │ │ │ │ -00026920: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00026930: 6e6f 223e 2020 3838 353c 2f73 7061 6e3e no"> 885 │ │ │ │ -00026940: 2020 2020 3c73 7061 6e20 636c 6173 733d re │ │ │ │ -00026960: 7475 726e 3c2f 7370 616e 3e20 726f 6f74 turn root │ │ │ │ -00026970: 5f2d 2667 743b 6f70 6572 6174 6f72 2028 _->operator ( │ │ │ │ -00026980: 2928 7829 3b3c 2f64 6976 3e0a 3c64 6976 )(x);
    .
    < │ │ │ │ -000269c0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000269d0: 6e6f 223e 2020 3838 363c 2f73 7061 6e3e no"> 886 │ │ │ │ -000269e0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -000269f0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00026a60: 6120 6964 3d22 6c30 3038 3838 2220 6e61 a id="l00888" na │ │ │ │ -00026a70: 6d65 3d22 6c30 3038 3838 223e 3c2f 613e me="l00888"> │ │ │ │ -00026a80: 3c73 7061 6e20 636c 6173 733d 226c 696e 888 templat │ │ │ │ -00026ac0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ -00026b10: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ -00026b20: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ -00026b80: 6120 6964 3d22 6c30 3038 3839 2220 6e61 a id="l00889" na │ │ │ │ -00026b90: 6d65 3d22 6c30 3038 3839 223e 3c2f 613e me="l00889"> │ │ │ │ -00026ba0: 3c73 7061 6e20 636c 6173 733d 226c 696e 889< │ │ │ │ -00026c00: 2f61 3e3c 2f73 7061 6e3e 2020 3c61 2063 /a> Decision │ │ │ │ -00026c40: 5472 6565 266c 743b 4c2c 2059 2667 743b Tree<L, Y> │ │ │ │ -00026c50: 3c2f 613e 203c 6120 636c 6173 733d 2263 DecisionTr │ │ │ │ -00026cb0: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ -00026cc0: 6170 706c 793c 2f61 3e28 3c73 7061 6e20 apply( │ │ │ │ -00026ce0: 636f 6e73 743c 2f73 7061 6e3e 203c 6120 const Una │ │ │ │ -00026d40: 7279 3c2f 613e 2661 6d70 3b20 6f70 293c ry& op)< │ │ │ │ -00026d50: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00026d60: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    890 │ │ │ │ -00026dd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 // It i │ │ │ │ -00026df0: 7320 756e 636c 6561 7220 7768 6174 2073 s unclear what s │ │ │ │ -00026e00: 686f 756c 6420 6861 7070 656e 2069 6620 hould happen if │ │ │ │ -00026e10: 7472 6565 2069 7320 656d 7074 793a 3c2f tree is empty:
    .
    < │ │ │ │ -00026e60: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00026e70: 6e6f 223e 2020 3839 313c 2f73 7061 6e3e no"> 891 │ │ │ │ -00026e80: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ -00026ea0: 3c2f 7370 616e 3e20 2865 6d70 7479 2829 (empty() │ │ │ │ -00026eb0: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    892 │ │ │ │ -00026f10: 2020 2020 3c73 7061 6e20 636c 6173 733d th │ │ │ │ -00026f30: 726f 773c 2f73 7061 6e3e 2073 7464 3a3a row std:: │ │ │ │ -00026f40: 7275 6e74 696d 655f 6572 726f 7228 3c2f runtime_error(.
    8 │ │ │ │ -00026fa0: 3933 3c2f 7370 616e 3e20 2020 2020 2020 93 │ │ │ │ -00026fb0: 2020 203c 7370 616e 2063 6c61 7373 3d22 & │ │ │ │ -00026fd0: 7175 6f74 3b44 6563 6973 696f 6e54 7265 quot;DecisionTre │ │ │ │ -00026fe0: 653a 3a61 7070 6c79 2875 6e61 7279 206f e::apply(unary o │ │ │ │ -00026ff0: 7029 2075 6e64 6566 696e 6564 2066 6f72 p) undefined for │ │ │ │ -00027000: 2065 6d70 7479 2074 7265 652e 2671 756f empty tree.&quo │ │ │ │ -00027010: 743b 3c2f 7370 616e 3e29 3b3c 2f64 6976 t;);
    .
    894< │ │ │ │ -00027070: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    895< │ │ │ │ -000270d0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> return De │ │ │ │ -00027130: 6369 7369 6f6e 5472 6565 3c2f 613e 2872 cisionTree(r │ │ │ │ -00027140: 6f6f 745f 2d26 6774 3b61 7070 6c79 286f oot_->apply(o │ │ │ │ -00027150: 7029 293b 3c2f 6469 763e 0a3c 6469 7620 p));
    .
    896 │ │ │ │ -000271b0: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ -000271c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ -00027250: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00027260: 6e6f 223e 2020 3839 393c 2f73 7061 6e3e no"> 899 │ │ │ │ -00027270: 2020 3c73 7061 6e20 636c 6173 733d 226b template │ │ │ │ -00027290: 3c2f 7370 616e 3e20 266c 743b 3c73 7061 <typename L, typen │ │ │ │ -000272e0: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ -000272f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    . │ │ │ │ -000274a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000274d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 901 const Un │ │ │ │ -00027520: 6172 7941 7373 6967 6e6d 656e 7426 616d aryAssignment&am │ │ │ │ -00027530: 703b 206f 7029 3c73 7061 6e20 636c 6173 p; op) con │ │ │ │ -00027550: 7374 203c 2f73 7061 6e3e 7b3c 2f64 6976 st {
    .
    902< │ │ │ │ -000275b0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> │ │ │ │ -000275d0: 2f2f 2049 7420 6973 2075 6e63 6c65 6172 // It is unclear │ │ │ │ -000275e0: 2077 6861 7420 7368 6f75 6c64 2068 6170 what should hap │ │ │ │ -000275f0: 7065 6e20 6966 2074 7265 6520 6973 2065 pen if tree is e │ │ │ │ -00027600: 6d70 7479 3a3c 2f73 7061 6e3e 3c2f 6469 mpty:.
    903 │ │ │ │ -00027660: 3c2f 7370 616e 3e20 2020 203c 7370 616e if ( │ │ │ │ -00027690: 656d 7074 7928 2929 207b 3c2f 6469 763e empty()) {
    │ │ │ │ -000276a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000276d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 904 throw std::runtime_e │ │ │ │ -00027730: 7272 6f72 283c 2f64 6976 3e0a 3c64 6976 rror(
    .
    < │ │ │ │ -00027770: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00027780: 6e6f 223e 2020 3930 353c 2f73 7061 6e3e no"> 905 │ │ │ │ -00027790: 2020 2020 2020 2020 2020 3c73 7061 6e20 "Deci │ │ │ │ -000277c0: 7369 6f6e 5472 6565 3a3a 6170 706c 7928 sionTree::apply( │ │ │ │ -000277d0: 756e 6172 7920 6f70 2920 756e 6465 6669 unary op) undefi │ │ │ │ -000277e0: 6e65 6420 666f 7220 656d 7074 7920 7472 ned for empty tr │ │ │ │ -000277f0: 6565 2e26 7175 6f74 3b3c 2f73 7061 6e3e ee." │ │ │ │ -00027800: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    906 │ │ │ │ -00027860: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    907 │ │ │ │ -000278c0: 203c 6120 636c 6173 733d 2263 6f64 6520 Ass │ │ │ │ -000278f0: 6967 6e6d 656e 7426 6c74 3b4c 2667 743b ignment<L> │ │ │ │ -00027900: 3c2f 613e 2061 7373 6967 6e6d 656e 743b assignment; │ │ │ │ -00027910: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00027960: 2039 3038 3c2f 7370 616e 3e20 2020 203c 908 < │ │ │ │ -00027970: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00027980: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ -00027990: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> DecisionTree< │ │ │ │ -000279d0: 2f61 3e28 726f 6f74 5f2d 2667 743b 6170 /a>(root_->ap │ │ │ │ -000279e0: 706c 7928 6f70 2c20 6173 7369 676e 6d65 ply(op, assignme │ │ │ │ -000279f0: 6e74 2929 3b3c 2f64 6976 3e0a 3c64 6976 nt));
    .
    < │ │ │ │ -00027a30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00027a40: 6e6f 223e 2020 3930 393c 2f73 7061 6e3e no"> 909 │ │ │ │ -00027a50: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -00027a60: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00027ad0: 6120 6964 3d22 6c30 3039 3131 2220 6e61 a id="l00911" na │ │ │ │ -00027ae0: 6d65 3d22 6c30 3039 3131 223e 3c2f 613e me="l00911"> │ │ │ │ -00027af0: 3c73 7061 6e20 636c 6173 733d 226c 696e 911 /****** │ │ │ │ -00027b30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00027b40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00027b50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00027b60: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00027b70: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    9 │ │ │ │ -00027bd0: 3132 3c2f 7370 616e 3e20 203c 7370 616e 12 template │ │ │ │ -00027c00: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ -00027c20: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ -00027c60: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ -00027c70: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ -00027c80: 6e30 3039 3133 2220 6461 7461 2d73 7461 n00913" data-sta │ │ │ │ -00027c90: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ -00027ca0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    . │ │ │ │ -00027e70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -00027f70: 2020 3931 353c 2f73 7061 6e3e 2020 2020 915 │ │ │ │ -00027f80: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // It is u │ │ │ │ -00027fa0: 6e63 6c65 6172 2077 6861 7420 7368 6f75 nclear what shou │ │ │ │ -00027fb0: 6c64 2068 6170 7065 6e20 6966 2065 6974 ld happen if eit │ │ │ │ -00027fc0: 6865 7220 7472 6565 2069 7320 656d 7074 her tree is empt │ │ │ │ -00027fd0: 793a 3c2f 7370 616e 3e3c 2f64 6976 3e0a y:
    . │ │ │ │ -00027fe0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ -000280d0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    918 │ │ │ │ -000281c0: 2020 2020 2020 2020 203c 7370 616e 2063 "Decis │ │ │ │ -000281f0: 696f 6e54 7265 653a 3a61 7070 6c79 2862 ionTree::apply(b │ │ │ │ -00028200: 696e 6172 7920 6f70 2920 756e 6465 6669 inary op) undefi │ │ │ │ -00028210: 6e65 6420 666f 7220 656d 7074 7920 7472 ned for empty tr │ │ │ │ -00028220: 6565 732e 2671 756f 743b 3c2f 7370 616e ees.");
    .
    919 │ │ │ │ -00028290: 2020 7d3c 2f64 6976 3e0a 3c64 6976 2063 }
    .
    920 │ │ │ │ -000282f0: 2020 3c73 7061 6e20 636c 6173 733d 2263 // apply │ │ │ │ -00028310: 2074 6865 206f 7065 7261 746f 6e20 6f6e the operaton on │ │ │ │ -00028320: 2074 6865 2072 6f6f 7420 6f66 2062 6f74 the root of bot │ │ │ │ -00028330: 6820 6469 6167 7261 6d73 3c2f 7370 616e h diagrams
    .
    │ │ │ │ -00028390: 2020 3932 313c 2f73 7061 6e3e 2020 2020 921 │ │ │ │ -000283a0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ -000283f0: 4e6f 6465 5074 723c 2f61 3e20 6820 3d20 NodePtr h = │ │ │ │ -00028400: 726f 6f74 5f2d 2667 743b 6170 706c 795f root_->apply_ │ │ │ │ -00028410: 665f 6f70 5f67 282a 672e 3c61 2063 6c61 f_op_g(*g.root_ │ │ │ │ -00028470: 3c2f 613e 2c20 6f70 293b 3c2f 6469 763e , op);
    │ │ │ │ -00028480: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -000284b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 922 / │ │ │ │ -000284f0: 2f20 6372 6561 7465 2061 206e 6577 2063 / create a new c │ │ │ │ -00028500: 6c61 7373 2077 6974 6820 7468 6520 7265 lass with the re │ │ │ │ -00028510: 7375 6c74 696e 6720 726f 6f74 2026 7175 sulting root &qu │ │ │ │ -00028520: 6f74 3b68 2671 756f 743b 3c2f 7370 616e ot;h"
    .
    │ │ │ │ -00028580: 2020 3932 333c 2f73 7061 6e3e 2020 2020 923 │ │ │ │ -00028590: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Deci │ │ │ │ -000285c0: 7369 6f6e 5472 6565 3c2f 613e 2072 6573 sionTree res │ │ │ │ -000285d0: 756c 7428 6829 3b3c 2f64 6976 3e0a 3c64 ult(h);
    . │ │ │ │ -000285f0: 3c61 2069 643d 226c 3030 3932 3422 206e 924 │ │ │ │ -00028650: 7265 7475 726e 3c2f 7370 616e 3e20 7265 return re │ │ │ │ -00028660: 7375 6c74 3b3c 2f64 6976 3e0a 3c64 6976 sult;
    .
    < │ │ │ │ -000286a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000286b0: 6e6f 223e 2020 3932 353c 2f73 7061 6e3e no"> 925 │ │ │ │ -000286c0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ -000286d0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00028740: 6120 6964 3d22 6c30 3039 3237 2220 6e61 a id="l00927" na │ │ │ │ -00028750: 6d65 3d22 6c30 3039 3237 223e 3c2f 613e me="l00927"> │ │ │ │ -00028760: 3c73 7061 6e20 636c 6173 733d 226c 696e 927 /****** │ │ │ │ -000287a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000287b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000287c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000287d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -000287e0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    9 │ │ │ │ -00028840: 3238 3c2f 7370 616e 3e20 203c 7370 616e 28 // The way this │ │ │ │ -00028870: 2077 6f72 6b73 3a3c 2f73 7061 6e3e 3c2f works:.
    9 │ │ │ │ -000288d0: 3239 3c2f 7370 616e 3e20 203c 7370 616e 29 // We have an A │ │ │ │ -00028900: 4454 2c20 7069 6374 7572 6520 6974 2061 DT, picture it a │ │ │ │ -00028910: 7320 6120 7472 6565 2e3c 2f73 7061 6e3e s a tree. │ │ │ │ -00028920: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00028970: 2039 3330 3c2f 7370 616e 3e20 203c 7370 930 // At a certa │ │ │ │ -000289a0: 696e 2064 6570 7468 2c20 7765 2068 6176 in depth, we hav │ │ │ │ -000289b0: 6520 6120 6272 616e 6368 206f 6e20 2671 e a branch on &q │ │ │ │ -000289c0: 756f 743b 6c61 6265 6c26 7175 6f74 3b2e uot;label". │ │ │ │ -000289d0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ -000289f0: 3c61 2069 643d 226c 3030 3933 3122 206e 931 // The │ │ │ │ -00028a50: 2066 756e 6374 696f 6e20 2671 756f 743b function " │ │ │ │ -00028a60: 6368 6f6f 7365 286c 6162 656c 2c69 6e64 choose(label,ind │ │ │ │ -00028a70: 6578 2926 7175 6f74 3b20 7769 6c6c 2072 ex)" will r │ │ │ │ -00028a80: 6574 7572 6e20 6120 7472 6565 206f 6620 eturn a tree of │ │ │ │ -00028a90: 6f6e 6520 6c65 7373 2064 6570 7468 2c3c one less depth,< │ │ │ │ -00028aa0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -00028ac0: 6120 6964 3d22 6c30 3039 3332 2220 6e61 a id="l00932" na │ │ │ │ -00028ad0: 6d65 3d22 6c30 3039 3332 223e 3c2f 613e me="l00932"> │ │ │ │ -00028ae0: 3c73 7061 6e20 636c 6173 733d 226c 696e 932 // wher │ │ │ │ -00028b20: 6520 7468 6572 6520 6973 206e 6f20 6d6f e there is no mo │ │ │ │ -00028b30: 7265 2062 7261 6e63 6820 6f6e 2026 7175 re branch on &qu │ │ │ │ -00028b40: 6f74 3b6c 6162 656c 2671 756f 743b 3a20 ot;label": │ │ │ │ -00028b50: 6f6e 6c79 2074 6865 2073 7562 7472 6565 only the subtree │ │ │ │ -00028b60: 2075 6e64 6572 2074 6861 743c 2f73 7061 under that
    .
    933 < │ │ │ │ -00028bd0: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00028be0: 656e 7422 3e2f 2f20 6272 616e 6368 2070 ent">// branch p │ │ │ │ -00028bf0: 6f69 6e74 2063 6f72 7265 7370 6f6e 6469 oint correspondi │ │ │ │ -00028c00: 6e67 2074 6f20 7468 6520 7661 6c75 6520 ng to the value │ │ │ │ -00028c10: 2671 756f 743b 696e 6465 7826 7175 6f74 "index" │ │ │ │ -00028c20: 3b20 6973 206c 6566 7420 696e 7374 6561 ; is left instea │ │ │ │ -00028c30: 642e 3c2f 7370 616e 3e3c 2f64 6976 3e0a d.
    . │ │ │ │ -00028c40: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ -00028d20: 3c61 2069 643d 226c 3030 3933 3522 206e 935 // Thi │ │ │ │ -00028d80: 7320 696d 706c 656d 656e 7473 206d 6172 s implements mar │ │ │ │ -00028d90: 6769 6e61 6c69 7a61 7469 6f6e 2069 6e20 ginalization in │ │ │ │ -00028da0: 4461 7277 6963 6865 3039 626f 6f6b 2c20 Darwiche09book, │ │ │ │ -00028db0: 7067 2033 3330 3c2f 7370 616e 3e3c 2f64 pg 330.
    93 │ │ │ │ -00028e10: 363c 2f73 7061 6e3e 2020 3c73 7061 6e20 6 │ │ │ │ -00028e30: 7465 6d70 6c61 7465 3c2f 7370 616e 3e26 template& │ │ │ │ -00028e40: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ -00028e60: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    ...
    938 │ │ │ │ -000290d0: 2020 2020 3c73 7061 6e20 636c 6173 733d si │ │ │ │ -000290f0: 7a65 5f74 3c2f 7370 616e 3e20 6361 7264 ze_t card │ │ │ │ -00029100: 696e 616c 6974 792c 203c 7370 616e 2063 inality, c │ │ │ │ -00029120: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ -00029130: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const { │ │ │ │ -00029160: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -000291b0: 2039 3339 3c2f 7370 616e 3e20 2020 203c 939 < │ │ │ │ -000291c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -000291d0: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ -000291e0: 3237 3936 2e68 746d 6c22 3e44 6563 6973 2796.html">Decis │ │ │ │ -000291f0: 696f 6e54 7265 653c 2f61 3e20 7265 7375 ionTree resu │ │ │ │ -00029200: 6c74 203d 2063 686f 6f73 6528 6c61 6265 lt = choose(labe │ │ │ │ -00029210: 6c2c 2030 293b 3c2f 6469 763e 0a3c 6469 l, 0);
    .< │ │ │ │ -00029230: 6120 6964 3d22 6c30 3039 3430 2220 6e61 a id="l00940" na │ │ │ │ -00029240: 6d65 3d22 6c30 3039 3430 223e 3c2f 613e me="l00940"> │ │ │ │ -00029250: 3c73 7061 6e20 636c 6173 733d 226c 696e 940 f │ │ │ │ -00029290: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (size_t index = 1; in │ │ │ │ -000292d0: 6465 7820 266c 743b 2063 6172 6469 6e61 dex < cardina │ │ │ │ -000292e0: 6c69 7479 3b20 696e 6465 782b 2b29 207b lity; index++) { │ │ │ │ -000292f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00029340: 2039 3431 3c2f 7370 616e 3e20 2020 2020 941 │ │ │ │ -00029350: 203c 6120 636c 6173 733d 2263 6f64 6520 Dec │ │ │ │ -00029380: 6973 696f 6e54 7265 653c 2f61 3e20 6368 isionTree ch │ │ │ │ -00029390: 6f73 656e 203d 2063 686f 6f73 6528 6c61 osen = choose(la │ │ │ │ -000293a0: 6265 6c2c 2069 6e64 6578 293b 3c2f 6469 bel, index);.
    942 │ │ │ │ -00029400: 3c2f 7370 616e 3e20 2020 2020 2072 6573 res │ │ │ │ -00029410: 756c 7420 3d20 7265 7375 6c74 2e61 7070 ult = result.app │ │ │ │ -00029420: 6c79 2863 686f 7365 6e2c 206f 7029 3b3c ly(chosen, op);< │ │ │ │ -00029430: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00029480: 3934 333c 2f73 7061 6e3e 2020 2020 7d3c 943 }< │ │ │ │ -00029490: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -000294e0: 3934 343c 2f73 7061 6e3e 2020 2020 3c73 944 return result;.
    94 │ │ │ │ -00029570: 353c 2f73 7061 6e3e 2020 7d3c 2f64 6976 5 }
    .
    .
    946 .
    9 │ │ │ │ -00029630: 3437 3c2f 7370 616e 3e3c 7370 616e 2063 47 │ │ │ │ -00029650: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ -00029660: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00029670: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00029680: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -00029690: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ -000296a0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -000296c0: 6120 6964 3d22 6c30 3039 3438 2220 6e61 a id="l00948" na │ │ │ │ -000296d0: 6d65 3d22 6c30 3039 3438 223e 3c2f 613e me="l00948"> │ │ │ │ -000296e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 948 templat │ │ │ │ -00029720: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename
    L, type │ │ │ │ -00029770: 6e61 6d65 3c2f 7370 616e 3e20 5926 6774 name Y> │ │ │ │ -00029780: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    . │ │ │ │ -000297e0: 3c61 2069 643d 226c 3030 3934 3922 206e 949 │ │ │ │ -00029860: 3c2f 613e 3c2f 7370 616e 3e20 203c 7370 void DecisionTree │ │ │ │ -000298f0: 266c 743b 4c2c 2059 2667 743b 3a3a 646f <L, Y>::do │ │ │ │ -00029900: 743c 2f61 3e28 7374 643a 3a6f 7374 7265 t(std::ostre │ │ │ │ -00029910: 616d 2661 6d70 3b20 6f73 2c3c 2f64 6976 am& os,
    .
    950< │ │ │ │ -00029970: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -00029980: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00029990: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ -000299b0: 3c2f 7370 616e 3e20 4c61 6265 6c46 6f72 LabelFor │ │ │ │ -000299c0: 6d61 7474 6572 2661 6d70 3b20 6c61 6265 matter& labe │ │ │ │ -000299d0: 6c46 6f72 6d61 7474 6572 2c3c 2f64 6976 lFormatter,
    .
    951< │ │ │ │ -00029a30: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -00029a40: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00029a50: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ -00029a70: 3c2f 7370 616e 3e20 5661 6c75 6546 6f72 ValueFor │ │ │ │ -00029a80: 6d61 7474 6572 2661 6d70 3b20 7661 6c75 matter& valu │ │ │ │ -00029a90: 6546 6f72 6d61 7474 6572 2c3c 2f64 6976 eFormatter,
    .
    952< │ │ │ │ -00029af0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ -00029b00: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -00029b10: 2020 2020 203c 7370 616e 2063 6c61 7373 b │ │ │ │ -00029b30: 6f6f 6c3c 2f73 7061 6e3e 2073 686f 775a ool showZ │ │ │ │ -00029b40: 6572 6f29 3c73 7061 6e20 636c 6173 733d ero) const │ │ │ │ -00029b60: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ -00029b70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ -00029c60: 2020 3935 343c 2f73 7061 6e3e 2020 2020 954 │ │ │ │ -00029c70: 726f 6f74 5f2d 2667 743b 646f 7428 6f73 root_->dot(os │ │ │ │ -00029c80: 2c20 6c61 6265 6c46 6f72 6d61 7474 6572 , labelFormatter │ │ │ │ -00029c90: 2c20 7661 6c75 6546 6f72 6d61 7474 6572 , valueFormatter │ │ │ │ -00029ca0: 2c20 7368 6f77 5a65 726f 293b 3c2f 6469 , showZero);.
    955 │ │ │ │ -00029d00: 3c2f 7370 616e 3e20 2020 206f 7320 266c os &l │ │ │ │ -00029d10: 743b 266c 743b 203c 7370 616e 2063 6c61 t;< " [order │ │ │ │ -00029d40: 696e 673d 6f75 745d 7d26 7175 6f74 3b3c ing=out]}"< │ │ │ │ -00029d50: 2f73 7061 6e3e 2026 6c74 3b26 6c74 3b20 /span> << │ │ │ │ -00029d60: 7374 643a 3a65 6e64 6c3b 3c2f 6469 763e std::endl;
    │ │ │ │ -00029d70: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -00029dd0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00029e20: 3935 373c 2f73 7061 6e3e 203c 2f64 6976 957
    .
    958< │ │ │ │ -00029e80: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ -00029ea0: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ -00029eb0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ -00029ed0: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ -00029f00: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ -00029fe0: 2020 3935 393c 2f61 3e3c 2f73 7061 6e3e 959 │ │ │ │ -00029ff0: 2020 3c73 7061 6e20 636c 6173 733d 226b void │ │ │ │ -0002a010: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Decisio │ │ │ │ -0002a070: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ -0002a080: 3b3a 3a64 6f74 3c2f 613e 283c 7370 616e ;::dot(const st │ │ │ │ -0002a0b0: 643a 3a73 7472 696e 6726 616d 703b 206e d::string& n │ │ │ │ -0002a0c0: 616d 652c 3c2f 6469 763e 0a3c 6469 7620 ame,
    .
    960 │ │ │ │ -0002a120: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0002a130: 2020 2020 2020 2020 2020 2020 2020 3c73 const │ │ │ │ -0002a160: 204c 6162 656c 466f 726d 6174 7465 7226 LabelFormatter& │ │ │ │ -0002a170: 616d 703b 206c 6162 656c 466f 726d 6174 amp; labelFormat │ │ │ │ -0002a180: 7465 722c 3c2f 6469 763e 0a3c 6469 7620 ter,
    .
    961 │ │ │ │ -0002a1e0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0002a1f0: 2020 2020 2020 2020 2020 2020 2020 3c73 const │ │ │ │ -0002a220: 2056 616c 7565 466f 726d 6174 7465 7226 ValueFormatter& │ │ │ │ -0002a230: 616d 703b 2076 616c 7565 466f 726d 6174 amp; valueFormat │ │ │ │ -0002a240: 7465 722c 3c2f 6469 763e 0a3c 6469 7620 ter,
    .
    962 │ │ │ │ -0002a2a0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0002a2b0: 2020 2020 2020 2020 2020 2020 2020 3c73 bool showZero) const {
    .
    963 │ │ │ │ -0002a370: 2073 7464 3a3a 6f66 7374 7265 616d 206f std::ofstream o │ │ │ │ -0002a380: 7328 286e 616d 6520 2b20 3c73 7061 6e20 s((name + ".dot │ │ │ │ -0002a3b0: 2671 756f 743b 3c2f 7370 616e 3e29 2e63 ").c │ │ │ │ -0002a3c0: 5f73 7472 2829 293b 3c2f 6469 763e 0a3c _str());
    .< │ │ │ │ -0002a3d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ -0002a3e0: 3e3c 6120 6964 3d22 6c30 3039 3634 2220 > 964 dot( │ │ │ │ -0002a480: 6f73 2c20 6c61 6265 6c46 6f72 6d61 7474 os, labelFormatt │ │ │ │ -0002a490: 6572 2c20 7661 6c75 6546 6f72 6d61 7474 er, valueFormatt │ │ │ │ -0002a4a0: 6572 2c20 7368 6f77 5a65 726f 293b 3c2f er, showZero);.
    9 │ │ │ │ -0002a500: 3635 3c2f 7370 616e 3e20 2020 203c 7370 65 int result =
    │ │ │ │ -0002a540: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0002a570: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 966 sys │ │ │ │ -0002a5a0: 7465 6d28 283c 7370 616e 2063 6c61 7373 tem(("dot -Tpdf │ │ │ │ -0002a5d0: 2026 7175 6f74 3b3c 2f73 7061 6e3e 202b " + │ │ │ │ -0002a5e0: 206e 616d 6520 2b20 3c73 7061 6e20 636c name + ".dot - │ │ │ │ -0002a610: 6f20 2671 756f 743b 3c2f 7370 616e 3e20 o " │ │ │ │ -0002a620: 2b20 6e61 6d65 202b 203c 7370 616e 2063 + name + ".pdf │ │ │ │ -0002a650: 2667 743b 2661 6d70 3b20 2f64 6576 2f6e >& /dev/n │ │ │ │ -0002a660: 756c 6c26 7175 6f74 3b3c 2f73 7061 6e3e ull" │ │ │ │ -0002a670: 293c 2f64 6976 3e0a 3c64 6976 2063 6c61 )
    .
    │ │ │ │ -0002a6c0: 2020 3936 373c 2f73 7061 6e3e 2020 2020 967 │ │ │ │ -0002a6d0: 2020 2020 2020 2020 2020 2020 2020 202e . │ │ │ │ -0002a6e0: 635f 7374 7228 2929 3b3c 2f64 6976 3e0a c_str());
    . │ │ │ │ -0002a6f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    │ │ │ │ -0002a780: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -0002a7b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 969 throw std::runtime_e │ │ │ │ -0002a810: 7272 6f72 283c 7370 616e 2063 6c61 7373 rror("DecisionT │ │ │ │ -0002a840: 7265 653a 3a64 6f74 2073 7973 7465 6d20 ree::dot system │ │ │ │ -0002a850: 6361 6c6c 2066 6169 6c65 6426 7175 6f74 call failed" │ │ │ │ -0002a860: 3b3c 2f73 7061 6e3e 293b 3c2f 6469 763e ;);
    │ │ │ │ -0002a870: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ -0002a8d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -0002a920: 3937 313c 2f73 7061 6e3e 203c 2f64 6976 971
    .
    972< │ │ │ │ -0002a980: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ -0002a9a0: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ -0002a9b0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ -0002a9d0: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ -0002aa00: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ -0002aae0: 2020 3937 333c 2f61 3e3c 2f73 7061 6e3e 973 │ │ │ │ -0002aaf0: 2020 7374 643a 3a73 7472 696e 6720 3c61 std::string D │ │ │ │ -0002ab50: 6563 6973 696f 6e54 7265 6526 6c74 3b4c ecisionTree<L │ │ │ │ -0002ab60: 2c20 5926 6774 3b3a 3a64 6f74 3c2f 613e , Y>::dot │ │ │ │ -0002ab70: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const LabelFormatt │ │ │ │ -0002aba0: 6572 2661 6d70 3b20 6c61 6265 6c46 6f72 er& labelFor │ │ │ │ -0002abb0: 6d61 7474 6572 2c3c 2f64 6976 3e0a 3c64 matter,
    . │ │ │ │ -0002abd0: 3c61 2069 643d 226c 3030 3937 3422 206e 974 │ │ │ │ -0002ac20: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0002ac30: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ -0002ac50: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ -0002ac60: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ -0002ac70: 616c 7565 466f 726d 6174 7465 722c 3c2f alueFormatter,.
    9 │ │ │ │ -0002acd0: 3735 3c2f 7370 616e 3e20 2020 2020 2020 75 │ │ │ │ -0002ace0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ -0002acf0: 2020 2020 2020 2020 2020 2020 2020 203c < │ │ │ │ -0002ad00: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -0002ad10: 6f72 6474 7970 6522 3e62 6f6f 6c3c 2f73 ordtype">bool showZero) const {
    .
    976 │ │ │ │ -0002adb0: 2020 7374 643a 3a73 7472 696e 6773 7472 std::stringstr │ │ │ │ -0002adc0: 6561 6d20 7373 3b3c 2f64 6976 3e0a 3c64 eam ss;
    . │ │ │ │ -0002ade0: 3c61 2069 643d 226c 3030 3937 3722 206e 977 dot(s │ │ │ │ -0002ae80: 732c 206c 6162 656c 466f 726d 6174 7465 s, labelFormatte │ │ │ │ -0002ae90: 722c 2076 616c 7565 466f 726d 6174 7465 r, valueFormatte │ │ │ │ -0002aea0: 722c 2073 686f 775a 6572 6f29 3b3c 2f64 r, showZero);.
    97 │ │ │ │ -0002af00: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 return ss.str();.
    97 │ │ │ │ -0002af90: 393c 2f73 7061 6e3e 2020 7d3c 2f64 6976 9 }
    .
    .
    980 .
    9 │ │ │ │ -0002b050: 3831 3c2f 7370 616e 3e3c 7370 616e 2063 81/ │ │ │ │ -0002b070: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0002b080: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0002b090: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0002b0a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ -0002b0b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ -0002b0c0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ -0002b0e0: 6120 6964 3d22 6c30 3039 3832 2220 6e61 a id="l00982" na │ │ │ │ -0002b0f0: 6d65 3d22 6c30 3039 3832 223e 3c2f 613e me="l00982"> │ │ │ │ -0002b100: 3c73 7061 6e20 636c 6173 733d 226c 696e 982
    .
    983 } │ │ │ │ -0002b180: 2020 3c73 7061 6e20 636c 6173 733d 2263 // names │ │ │ │ -0002b1a0: 7061 6365 2067 7473 616d 3c2f 7370 616e pace gtsam
    .
    .< │ │ │ │ -0002b1c0: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ..
    .
    .
    .
    . │ │ │ │ -0002b1f0: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c2f
    .
    ..
    ..
    .
    │ │ │ │ -0002b220: 0a3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c .
    .
    .< │ │ │ │ -0002b230: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ..
    .
    .
    .
    . │ │ │ │ -0002b260: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c2f
    .
    ..
    ..
    .
    │ │ │ │ -0002b290: 0a3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c .
    .
    .< │ │ │ │ -0002b2a0: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ...
    Global function │ │ │ │ -0002b3e0: 7320 696e 2061 2073 6570 6172 6174 6520 s in a separate │ │ │ │ -0002b3f0: 7465 7374 696e 6720 6e61 6d65 7370 6163 testing namespac │ │ │ │ -0002b400: 652e 3c2f 6469 763e 3c64 6976 2063 6c61 e.
    De │ │ │ │ -0002b420: 6669 6e69 7469 6f6e 3c2f 623e 2063 6861 finition cha │ │ │ │ -0002b430: 7274 5465 7374 696e 672e 683a 3238 3c2f rtTesting.h:28
    .
    │ │ │ │ -0002b490: 3c64 6976 2063 6c61 7373 3d22 7474 6e61
    gtsa │ │ │ │ -0002b4e0: 6d3a 3a73 706c 6974 3c2f 613e 3c2f 6469 m::split
    void split │ │ │ │ -0002b510: 2863 6f6e 7374 2047 2026 616d 703b 672c (const G &g, │ │ │ │ -0002b520: 2063 6f6e 7374 2050 7265 6465 6365 7373 const Predecess │ │ │ │ -0002b530: 6f72 4d61 7026 6c74 3b20 4b45 5920 2667 orMap< KEY &g │ │ │ │ -0002b540: 743b 2026 616d 703b 7472 6565 2c20 4720 t; &tree, G │ │ │ │ -0002b550: 2661 6d70 3b41 6231 2c20 4720 2661 6d70 &Ab1, G & │ │ │ │ -0002b560: 3b41 6232 293c 2f64 6976 3e3c 6469 7620 ;Ab2)
    Sp │ │ │ │ -0002b580: 6c69 7420 7468 6520 6772 6170 6820 696e lit the graph in │ │ │ │ -0002b590: 746f 2074 776f 2070 6172 7473 3a20 6f6e to two parts: on │ │ │ │ -0002b5a0: 6520 636f 7272 6573 706f 6e64 7320 746f e corresponds to │ │ │ │ -0002b5b0: 2074 6865 2067 6976 656e 2073 7061 6e6e the given spann │ │ │ │ -0002b5c0: 696e 6720 7472 6565 2c20 616e 6420 7468 ing tree, and th │ │ │ │ -0002b5d0: 6520 6f74 6865 7220 636f 7272 6573 706f e other correspo │ │ │ │ -0002b5e0: 6e64 732e 2e2e 3c2f 6469 763e 3c64 6976 nds...
    < │ │ │ │ -0002b600: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -0002b610: 2067 7261 7068 2d69 6e6c 2e68 3a32 3535 graph-inl.h:255 │ │ │ │ -0002b620: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ -0002b6c0: 7361 6d3a 3a64 6f74 3c2f 613e 3c2f 6469 sam::dot
    double dot │ │ │ │ -0002b6f0: 2863 6f6e 7374 2056 3120 2661 6d70 3b61 (const V1 &a │ │ │ │ -0002b700: 2c20 636f 6e73 7420 5632 2026 616d 703b , const V2 & │ │ │ │ -0002b710: 6229 3c2f 6469 763e 3c64 6976 2063 6c61 b)
    Dot p │ │ │ │ -0002b730: 726f 6475 6374 2e3c 2f64 6976 3e3c 6469 roduct.
    │ │ │ │ -0002b750: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition Vector.h:195
    .< │ │ │ │ -0002b7e0: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ -0002b7f0: 223e 5465 6d70 6c61 7465 2074 6f20 6372 ">Template to cr │ │ │ │ -0002b800: 6561 7465 2061 2062 696e 6172 7920 7072 eate a binary pr │ │ │ │ -0002b810: 6564 6963 6174 652e 3c2f 6469 763e 3c64 edicate.
    Definition Testable.h:11 │ │ │ │ -0002b850: 313c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 1
    .
    gtsam │ │ │ │ -0002b8b0: 3a3a 4173 7369 676e 6d65 6e74 3c2f 613e ::Assignment │ │ │ │ -0002b8c0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    An assi │ │ │ │ -0002b8e0: 676e 6d65 6e74 2066 726f 6d20 6c61 6265 gnment from labe │ │ │ │ -0002b8f0: 6c73 2074 6f20 7661 6c75 6520 696e 6465 ls to value inde │ │ │ │ -0002b900: 7820 2873 697a 655f 7429 2e3c 2f64 6976 x (size_t).
    Definitio │ │ │ │ -0002b930: 6e3c 2f62 3e20 4173 7369 676e 6d65 6e74 n Assignment │ │ │ │ -0002b940: 2e68 3a33 373c 2f64 6976 3e3c 2f64 6976 .h:37
    .
    g │ │ │ │ -0002b9a0: 7473 616d 3a3a 4465 6369 7369 6f6e 5472 tsam::DecisionTr │ │ │ │ -0002b9b0: 6565 3a3a 4c65 6166 3c2f 613e 3c2f 6469 ee::Leaf
    Definiti │ │ │ │ -0002b9e0: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ -0002b9f0: 7265 652d 696e 6c2e 683a 3532 3c2f 6469 ree-inl.h:52
    .
    gtsam: │ │ │ │ -0002baa0: 3a44 6563 6973 696f 6e54 7265 653a 3a4c :DecisionTree::L │ │ │ │ -0002bab0: 6561 663a 3a63 686f 6f73 653c 2f61 3e3c eaf::choose< │ │ │ │ -0002bac0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    NodePtr │ │ │ │ -0002bae0: 2063 686f 6f73 6528 636f 6e73 7420 4c20 choose(const L │ │ │ │ -0002baf0: 2661 6d70 3b6c 6162 656c 2c20 7369 7a65 &label, size │ │ │ │ -0002bb00: 5f74 2069 6e64 6578 2920 636f 6e73 7420 _t index) const │ │ │ │ -0002bb10: 6f76 6572 7269 6465 3c2f 6469 763e 3c64 override
    choose a branch │ │ │ │ -0002bb40: 2c20 6372 6561 7465 206e 6577 206d 656d , create new mem │ │ │ │ -0002bb50: 6f72 7920 213c 2f64 6976 3e3c 6469 7620 ory !
    Definition │ │ │ │ -0002bb80: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -0002bb90: 2e68 3a31 3439 3c2f 6469 763e 3c2f 6469 .h:149
    .
    gtsam::Decis │ │ │ │ -0002bc40: 696f 6e54 7265 653a 3a4c 6561 663a 3a6f ionTree::Leaf::o │ │ │ │ -0002bc50: 7065 7261 746f 7228 293c 2f61 3e3c 2f64 perator()
    const Y & │ │ │ │ -0002bc80: 616d 703b 206f 7065 7261 746f 7228 2928 amp; operator()( │ │ │ │ -0002bc90: 636f 6e73 7420 4173 7369 676e 6d65 6e74 const Assignment │ │ │ │ -0002bca0: 266c 743b 204c 2026 6774 3b20 2661 6d70 < L > & │ │ │ │ -0002bcb0: 3b78 2920 636f 6e73 7420 6f76 6572 7269 ;x) const overri │ │ │ │ -0002bcc0: 6465 3c2f 6469 763e 3c64 6976 2063 6c61 de
    evalu │ │ │ │ -0002bce0: 6174 653c 2f64 6976 3e3c 6469 7620 636c ate
    D │ │ │ │ -0002bd00: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -0002bd10: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ -0002bd20: 3a31 3130 3c2f 6469 763e 3c2f 6469 763e :110
    │ │ │ │ -0002bd30: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    │ │ │ │ -0002be00: 4e6f 6465 5074 7220 6170 706c 7928 636f NodePtr apply(co │ │ │ │ -0002be10: 6e73 7420 556e 6172 7941 7373 6967 6e6d nst UnaryAssignm │ │ │ │ -0002be20: 656e 7420 2661 6d70 3b6f 702c 2063 6f6e ent &op, con │ │ │ │ -0002be30: 7374 2041 7373 6967 6e6d 656e 7426 6c74 st Assignment< │ │ │ │ -0002be40: 3b20 4c20 2667 743b 2026 616d 703b 6173 ; L > &as │ │ │ │ -0002be50: 7369 676e 6d65 6e74 2920 636f 6e73 7420 signment) const │ │ │ │ -0002be60: 6f76 6572 7269 6465 3c2f 6469 763e 3c64 override
    Apply unary ope │ │ │ │ -0002be90: 7261 746f 7220 7769 7468 2061 7373 6967 rator with assig │ │ │ │ -0002bea0: 6e6d 656e 742e 3c2f 6469 763e 3c64 6976 nment.
    < │ │ │ │ -0002bec0: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -0002bed0: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ -0002bee0: 6c2e 683a 3132 313c 2f64 6976 3e3c 2f64 l.h:121
    .
    │ │ │ │ -0002bfb0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    bool equals( │ │ │ │ -0002bfd0: 636f 6e73 7420 4e6f 6465 2026 616d 703b const Node & │ │ │ │ -0002bfe0: 712c 2063 6f6e 7374 2043 6f6d 7061 7265 q, const Compare │ │ │ │ -0002bff0: 4675 6e63 2026 616d 703b 636f 6d70 6172 Func &compar │ │ │ │ -0002c000: 6529 2063 6f6e 7374 206f 7665 7272 6964 e) const overrid │ │ │ │ -0002c010: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    equali │ │ │ │ -0002c030: 7479 2075 7020 746f 2074 6f6c 6572 616e ty up to toleran │ │ │ │ -0002c040: 6365 3c2f 6469 763e 3c64 6976 2063 6c61 ce
    De │ │ │ │ -0002c060: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ -0002c070: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ -0002c080: 3837 3c2f 6469 763e 3c2f 6469 763e 0a3c 87
    .< │ │ │ │ -0002c090: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -0002c0a0: 6964 3d22 6161 3032 3737 365f 6874 6d6c id="aa02776_html │ │ │ │ -0002c0b0: 5f61 3763 3231 6461 6661 6262 3566 6261 _a7c21dafabb5fba │ │ │ │ -0002c0c0: 3661 3661 6263 6362 3839 6564 3230 3739 6a6abccb89ed2079 │ │ │ │ -0002c0d0: 3563 223e 3c64 6976 2063 6c61 7373 3d22 5c">Y constant_
    constant s │ │ │ │ -0002c190: 746f 7265 6420 696e 2074 6869 7320 6c65 tored in this le │ │ │ │ -0002c1a0: 6166 3c2f 6469 763e 3c64 6976 2063 6c61 af
    De │ │ │ │ -0002c1c0: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ -0002c1d0: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ -0002c1e0: 3534 3c2f 6469 763e 3c2f 6469 763e 0a3c 54
    .< │ │ │ │ -0002c1f0: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -0002c200: 6964 3d22 6161 3032 3737 365f 6874 6d6c id="aa02776_html │ │ │ │ -0002c210: 5f61 3835 6536 3464 6336 6431 6434 6534 _a85e64dc6d1d4e4 │ │ │ │ -0002c220: 3063 6135 3639 6439 3739 6236 3535 3433 0ca569d979b65543 │ │ │ │ -0002c230: 6535 223e 3c64 6976 2063 6c61 7373 3d22 e5">
    vo │ │ │ │ -0002c2c0: 6964 2070 7269 6e74 2863 6f6e 7374 2073 id print(const s │ │ │ │ -0002c2d0: 7464 3a3a 7374 7269 6e67 2026 616d 703b td::string & │ │ │ │ -0002c2e0: 732c 2063 6f6e 7374 204c 6162 656c 466f s, const LabelFo │ │ │ │ -0002c2f0: 726d 6174 7465 7220 2661 6d70 3b6c 6162 rmatter &lab │ │ │ │ -0002c300: 656c 466f 726d 6174 7465 722c 2063 6f6e elFormatter, con │ │ │ │ -0002c310: 7374 2056 616c 7565 466f 726d 6174 7465 st ValueFormatte │ │ │ │ -0002c320: 7220 2661 6d70 3b76 616c 7565 466f 726d r &valueForm │ │ │ │ -0002c330: 6174 7465 7229 2063 6f6e 7374 206f 7665 atter) const ove │ │ │ │ -0002c340: 7272 6964 653c 2f64 6976 3e3c 6469 7620 rride
    pr │ │ │ │ -0002c360: 696e 743c 2f64 6976 3e3c 6469 7620 636c int
    D │ │ │ │ -0002c380: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -0002c390: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ -0002c3a0: 3a39 343c 2f64 6976 3e3c 2f64 6976 3e0a :94
    . │ │ │ │ -0002c3b0: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    N │ │ │ │ -0002c480: 6f64 6550 7472 2061 7070 6c79 2863 6f6e odePtr apply(con │ │ │ │ -0002c490: 7374 2055 6e61 7279 2026 616d 703b 6f70 st Unary &op │ │ │ │ -0002c4a0: 2920 636f 6e73 7420 6f76 6572 7269 6465 ) const override │ │ │ │ -0002c4b0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    apply u │ │ │ │ -0002c4d0: 6e61 7279 206f 7065 7261 746f 723c 2f64 nary operator
    Definit │ │ │ │ -0002c500: 696f 6e3c 2f62 3e20 4465 6369 7369 6f6e ion Decision │ │ │ │ -0002c510: 5472 6565 2d69 6e6c 2e68 3a31 3135 3c2f Tree-inl.h:115
    .
    │ │ │ │ -0002c570: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 .
    < │ │ │ │ -0002c760: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ -0002c770: 6922 3e4c 6561 6628 636f 6e73 7420 5920 i">Leaf(const Y │ │ │ │ -0002c780: 2661 6d70 3b63 6f6e 7374 616e 742c 2073 &constant, s │ │ │ │ -0002c790: 697a 655f 7420 6e72 4173 7369 676e 6d65 ize_t nrAssignme │ │ │ │ -0002c7a0: 6e74 733d 3129 3c2f 6469 763e 3c64 6976 nts=1)
    C │ │ │ │ -0002c7c0: 6f6e 7374 7275 6374 6f72 2066 726f 6d20 onstructor from │ │ │ │ -0002c7d0: 636f 6e73 7461 6e74 2e3c 2f64 6976 3e3c constant.
    < │ │ │ │ -0002c7e0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ -0002c7f0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ -0002c800: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ -0002c810: 2d69 6e6c 2e68 3a36 353c 2f64 6976 3e3c -inl.h:65
    < │ │ │ │ -0002c820: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    < │ │ │ │ -0002c880: 6120 6872 6566 3d22 6130 3237 3736 2e68 a href="a02776.h │ │ │ │ -0002c890: 746d 6c23 6162 3932 3036 3930 3338 3030 tml#ab9206903800 │ │ │ │ -0002c8a0: 3830 3735 3165 6138 3037 6535 6233 6365 80751ea807e5b3ce │ │ │ │ -0002c8b0: 3962 3638 3322 3e67 7473 616d 3a3a 4465 9b683">gtsam::De │ │ │ │ -0002c8c0: 6369 7369 6f6e 5472 6565 3a3a 4c65 6166 cisionTree::Leaf │ │ │ │ -0002c8d0: 3a3a 6e72 4173 7369 676e 6d65 6e74 735f ::nrAssignments_ │ │ │ │ -0002c8e0: 3c2f 613e 3c2f 6469 763e 3c64 6976 2063
    si │ │ │ │ -0002c900: 7a65 5f74 206e 7241 7373 6967 6e6d 656e ze_t nrAssignmen │ │ │ │ -0002c910: 7473 5f3c 2f64 6976 3e3c 6469 7620 636c ts_
    The │ │ │ │ -0002c930: 6e75 6d62 6572 206f 6620 6173 7369 676e number of assign │ │ │ │ -0002c940: 6d65 6e74 7320 636f 6e74 6169 6e65 6420 ments contained │ │ │ │ -0002c950: 7769 7468 696e 2074 6869 7320 6c65 6166 within this leaf │ │ │ │ -0002c960: 2e3c 2f64 6976 3e3c 6469 7620 636c 6173 .
    Def │ │ │ │ -0002c980: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ -0002c990: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a35 sionTree-inl.h:5 │ │ │ │ -0002c9a0: 393c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 9
    .
    void │ │ │ │ -0002ca80: 646f 7428 7374 643a 3a6f 7374 7265 616d dot(std::ostream │ │ │ │ -0002ca90: 2026 616d 703b 6f73 2c20 636f 6e73 7420 &os, const │ │ │ │ -0002caa0: 4c61 6265 6c46 6f72 6d61 7474 6572 2026 LabelFormatter & │ │ │ │ -0002cab0: 616d 703b 6c61 6265 6c46 6f72 6d61 7474 amp;labelFormatt │ │ │ │ -0002cac0: 6572 2c20 636f 6e73 7420 5661 6c75 6546 er, const ValueF │ │ │ │ -0002cad0: 6f72 6d61 7474 6572 2026 616d 703b 7661 ormatter &va │ │ │ │ -0002cae0: 6c75 6546 6f72 6d61 7474 6572 2c20 626f lueFormatter, bo │ │ │ │ -0002caf0: 6f6c 2073 686f 775a 6572 6f29 2063 6f6e ol showZero) con │ │ │ │ -0002cb00: 7374 206f 7665 7272 6964 653c 2f64 6976 st override
    Write graphv │ │ │ │ -0002cb30: 697a 2066 6f72 6d61 7420 746f 2073 7472 iz format to str │ │ │ │ -0002cb40: 6561 6d20 6f73 2e3c 2f64 6976 3e3c 6469 eam os.
    │ │ │ │ -0002cb60: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ -0002cb80: 6e6c 2e68 3a31 3030 3c2f 6469 763e 3c2f nl.h:100
    .
    < │ │ │ │ -0002cc50: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ -0002cc60: 6922 3e4c 6561 6628 293c 2f64 6976 3e3c i">Leaf()
    < │ │ │ │ -0002cc70: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ -0002cc80: 223e 4465 6661 756c 7420 636f 6e73 7472 ">Default constr │ │ │ │ -0002cc90: 7563 746f 7220 666f 7220 7365 7269 616c uctor for serial │ │ │ │ -0002cca0: 697a 6174 696f 6e2e 3c2f 6469 763e 3c64 ization.
    Definition DecisionTree- │ │ │ │ -0002cce0: 696e 6c2e 683a 3632 3c2f 6469 763e 3c2f inl.h:62
    .
    gtsam::Dec │ │ │ │ -0002cd90: 6973 696f 6e54 7265 653a 3a4c 6561 663a isionTree::Leaf: │ │ │ │ -0002cda0: 3a73 616d 654c 6561 663c 2f61 3e3c 2f64 :sameLeaf
    bool same │ │ │ │ -0002cdd0: 4c65 6166 2863 6f6e 7374 204e 6f64 6520 Leaf(const Node │ │ │ │ -0002cde0: 2661 6d70 3b71 2920 636f 6e73 7420 6f76 &q) const ov │ │ │ │ -0002cdf0: 6572 7269 6465 3c2f 6469 763e 3c64 6976 erride
    p │ │ │ │ -0002ce10: 6f6c 796d 6f72 7068 6963 2065 7175 616c olymorphic equal │ │ │ │ -0002ce20: 6974 793a 2069 7320 7120 6120 6c65 6166 ity: is q a leaf │ │ │ │ -0002ce30: 2061 6e64 2069 7320 6974 2074 6865 2073 and is it the s │ │ │ │ -0002ce40: 616d 6520 6173 2074 6869 7320 6c65 6166 ame as this leaf │ │ │ │ -0002ce50: 3f3c 2f64 6976 3e3c 6469 7620 636c 6173 ?
    Def │ │ │ │ -0002ce70: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ -0002ce80: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a38 sionTree-inl.h:8 │ │ │ │ -0002ce90: 323c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 2
    .
    │ │ │ │ -0002cf70: 636f 6e73 7420 5920 2661 6d70 3b20 636f const Y & co │ │ │ │ -0002cf80: 6e73 7461 6e74 2829 2063 6f6e 7374 3c2f nstant() const
    Return th │ │ │ │ -0002cfb0: 6520 636f 6e73 7461 6e74 2e3c 2f64 6976 e constant.
    Definitio │ │ │ │ -0002cfe0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -0002cff0: 6565 2d69 6e6c 2e68 3a36 393c 2f64 6976 ee-inl.h:69
    .
    s │ │ │ │ -0002d0e0: 697a 655f 7420 6e72 4173 7369 676e 6d65 ize_t nrAssignme │ │ │ │ -0002d0f0: 6e74 7328 2920 636f 6e73 743c 2f64 6976 nts() const
    Return the n │ │ │ │ -0002d120: 756d 6265 7220 6f66 2061 7373 6967 6e6d umber of assignm │ │ │ │ -0002d130: 656e 7473 2063 6f6e 7461 696e 6564 2077 ents contained w │ │ │ │ -0002d140: 6974 6869 6e20 7468 6973 206c 6561 662e ithin this leaf. │ │ │ │ -0002d150: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ -0002d170: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ -0002d180: 696f 6e54 7265 652d 696e 6c2e 683a 3734 ionTree-inl.h:74 │ │ │ │ -0002d190: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    . │ │ │ │ -0002d1c0: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 < │ │ │ │ -0002d210: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ -0002d220: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ -0002d230: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ -0002d240: 2d69 6e6c 2e68 3a31 3732 3c2f 6469 763e -inl.h:172
    │ │ │ │ -0002d250: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0002d2b0: 3c61 2068 7265 663d 2261 3032 3738 302e gtsam::D │ │ │ │ -0002d2f0: 6563 6973 696f 6e54 7265 653a 3a43 686f ecisionTree::Cho │ │ │ │ -0002d300: 6963 653a 3a61 7070 6c79 3c2f 613e 3c2f ice::apply
    NodePtr │ │ │ │ -0002d330: 6170 706c 7928 636f 6e73 7420 556e 6172 apply(const Unar │ │ │ │ -0002d340: 7920 2661 6d70 3b6f 7029 2063 6f6e 7374 y &op) const │ │ │ │ -0002d350: 206f 7665 7272 6964 653c 2f64 6976 3e3c override
    < │ │ │ │ -0002d360: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ -0002d370: 223e 6170 706c 7920 756e 6172 7920 6f70 ">apply unary op │ │ │ │ -0002d380: 6572 6174 6f72 2e3c 2f64 6976 3e3c 6469 erator.
    │ │ │ │ -0002d3a0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ -0002d3c0: 6e6c 2e68 3a33 3839 3c2f 6469 763e 3c2f nl.h:389
    .
    gtsam::Dec │ │ │ │ -0002d470: 6973 696f 6e54 7265 653a 3a43 686f 6963 isionTree::Choic │ │ │ │ -0002d480: 653a 3a43 686f 6963 653c 2f61 3e3c 2f64 e::Choice
    Choice(co │ │ │ │ -0002d4b0: 6e73 7420 4c20 2661 6d70 3b6c 6162 656c nst L &label │ │ │ │ -0002d4c0: 2c20 636f 6e73 7420 4368 6f69 6365 2026 , const Choice & │ │ │ │ -0002d4d0: 616d 703b 662c 2063 6f6e 7374 2055 6e61 amp;f, const Una │ │ │ │ -0002d4e0: 7279 4173 7369 676e 6d65 6e74 2026 616d ryAssignment &am │ │ │ │ -0002d4f0: 703b 6f70 2c20 636f 6e73 7420 4173 7369 p;op, const Assi │ │ │ │ -0002d500: 676e 6d65 6e74 266c 743b 204c 2026 6774 gnment< L > │ │ │ │ -0002d510: 3b20 2661 6d70 3b61 7373 6967 6e6d 656e ; &assignmen │ │ │ │ -0002d520: 7429 3c2f 6469 763e 3c64 6976 2063 6c61 t)
    Const │ │ │ │ -0002d540: 7275 6374 6f72 2077 6869 6368 2061 6363 ructor which acc │ │ │ │ -0002d550: 6570 7473 2061 2055 6e61 7279 4173 7369 epts a UnaryAssi │ │ │ │ -0002d560: 676e 6d65 6e74 206f 7020 616e 6420 7468 gnment op and th │ │ │ │ -0002d570: 6520 636f 7272 6573 706f 6e64 696e 6720 e corresponding │ │ │ │ -0002d580: 6173 7369 676e 6d65 6e74 2e3c 2f64 6976 assignment.
    Definitio │ │ │ │ -0002d5b0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -0002d5c0: 6565 2d69 6e6c 2e68 3a33 3639 3c2f 6469 ee-inl.h:369
    .
    const │ │ │ │ -0002d6b0: 4c20 2661 6d70 3b20 6c61 6265 6c28 2920 L & label() │ │ │ │ -0002d6c0: 636f 6e73 743c 2f64 6976 3e3c 6469 7620 const
    Re │ │ │ │ -0002d6e0: 7475 726e 2074 6865 206c 6162 656c 206f turn the label o │ │ │ │ -0002d6f0: 6620 7468 6973 2063 686f 6963 6520 6e6f f this choice no │ │ │ │ -0002d700: 6465 2e3c 2f64 6976 3e3c 6469 7620 636c de.
    D │ │ │ │ -0002d720: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -0002d730: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ -0002d740: 3a32 3538 3c2f 6469 763e 3c2f 6469 763e :258
    │ │ │ │ -0002d750: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    void print(con │ │ │ │ -0002d830: 7374 2073 7464 3a3a 7374 7269 6e67 2026 st std::string & │ │ │ │ -0002d840: 616d 703b 732c 2063 6f6e 7374 204c 6162 amp;s, const Lab │ │ │ │ -0002d850: 656c 466f 726d 6174 7465 7220 2661 6d70 elFormatter & │ │ │ │ -0002d860: 3b6c 6162 656c 466f 726d 6174 7465 722c ;labelFormatter, │ │ │ │ -0002d870: 2063 6f6e 7374 2056 616c 7565 466f 726d const ValueForm │ │ │ │ -0002d880: 6174 7465 7220 2661 6d70 3b76 616c 7565 atter &value │ │ │ │ -0002d890: 466f 726d 6174 7465 7229 2063 6f6e 7374 Formatter) const │ │ │ │ -0002d8a0: 206f 7665 7272 6964 653c 2f64 6976 3e3c override
    < │ │ │ │ -0002d8b0: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ -0002d8c0: 223e 7072 696e 7420 2861 7320 6120 7472 ">print (as a tr │ │ │ │ -0002d8d0: 6565 292e 3c2f 6469 763e 3c64 6976 2063 ee).
    │ │ │ │ -0002d8f0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -0002d900: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ -0002d910: 683a 3238 303c 2f64 6976 3e3c 2f64 6976 h:280
    .
    < │ │ │ │ -0002d9e0: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ -0002d9f0: 6922 3e4e 6f64 6550 7472 2061 7070 6c79 i">NodePtr apply │ │ │ │ -0002da00: 2863 6f6e 7374 2055 6e61 7279 4173 7369 (const UnaryAssi │ │ │ │ -0002da10: 676e 6d65 6e74 2026 616d 703b 6f70 2c20 gnment &op, │ │ │ │ -0002da20: 636f 6e73 7420 4173 7369 676e 6d65 6e74 const Assignment │ │ │ │ -0002da30: 266c 743b 204c 2026 6774 3b20 2661 6d70 < L > & │ │ │ │ -0002da40: 3b61 7373 6967 6e6d 656e 7429 2063 6f6e ;assignment) con │ │ │ │ -0002da50: 7374 206f 7665 7272 6964 653c 2f64 6976 st override
    Apply unary │ │ │ │ -0002da80: 6f70 6572 6174 6f72 2077 6974 6820 6173 operator with as │ │ │ │ -0002da90: 7369 676e 6d65 6e74 2e3c 2f64 6976 3e3c signment.
    < │ │ │ │ -0002daa0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ -0002dab0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ -0002dac0: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ -0002dad0: 2d69 6e6c 2e68 3a33 3935 3c2f 6469 763e -inl.h:395
    │ │ │ │ -0002dae0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -0002db40: 3c61 2068 7265 663d 2261 3032 3738 302e gtsam::D │ │ │ │ -0002db80: 6563 6973 696f 6e54 7265 653a 3a43 686f ecisionTree::Cho │ │ │ │ -0002db90: 6963 653a 3a6c 6162 656c 5f3c 2f61 3e3c ice::label_< │ │ │ │ -0002dba0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    L label │ │ │ │ -0002dbc0: 5f3c 2f64 6976 3e3c 6469 7620 636c 6173 _
    the la │ │ │ │ -0002dbe0: 6265 6c20 6f66 2074 6865 2076 6172 6961 bel of the varia │ │ │ │ -0002dbf0: 626c 6520 6f6e 2077 6869 6368 2077 6520 ble on which we │ │ │ │ -0002dc00: 7370 6c69 743c 2f64 6976 3e3c 6469 7620 split
    Definition │ │ │ │ -0002dc30: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -0002dc40: 2e68 3a31 3734 3c2f 6469 763e 3c2f 6469 .h:174
    .
    gtsam::Decis │ │ │ │ -0002dcf0: 696f 6e54 7265 653a 3a43 686f 6963 653a ionTree::Choice: │ │ │ │ -0002dd00: 3a73 616d 654c 6561 663c 2f61 3e3c 2f64 :sameLeaf
    bool same │ │ │ │ -0002dd30: 4c65 6166 2863 6f6e 7374 204e 6f64 6520 Leaf(const Node │ │ │ │ -0002dd40: 2661 6d70 3b71 2920 636f 6e73 7420 6f76 &q) const ov │ │ │ │ -0002dd50: 6572 7269 6465 3c2f 6469 763e 3c64 6976 erride
    p │ │ │ │ -0002dd70: 6f6c 796d 6f72 7068 6963 2065 7175 616c olymorphic equal │ │ │ │ -0002dd80: 6974 793a 2069 6620 7120 6973 2061 206c ity: if q is a l │ │ │ │ -0002dd90: 6561 662c 2063 6f75 6c64 2062 652e 2e2e eaf, could be... │ │ │ │ -0002dda0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ -0002ddc0: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ -0002ddd0: 696f 6e54 7265 652d 696e 6c2e 683a 3331 ionTree-inl.h:31 │ │ │ │ -0002dde0: 383c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 8
    .
    │ │ │ │ -0002dec0: 4368 6f69 6365 2863 6f6e 7374 2043 686f Choice(const Cho │ │ │ │ -0002ded0: 6963 6520 2661 6d70 3b66 2c20 636f 6e73 ice &f, cons │ │ │ │ -0002dee0: 7420 4368 6f69 6365 2026 616d 703b 672c t Choice &g, │ │ │ │ -0002def0: 2063 6f6e 7374 2042 696e 6172 7920 2661 const Binary &a │ │ │ │ -0002df00: 6d70 3b6f 7029 3c2f 6469 763e 3c64 6976 mp;op)
    C │ │ │ │ -0002df20: 6f6e 7374 7275 6374 2066 726f 6d20 6170 onstruct from ap │ │ │ │ -0002df30: 706c 7969 6e67 2062 696e 6172 7920 6f70 plying binary op │ │ │ │ -0002df40: 2074 6f20 7477 6f20 4368 6f69 6365 206e to two Choice n │ │ │ │ -0002df50: 6f64 6573 2e3c 2f64 6976 3e3c 6469 7620 odes.
    Definition │ │ │ │ -0002df80: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -0002df90: 2e68 3a32 3330 3c2f 6469 763e 3c2f 6469 .h:230
    .
    gtsam::Decis │ │ │ │ -0002e040: 696f 6e54 7265 653a 3a43 686f 6963 653a ionTree::Choice: │ │ │ │ -0002e050: 3a70 7573 685f 6261 636b 3c2f 613e 3c2f :push_back
    void pus │ │ │ │ -0002e080: 685f 6261 636b 2863 6f6e 7374 204e 6f64 h_back(const Nod │ │ │ │ -0002e090: 6550 7472 2026 616d 703b 6e6f 6465 293c ePtr &node)< │ │ │ │ -0002e0a0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    add a br │ │ │ │ -0002e0c0: 616e 6368 3a20 544f 444f 206d 6572 6765 anch: TODO merge │ │ │ │ -0002e0d0: 2069 6e74 6f20 636f 6e73 7472 7563 746f into constructo │ │ │ │ -0002e0e0: 723c 2f64 6976 3e3c 6469 7620 636c 6173 r
    Def │ │ │ │ -0002e100: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ -0002e110: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a32 sionTree-inl.h:2 │ │ │ │ -0002e120: 3731 3c2f 6469 763e 3c2f 6469 763e 0a3c 71
    .< │ │ │ │ -0002e130: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -0002e140: 6964 3d22 6161 3032 3738 305f 6874 6d6c id="aa02780_html │ │ │ │ -0002e150: 5f61 6131 3132 3136 6433 3066 6131 6430 _aa11216d30fa1d0 │ │ │ │ -0002e160: 6162 6632 6634 6432 3666 3832 6331 6637 abf2f4d26f82c1f7 │ │ │ │ -0002e170: 3832 223e 3c64 6976 2063 6c61 7373 3d22 82"> │ │ │ │ -0002e1f0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    std::vector& │ │ │ │ -0002e210: 6c74 3b20 4e6f 6465 5074 7220 2667 743b lt; NodePtr > │ │ │ │ -0002e220: 2062 7261 6e63 6865 735f 3c2f 6469 763e branches_
    │ │ │ │ -0002e230: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    The children │ │ │ │ -0002e250: 6f66 2074 6869 7320 4368 6f69 6365 206e of this Choice n │ │ │ │ -0002e260: 6f64 652e 3c2f 6469 763e 3c64 6976 2063 ode.
    │ │ │ │ -0002e280: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -0002e290: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ -0002e2a0: 683a 3137 373c 2f64 6976 3e3c 2f64 6976 h:177
    .
    │ │ │ │ -0002e370: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    Choice()
    Default con │ │ │ │ -0002e3b0: 7374 7275 6374 6f72 2066 6f72 2073 6572 structor for ser │ │ │ │ -0002e3c0: 6961 6c69 7a61 7469 6f6e 2e3c 2f64 6976 ialization.
    Definitio │ │ │ │ -0002e3f0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -0002e400: 6565 2d69 6e6c 2e68 3a31 3930 3c2f 6469 ee-inl.h:190
    .
    c │ │ │ │ -0002e4f0: 6f6e 7374 2059 2026 616d 703b 206f 7065 onst Y & ope │ │ │ │ -0002e500: 7261 746f 7228 2928 636f 6e73 7420 4173 rator()(const As │ │ │ │ -0002e510: 7369 676e 6d65 6e74 266c 743b 204c 2026 signment< L & │ │ │ │ -0002e520: 6774 3b20 2661 6d70 3b78 2920 636f 6e73 gt; &x) cons │ │ │ │ -0002e530: 7420 6f76 6572 7269 6465 3c2f 6469 763e t override
    │ │ │ │ -0002e540: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    evaluate
    Definitio │ │ │ │ -0002e580: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -0002e590: 6565 2d69 6e6c 2e68 3a33 3336 3c2f 6469 ee-inl.h:336
    .
    Choic │ │ │ │ -0002e680: 6528 636f 6e73 7420 4c20 2661 6d70 3b6c e(const L &l │ │ │ │ -0002e690: 6162 656c 2c20 7369 7a65 5f74 2063 6f75 abel, size_t cou │ │ │ │ -0002e6a0: 6e74 293c 2f64 6976 3e3c 6469 7620 636c nt)
    Cons │ │ │ │ -0002e6c0: 7472 7563 746f 722c 2067 6976 656e 2063 tructor, given c │ │ │ │ -0002e6d0: 686f 6963 6520 6c61 6265 6c20 616e 6420 hoice label and │ │ │ │ -0002e6e0: 6d61 6e64 6174 6f72 7920 6578 7065 6374 mandatory expect │ │ │ │ -0002e6f0: 6564 2062 7261 6e63 6820 636f 756e 742e ed branch count. │ │ │ │ -0002e700: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ -0002e720: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ -0002e730: 696f 6e54 7265 652d 696e 6c2e 683a 3232 ionTree-inl.h:22 │ │ │ │ -0002e740: 343c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 4
    .
    │ │ │ │ -0002e820: 4e6f 6465 5074 7220 6368 6f6f 7365 2863 NodePtr choose(c │ │ │ │ -0002e830: 6f6e 7374 204c 2026 616d 703b 6c61 6265 onst L &labe │ │ │ │ -0002e840: 6c2c 2073 697a 655f 7420 696e 6465 7829 l, size_t index) │ │ │ │ -0002e850: 2063 6f6e 7374 206f 7665 7272 6964 653c const override< │ │ │ │ -0002e860: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    choose a │ │ │ │ -0002e880: 2062 7261 6e63 682c 2072 6563 7572 7369 branch, recursi │ │ │ │ -0002e890: 7665 6c79 3c2f 6469 763e 3c64 6976 2063 vely
    │ │ │ │ -0002e8b0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -0002e8c0: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ -0002e8d0: 683a 3433 343c 2f64 6976 3e3c 2f64 6976 h:434
    .
    void dot(std::o │ │ │ │ -0002e9c0: 7374 7265 616d 2026 616d 703b 6f73 2c20 stream &os, │ │ │ │ -0002e9d0: 636f 6e73 7420 4c61 6265 6c46 6f72 6d61 const LabelForma │ │ │ │ -0002e9e0: 7474 6572 2026 616d 703b 6c61 6265 6c46 tter &labelF │ │ │ │ -0002e9f0: 6f72 6d61 7474 6572 2c20 636f 6e73 7420 ormatter, const │ │ │ │ -0002ea00: 5661 6c75 6546 6f72 6d61 7474 6572 2026 ValueFormatter & │ │ │ │ -0002ea10: 616d 703b 7661 6c75 6546 6f72 6d61 7474 amp;valueFormatt │ │ │ │ -0002ea20: 6572 2c20 626f 6f6c 2073 686f 775a 6572 er, bool showZer │ │ │ │ -0002ea30: 6f29 2063 6f6e 7374 206f 7665 7272 6964 o) const overrid │ │ │ │ -0002ea40: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    output │ │ │ │ -0002ea60: 2074 6f20 6772 6170 6876 697a 2028 6173 to graphviz (as │ │ │ │ -0002ea70: 2061 2061 2067 7261 7068 293c 2f64 6976 a a graph)
    Definitio │ │ │ │ -0002eaa0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -0002eab0: 6565 2d69 6e6c 2e68 3a32 3930 3c2f 6469 ee-inl.h:290
    .< │ │ │ │ -0002ec50: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    < │ │ │ │ -0002ecb0: 6120 6872 6566 3d22 6130 3237 3830 2e68 a href="a02780.h │ │ │ │ -0002ecc0: 746d 6c23 6166 3936 6332 3331 3633 3662 tml#af96c231636b │ │ │ │ -0002ecd0: 3534 3862 6234 6466 3934 3165 3564 3137 548bb4df941e5d17 │ │ │ │ -0002ece0: 3265 3032 6622 3e67 7473 616d 3a3a 4465 2e02f">gtsam::De │ │ │ │ -0002ecf0: 6369 7369 6f6e 5472 6565 3a3a 4368 6f69 cisionTree::Choi │ │ │ │ -0002ed00: 6365 3a3a 556e 6971 7565 3c2f 613e 3c2f ce::Unique
    static N │ │ │ │ -0002ed30: 6f64 6550 7472 2055 6e69 7175 6528 636f odePtr Unique(co │ │ │ │ -0002ed40: 6e73 7420 4368 6f69 6365 5074 7220 2661 nst ChoicePtr &a │ │ │ │ -0002ed50: 6d70 3b66 293c 2f64 6976 3e3c 6469 7620 mp;f)
    If │ │ │ │ -0002ed70: 2061 6c6c 2062 7261 6e63 6865 7320 6f66 all branches of │ │ │ │ -0002ed80: 2061 2063 686f 6963 6520 6e6f 6465 2066 a choice node f │ │ │ │ -0002ed90: 2061 7265 2074 6865 2073 616d 652c 206a are the same, j │ │ │ │ -0002eda0: 7573 7420 7265 7475 726e 2061 2062 7261 ust return a bra │ │ │ │ -0002edb0: 6e63 682e 3c2f 6469 763e 3c64 6976 2063 nch.
    │ │ │ │ -0002edd0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -0002ede0: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ -0002edf0: 683a 3230 303c 2f64 6976 3e3c 2f64 6976 h:200
    .
    │ │ │ │ -0002eec0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    bool equals( │ │ │ │ -0002eee0: 636f 6e73 7420 4e6f 6465 2026 616d 703b const Node & │ │ │ │ -0002eef0: 712c 2063 6f6e 7374 2043 6f6d 7061 7265 q, const Compare │ │ │ │ -0002ef00: 4675 6e63 2026 616d 703b 636f 6d70 6172 Func &compar │ │ │ │ -0002ef10: 6529 2063 6f6e 7374 206f 7665 7272 6964 e) const overrid │ │ │ │ -0002ef20: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    equali │ │ │ │ -0002ef40: 7479 3c2f 6469 763e 3c64 6976 2063 6c61 ty
    De │ │ │ │ -0002ef60: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ -0002ef70: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ -0002ef80: 3332 333c 2f64 6976 3e3c 2f64 6976 3e0a 323
    . │ │ │ │ -0002ef90: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    gts │ │ │ │ -0002efe0: 616d 3a3a 5669 7369 743c 2f61 3e3c 2f64 am::Visit
    Functor pe │ │ │ │ -0002f010: 7266 6f72 6d69 6e67 2064 6570 7468 2d66 rforming depth-f │ │ │ │ -0002f020: 6972 7374 2076 6973 6974 2074 6f20 6561 irst visit to ea │ │ │ │ -0002f030: 6368 206c 6561 6620 7769 7468 2074 6865 ch leaf with the │ │ │ │ -0002f040: 206c 6561 6620 7661 6c75 6520 6173 2074 leaf value as t │ │ │ │ -0002f050: 6865 2061 7267 756d 656e 742e 3c2f 6469 he argument.
    Definiti │ │ │ │ -0002f080: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ -0002f090: 7265 652d 696e 6c2e 683a 3731 353c 2f64 ree-inl.h:715
    .
    < │ │ │ │ -0002f0f0: 6469 7620 636c 6173 733d 2274 746e 616d div class="ttnam │ │ │ │ -0002f100: 6522 3e3c 6120 6872 6566 3d22 6130 3237 e">gtsam │ │ │ │ -0002f140: 3a3a 5669 7369 743a 3a66 3c2f 613e 3c2f ::Visit::f
    F f
    folding func │ │ │ │ -0002f190: 7469 6f6e 206f 626a 6563 742e 3c2f 6469 tion object.
    Definiti │ │ │ │ -0002f1c0: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ -0002f1d0: 7265 652d 696e 6c2e 683a 3731 383c 2f64 ree-inl.h:718
    .
    < │ │ │ │ -0002f230: 6469 7620 636c 6173 733d 2274 746e 616d div class="ttnam │ │ │ │ -0002f240: 6522 3e3c 6120 6872 6566 3d22 6130 3237 e">gtsam │ │ │ │ -0002f280: 3a3a 5669 7369 743a 3a6f 7065 7261 746f ::Visit::operato │ │ │ │ -0002f290: 7228 293c 2f61 3e3c 2f64 6976 3e3c 6469 r()
    void operator() │ │ │ │ -0002f2c0: 2863 6f6e 7374 2074 7970 656e 616d 6520 (const typename │ │ │ │ -0002f2d0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ -0002f2e0: 204c 2c20 5920 2667 743b 3a3a 4e6f 6465 L, Y >::Node │ │ │ │ -0002f2f0: 5074 7220 2661 6d70 3b6e 6f64 6529 2063 Ptr &node) c │ │ │ │ -0002f300: 6f6e 7374 3c2f 6469 763e 3c64 6976 2063 onst
    Do │ │ │ │ -0002f320: 6120 6465 7074 682d 6669 7273 7420 7669 a depth-first vi │ │ │ │ -0002f330: 7369 7420 6f6e 2074 6865 2074 7265 6520 sit on the tree │ │ │ │ -0002f340: 726f 6f74 6564 2061 7420 6e6f 6465 2e3c rooted at node.< │ │ │ │ -0002f350: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ -0002f370: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ -0002f380: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3231 onTree-inl.h:721 │ │ │ │ -0002f390: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    V │ │ │ │ -0002f460: 6973 6974 2846 2066 293c 2f64 6976 3e3c isit(F f)
    < │ │ │ │ -0002f470: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ -0002f480: 223e 436f 6e73 7472 7563 7420 6672 6f6d ">Construct from │ │ │ │ -0002f490: 2066 6f6c 6469 6e67 2066 756e 6374 696f folding functio │ │ │ │ -0002f4a0: 6e2e 3c2f 6469 763e 3c64 6976 2063 6c61 n.
    De │ │ │ │ -0002f4c0: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ -0002f4d0: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ -0002f4e0: 3731 373c 2f64 6976 3e3c 2f64 6976 3e0a 717
    . │ │ │ │ -0002f4f0: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    Functo │ │ │ │ -0002f570: 7220 7065 7266 6f72 6d69 6e67 2064 6570 r performing dep │ │ │ │ -0002f580: 7468 2d66 6972 7374 2076 6973 6974 2074 th-first visit t │ │ │ │ -0002f590: 6f20 6561 6368 206c 6561 6620 7769 7468 o each leaf with │ │ │ │ -0002f5a0: 2074 6865 204c 6561 6620 6f62 6a65 6374 the Leaf object │ │ │ │ -0002f5b0: 2070 6173 7365 6420 6173 2061 6e20 6172 passed as an ar │ │ │ │ -0002f5c0: 6775 6d65 6e74 2e3c 2f64 6976 3e3c 6469 gument.
    │ │ │ │ -0002f5e0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ -0002f600: 6e6c 2e68 3a37 3532 3c2f 6469 763e 3c2f nl.h:752
    .
    V │ │ │ │ -0002f6e0: 6973 6974 4c65 6166 2846 2066 293c 2f64 isitLeaf(F f)
    Construct │ │ │ │ -0002f710: 6672 6f6d 2066 6f6c 6469 6e67 2066 756e from folding fun │ │ │ │ -0002f720: 6374 696f 6e2e 3c2f 6469 763e 3c64 6976 ction.
    < │ │ │ │ -0002f740: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -0002f750: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ -0002f760: 6c2e 683a 3735 343c 2f64 6976 3e3c 2f64 l.h:754
    .
    v │ │ │ │ -0002f840: 6f69 6420 6f70 6572 6174 6f72 2829 2863 oid operator()(c │ │ │ │ -0002f850: 6f6e 7374 2074 7970 656e 616d 6520 4465 onst typename De │ │ │ │ -0002f860: 6369 7369 6f6e 5472 6565 266c 743b 204c cisionTree< L │ │ │ │ -0002f870: 2c20 5920 2667 743b 3a3a 4e6f 6465 5074 , Y >::NodePt │ │ │ │ -0002f880: 7220 2661 6d70 3b6e 6f64 6529 2063 6f6e r &node) con │ │ │ │ -0002f890: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Do a │ │ │ │ -0002f8b0: 6465 7074 682d 6669 7273 7420 7669 7369 depth-first visi │ │ │ │ -0002f8c0: 7420 6f6e 2074 6865 2074 7265 6520 726f t on the tree ro │ │ │ │ -0002f8d0: 6f74 6564 2061 7420 6e6f 6465 2e3c 2f64 oted at node.
    Definit │ │ │ │ -0002f900: 696f 6e3c 2f62 3e20 4465 6369 7369 6f6e ion Decision │ │ │ │ -0002f910: 5472 6565 2d69 6e6c 2e68 3a37 3538 3c2f Tree-inl.h:758
    ..< │ │ │ │ -0002fa70: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -0002fa80: 6964 3d22 6161 3032 3739 325f 6874 6d6c id="aa02792_html │ │ │ │ -0002fa90: 223e 3c64 6976 2063 6c61 7373 3d22 7474 ">
    gtsa │ │ │ │ -0002fac0: 6d3a 3a56 6973 6974 5769 7468 3c2f 613e m::VisitWith │ │ │ │ -0002fad0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Functor │ │ │ │ -0002faf0: 2070 6572 666f 726d 696e 6720 6465 7074 performing dept │ │ │ │ -0002fb00: 682d 6669 7273 7420 7669 7369 7420 746f h-first visit to │ │ │ │ -0002fb10: 2065 6163 6820 6c65 6166 2077 6974 6820 each leaf with │ │ │ │ -0002fb20: 7468 6520 6c65 6166 2773 2041 7373 6967 the leaf's Assig │ │ │ │ -0002fb30: 6e6d 656e 7426 6c74 3b4c 2667 743b 2061 nment<L> a │ │ │ │ -0002fb40: 6e64 2076 616c 7565 2070 6173 7365 6420 nd value passed │ │ │ │ -0002fb50: 6173 2061 2e2e 2e3c 2f64 6976 3e3c 6469 as a...
    │ │ │ │ -0002fb70: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ -0002fb90: 6e6c 2e68 3a37 3836 3c2f 6469 763e 3c2f nl.h:786
    .
    V │ │ │ │ -0002fc70: 6973 6974 5769 7468 2846 2066 293c 2f64 isitWith(F f)
    Construct │ │ │ │ -0002fca0: 6672 6f6d 2066 6f6c 6469 6e67 2066 756e from folding fun │ │ │ │ -0002fcb0: 6374 696f 6e2e 3c2f 6469 763e 3c64 6976 ction.
    < │ │ │ │ -0002fcd0: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -0002fce0: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ -0002fcf0: 6c2e 683a 3738 383c 2f64 6976 3e3c 2f64 l.h:788
    .
    A │ │ │ │ -0002fdd0: 7373 6967 6e6d 656e 7426 6c74 3b20 4c20 ssignment< L │ │ │ │ -0002fde0: 2667 743b 2061 7373 6967 6e6d 656e 743c > assignment< │ │ │ │ -0002fdf0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Assignme │ │ │ │ -0002fe10: 6e74 2c20 6d75 7461 7469 6e67 2074 6872 nt, mutating thr │ │ │ │ -0002fe20: 6f75 6768 2072 6563 7572 7369 6f6e 2e3c ough recursion.< │ │ │ │ -0002fe30: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ -0002fe50: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ -0002fe60: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3839 onTree-inl.h:789 │ │ │ │ -0002fe70: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ -0002ff10: 7361 6d3a 3a56 6973 6974 5769 7468 3a3a sam::VisitWith:: │ │ │ │ -0002ff20: 6f70 6572 6174 6f72 2829 3c2f 613e 3c2f operator()
    void ope │ │ │ │ -0002ff50: 7261 746f 7228 2928 636f 6e73 7420 7479 rator()(const ty │ │ │ │ -0002ff60: 7065 6e61 6d65 2044 6563 6973 696f 6e54 pename DecisionT │ │ │ │ -0002ff70: 7265 6526 6c74 3b20 4c2c 2059 2026 6774 ree< L, Y > │ │ │ │ -0002ff80: 3b3a 3a4e 6f64 6550 7472 2026 616d 703b ;::NodePtr & │ │ │ │ -0002ff90: 6e6f 6465 293c 2f64 6976 3e3c 6469 7620 node)
    Do │ │ │ │ -0002ffb0: 2061 2064 6570 7468 2d66 6972 7374 2076 a depth-first v │ │ │ │ -0002ffc0: 6973 6974 206f 6e20 7468 6520 7472 6565 isit on the tree │ │ │ │ -0002ffd0: 2072 6f6f 7465 6420 6174 206e 6f64 652e rooted at node. │ │ │ │ -0002ffe0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ -00030000: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ -00030010: 696f 6e54 7265 652d 696e 6c2e 683a 3739 ionTree-inl.h:79 │ │ │ │ -00030020: 333c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 3
    .
    │ │ │ │ -000300f0: 4620 663c 2f64 6976 3e3c 6469 7620 636c F f
    fold │ │ │ │ -00030110: 696e 6720 6675 6e63 7469 6f6e 206f 626a ing function obj │ │ │ │ -00030120: 6563 742e 3c2f 6469 763e 3c64 6976 2063 ect.
    │ │ │ │ -00030140: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -00030150: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ -00030160: 683a 3739 303c 2f64 6976 3e3c 2f64 6976 h:790
    .
    a │ │ │ │ -000301f0: 2064 6563 6973 696f 6e20 7472 6565 2069 decision tree i │ │ │ │ -00030200: 7320 6120 6675 6e63 7469 6f6e 2066 726f s a function fro │ │ │ │ -00030210: 6d20 6173 7369 676e 6d65 6e74 7320 746f m assignments to │ │ │ │ -00030220: 2076 616c 7565 732e 3c2f 6469 763e 3c64 values.
    Definition DecisionTree. │ │ │ │ -00030260: 683a 3631 3c2f 6469 763e 3c2f 6469 763e h:61
    │ │ │ │ -00030270: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    gtsam::Decisio │ │ │ │ -00030310: 6e54 7265 653a 3a61 7070 6c79 3c2f 613e nTree::apply │ │ │ │ -00030320: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Decisi │ │ │ │ -00030340: 6f6e 5472 6565 2061 7070 6c79 2863 6f6e onTree apply(con │ │ │ │ -00030350: 7374 2055 6e61 7279 2026 616d 703b 6f70 st Unary &op │ │ │ │ -00030360: 2920 636f 6e73 743c 2f64 6976 3e3c 6469 ) const
    │ │ │ │ -00030380: 6170 706c 7920 556e 6172 7920 6f70 6572 apply Unary oper │ │ │ │ -00030390: 6174 696f 6e20 2671 756f 743b 6f70 2671 ation "op&q │ │ │ │ -000303a0: 756f 743b 2074 6f20 663c 2f64 6976 3e3c uot; to f
    < │ │ │ │ -000303b0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ -000303c0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ -000303d0: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ -000303e0: 2d69 6e6c 2e68 3a38 3839 3c2f 6469 763e -inl.h:889
    │ │ │ │ -000303f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00030450: 3c61 2068 7265 663d 2261 3032 3739 362e gtsam::D │ │ │ │ -00030490: 6563 6973 696f 6e54 7265 653a 3a63 6f6e ecisionTree::con │ │ │ │ -000304a0: 7665 7274 4672 6f6d 3c2f 613e 3c2f 6469 vertFrom
    NodePtr co │ │ │ │ -000304d0: 6e76 6572 7446 726f 6d28 636f 6e73 7420 nvertFrom(const │ │ │ │ -000304e0: 7479 7065 6e61 6d65 2044 6563 6973 696f typename Decisio │ │ │ │ -000304f0: 6e54 7265 6526 6c74 3b20 4d2c 2058 2026 nTree< M, X & │ │ │ │ -00030500: 6774 3b3a 3a4e 6f64 6550 7472 2026 616d gt;::NodePtr &am │ │ │ │ -00030510: 703b 662c 2073 7464 3a3a 6675 6e63 7469 p;f, std::functi │ │ │ │ -00030520: 6f6e 266c 743b 204c 2863 6f6e 7374 204d on< L(const M │ │ │ │ -00030530: 2026 616d 703b 2926 6774 3b20 4c5f 6f66 &)> L_of │ │ │ │ -00030540: 5f4d 2c20 7374 643a 3a66 756e 6374 696f _M, std::functio │ │ │ │ -00030550: 6e26 6c74 3b20 5928 636f 6e73 7420 5820 n< Y(const X │ │ │ │ -00030560: 2661 6d70 3b29 2667 743b 2059 5f6f 665f &)> Y_of_ │ │ │ │ -00030570: 5829 2063 6f6e 7374 3c2f 6469 763e 3c64 X) const
    Convert from a │ │ │ │ -000305a0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ -000305b0: 4d2c 2058 2667 743b 2074 6f20 4465 6369 M, X> to Deci │ │ │ │ -000305c0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ -000305d0: 2667 743b 2e3c 2f64 6976 3e3c 6469 7620 >.
    Definition │ │ │ │ -00030600: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -00030610: 2e68 3a36 3731 3c2f 6469 763e 3c2f 6469 .h:671
    ..
    │ │ │ │ -000308b0: 7479 7065 6e61 6d65 204e 6f64 653a 3a50 typename Node::P │ │ │ │ -000308c0: 7472 204e 6f64 6550 7472 3c2f 6469 763e tr NodePtr
    │ │ │ │ -000308d0: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    ------------- │ │ │ │ -000308f0: 2d2d 2d2d 2d2d 2d2d e280 9420 4e6f 6465 --------... Node │ │ │ │ -00030900: 2062 6173 6520 636c 6173 7320 2d2d 2d2d base class ---- │ │ │ │ -00030910: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00030920: 2d2d 2d2d e280 943c 2f64 6976 3e3c 6469 ----...
    │ │ │ │ -00030940: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree.h │ │ │ │ -00030960: 3a31 3433 3c2f 6469 763e 3c2f 6469 763e :143
    │ │ │ │ -00030970: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    std:: │ │ │ │ -00030a40: 7365 7426 6c74 3b20 4c20 2667 743b 206c set< L > l │ │ │ │ -00030a50: 6162 656c 7328 2920 636f 6e73 743c 2f64 abels() const
    Retrieve a │ │ │ │ -00030a80: 6c6c 2075 6e69 7175 6520 6c61 6265 6c73 ll unique labels │ │ │ │ -00030a90: 2061 7320 6120 7365 742e 3c2f 6469 763e as a set.
    │ │ │ │ -00030aa0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    Definition │ │ │ │ -00030ac0: 3c2f 623e 2044 6563 6973 696f 6e54 7265 DecisionTre │ │ │ │ -00030ad0: 652d 696e 6c2e 683a 3835 333c 2f64 6976 e-inl.h:853
    .bool empty() co │ │ │ │ -00030bc0: 6e73 743c 2f64 6976 3e3c 6469 7620 636c nst
    Chec │ │ │ │ -00030be0: 6b20 6966 2074 7265 6520 6973 2065 6d70 k if tree is emp │ │ │ │ -00030bf0: 7479 2e3c 2f64 6976 3e3c 6469 7620 636c ty.
    D │ │ │ │ -00030c10: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -00030c20: 6369 7369 6f6e 5472 6565 2e68 3a32 3537 cisionTree.h:257 │ │ │ │ -00030c30: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ -00030cd0: 7361 6d3a 3a44 6563 6973 696f 6e54 7265 sam::DecisionTre │ │ │ │ -00030ce0: 653a 3a76 6973 6974 3c2f 613e 3c2f 6469 e::visit
    void visit │ │ │ │ -00030d10: 2846 756e 6320 6629 2063 6f6e 7374 3c2f (Func f) const
    Visit all │ │ │ │ -00030d40: 206c 6561 7665 7320 696e 2064 6570 7468 leaves in depth │ │ │ │ -00030d50: 2d66 6972 7374 2066 6173 6869 6f6e 2e3c -first fashion.< │ │ │ │ -00030d60: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ -00030d80: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ -00030d90: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3336 onTree-inl.h:736 │ │ │ │ -00030da0: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    void v │ │ │ │ -00030e80: 6973 6974 4c65 6166 2846 756e 6320 6629 isitLeaf(Func f) │ │ │ │ -00030e90: 2063 6f6e 7374 3c2f 6469 763e 3c64 6976 const
    V │ │ │ │ -00030eb0: 6973 6974 2061 6c6c 206c 6561 7665 7320 isit all leaves │ │ │ │ -00030ec0: 696e 2064 6570 7468 2d66 6972 7374 2066 in depth-first f │ │ │ │ -00030ed0: 6173 6869 6f6e 2e3c 2f64 6976 3e3c 6469 ashion.
    │ │ │ │ -00030ef0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ -00030f10: 6e6c 2e68 3a37 3733 3c2f 6469 763e 3c2f nl.h:773
    .
    st │ │ │ │ -00030ff0: 643a 3a66 756e 6374 696f 6e26 6c74 3b20 d::function< │ │ │ │ -00031000: 5928 636f 6e73 7420 5920 2661 6d70 3b29 Y(const Y &) │ │ │ │ -00031010: 2667 743b 2055 6e61 7279 3c2f 6469 763e > Unary
    │ │ │ │ -00031020: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    Handy typedef │ │ │ │ -00031040: 7320 666f 7220 756e 6172 7920 616e 6420 s for unary and │ │ │ │ -00031050: 6269 6e61 7279 2066 756e 6374 696f 6e20 binary function │ │ │ │ -00031060: 7479 7065 732e 3c2f 6469 763e 3c64 6976 types.
    < │ │ │ │ -00031080: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -00031090: 2044 6563 6973 696f 6e54 7265 652e 683a DecisionTree.h: │ │ │ │ -000310a0: 3734 3c2f 6469 763e 3c2f 6469 763e 0a3c 74
    .< │ │ │ │ -000310b0: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -000310c0: 6964 3d22 6161 3032 3739 365f 6874 6d6c id="aa02796_html │ │ │ │ -000310d0: 5f61 3733 6361 3535 3833 6163 3264 6262 _a73ca5583ac2dbb │ │ │ │ -000310e0: 3236 6163 6366 6335 3464 6536 3961 3963 26accfc54de69a9c │ │ │ │ -000310f0: 3061 223e 3c64 6976 2063 6c61 7373 3d22 0a">
    │ │ │ │ -00031140: 6774 7361 6d3a 3a44 6563 6973 696f 6e54 gtsam::DecisionT │ │ │ │ -00031150: 7265 653a 3a66 6f6c 643c 2f61 3e3c 2f64 ree::fold
    X fold(Fu │ │ │ │ -00031180: 6e63 2066 2c20 5820 7830 2920 636f 6e73 nc f, X x0) cons │ │ │ │ -00031190: 743c 2f64 6976 3e3c 6469 7620 636c 6173 t
    Fold a │ │ │ │ -000311b0: 2062 696e 6172 7920 6675 6e63 7469 6f6e binary function │ │ │ │ -000311c0: 206f 7665 7220 7468 6520 7472 6565 2c20 over the tree, │ │ │ │ -000311d0: 7265 7475 726e 696e 6720 6163 6375 6d75 returning accumu │ │ │ │ -000311e0: 6c61 746f 722e 3c2f 6469 763e 3c64 6976 lator.
    < │ │ │ │ -00031200: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -00031210: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ -00031220: 6c2e 683a 3833 333c 2f64 6976 3e3c 2f64 l.h:833
    ..
    │ │ │ │ -00031500: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    GTSAM-style p │ │ │ │ -00031520: 7269 6e74 2e3c 2f64 6976 3e3c 6469 7620 rint.
    Definition │ │ │ │ -00031550: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -00031560: 2e68 3a38 3732 3c2f 6469 763e 3c2f 6469 .h:872
    .
    De │ │ │ │ -00031640: 6369 7369 6f6e 5472 6565 2063 6f6d 6269 cisionTree combi │ │ │ │ -00031650: 6e65 2863 6f6e 7374 204c 2026 616d 703b ne(const L & │ │ │ │ -00031660: 6c61 6265 6c2c 2073 697a 655f 7420 6361 label, size_t ca │ │ │ │ -00031670: 7264 696e 616c 6974 792c 2063 6f6e 7374 rdinality, const │ │ │ │ -00031680: 2042 696e 6172 7920 2661 6d70 3b6f 7029 Binary &op) │ │ │ │ -00031690: 2063 6f6e 7374 3c2f 6469 763e 3c64 6976 const
    c │ │ │ │ -000316b0: 6f6d 6269 6e65 2073 7562 7472 6565 7320 ombine subtrees │ │ │ │ -000316c0: 6f6e 206b 6579 2077 6974 6820 6269 6e61 on key with bina │ │ │ │ -000316d0: 7279 206f 7065 7261 7469 6f6e 2026 7175 ry operation &qu │ │ │ │ -000316e0: 6f74 3b6f 7026 7175 6f74 3b3c 2f64 6976 ot;op"
    Definitio │ │ │ │ -00031710: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ -00031720: 6565 2d69 6e6c 2e68 3a39 3337 3c2f 6469 ee-inl.h:937
    .
    gtsam: │ │ │ │ -000317d0: 3a44 6563 6973 696f 6e54 7265 653a 3a76 :DecisionTree::v │ │ │ │ -000317e0: 6973 6974 5769 7468 3c2f 613e 3c2f 6469 isitWith
    void visit │ │ │ │ -00031810: 5769 7468 2846 756e 6320 6629 2063 6f6e With(Func f) con │ │ │ │ -00031820: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Visit │ │ │ │ -00031840: 2061 6c6c 206c 6561 7665 7320 696e 2064 all leaves in d │ │ │ │ -00031850: 6570 7468 2d66 6972 7374 2066 6173 6869 epth-first fashi │ │ │ │ -00031860: 6f6e 2e3c 2f64 6976 3e3c 6469 7620 636c on.
    D │ │ │ │ -00031880: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -00031890: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ -000318a0: 3a38 3136 3c2f 6469 763e 3c2f 6469 763e :816
    │ │ │ │ -000318b0: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    c │ │ │ │ -00031980: 6f6e 7374 2059 2026 616d 703b 206f 7065 onst Y & ope │ │ │ │ -00031990: 7261 746f 7228 2928 636f 6e73 7420 4173 rator()(const As │ │ │ │ -000319a0: 7369 676e 6d65 6e74 266c 743b 204c 2026 signment< L & │ │ │ │ -000319b0: 6774 3b20 2661 6d70 3b78 2920 636f 6e73 gt; &x) cons │ │ │ │ -000319c0: 743c 2f64 6976 3e3c 6469 7620 636c 6173 t
    evalua │ │ │ │ -000319e0: 7465 3c2f 6469 763e 3c64 6976 2063 6c61 te
    De │ │ │ │ -00031a00: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ -00031a10: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ -00031a20: 3838 343c 2f64 6976 3e3c 2f64 6976 3e0a 884
    . │ │ │ │ -00031a30: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    gtsam::Decision │ │ │ │ -00031ad0: 5472 6565 3a3a 646f 743c 2f61 3e3c 2f64 Tree::dot
    void dot( │ │ │ │ -00031b00: 7374 643a 3a6f 7374 7265 616d 2026 616d std::ostream &am │ │ │ │ -00031b10: 703b 6f73 2c20 636f 6e73 7420 4c61 6265 p;os, const Labe │ │ │ │ -00031b20: 6c46 6f72 6d61 7474 6572 2026 616d 703b lFormatter & │ │ │ │ -00031b30: 6c61 6265 6c46 6f72 6d61 7474 6572 2c20 labelFormatter, │ │ │ │ -00031b40: 636f 6e73 7420 5661 6c75 6546 6f72 6d61 const ValueForma │ │ │ │ -00031b50: 7474 6572 2026 616d 703b 7661 6c75 6546 tter &valueF │ │ │ │ -00031b60: 6f72 6d61 7474 6572 2c20 626f 6f6c 2073 ormatter, bool s │ │ │ │ -00031b70: 686f 775a 6572 6f3d 7472 7565 2920 636f howZero=true) co │ │ │ │ -00031b80: 6e73 743c 2f64 6976 3e3c 6469 7620 636c nst
    outp │ │ │ │ -00031ba0: 7574 2074 6f20 6772 6170 6876 697a 2066 ut to graphviz f │ │ │ │ -00031bb0: 6f72 6d61 742c 2073 7472 6561 6d20 7665 ormat, stream ve │ │ │ │ -00031bc0: 7273 696f 6e3c 2f64 6976 3e3c 6469 7620 rsion
    Definition │ │ │ │ -00031bf0: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ -00031c00: 2e68 3a39 3439 3c2f 6469 763e 3c2f 6469 .h:949
    .
    . │ │ │ │ -00031d80: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    bo │ │ │ │ -00031e50: 6f6c 206f 7065 7261 746f 723d 3d28 636f ol operator==(co │ │ │ │ -00031e60: 6e73 7420 4465 6369 7369 6f6e 5472 6565 nst DecisionTree │ │ │ │ -00031e70: 2026 616d 703b 7129 2063 6f6e 7374 3c2f &q) const
    equality< │ │ │ │ -00031ea0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ -00031ec0: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ -00031ed0: 6f6e 5472 6565 2d69 6e6c 2e68 3a38 3739 onTree-inl.h:879 │ │ │ │ -00031ee0: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ -00031f80: 7361 6d3a 3a44 6563 6973 696f 6e54 7265 sam::DecisionTre │ │ │ │ -00031f90: 653a 3a4c 6162 656c 433c 2f61 3e3c 2f64 e::LabelC
    std::pair │ │ │ │ -00031fc0: 266c 743b 204c 2c20 7369 7a65 5f74 2026 < L, size_t & │ │ │ │ -00031fd0: 6774 3b20 4c61 6265 6c43 3c2f 6469 763e gt; LabelC
    │ │ │ │ -00031fe0: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    A label annot │ │ │ │ -00032000: 6174 6564 2077 6974 6820 6361 7264 696e ated with cardin │ │ │ │ -00032010: 616c 6974 792e 3c2f 6469 763e 3c64 6976 ality.
    < │ │ │ │ -00032030: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -00032040: 2044 6563 6973 696f 6e54 7265 652e 683a DecisionTree.h: │ │ │ │ -00032050: 3739 3c2f 6469 763e 3c2f 6469 763e 0a3c 79
    .< │ │ │ │ -00032060: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ -00032070: 6964 3d22 6161 3032 3739 365f 6874 6d6c id="aa02796_html │ │ │ │ -00032080: 5f61 6538 3065 3635 6139 3066 6539 6163 _ae80e65a90fe9ac │ │ │ │ -00032090: 3430 6235 6335 6134 6430 3764 6662 6264 40b5c5a4d07dfbbd │ │ │ │ -000320a0: 6137 223e 3c64 6976 2063 6c61 7373 3d22 a7">
    size_ │ │ │ │ -00032130: 7420 6e72 4c65 6176 6573 2829 2063 6f6e t nrLeaves() con │ │ │ │ -00032140: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Retur │ │ │ │ -00032160: 6e20 7468 6520 6e75 6d62 6572 206f 6620 n the number of │ │ │ │ -00032170: 6c65 6176 6573 2069 6e20 7468 6520 7472 leaves in the tr │ │ │ │ -00032180: 6565 2e3c 2f64 6976 3e3c 6469 7620 636c ee.
    D │ │ │ │ -000321a0: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ -000321b0: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ -000321c0: 3a38 3233 3c2f 6469 763e 3c2f 6469 763e :823
    │ │ │ │ -000321d0: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    DecisionTree()< │ │ │ │ -000322b0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Default │ │ │ │ -000322d0: 636f 6e73 7472 7563 746f 7220 2866 6f72 constructor (for │ │ │ │ -000322e0: 2073 6572 6961 6c69 7a61 7469 6f6e 293c serialization)< │ │ │ │ -000322f0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ -00032310: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ -00032320: 6f6e 5472 6565 2d69 6e6c 2e68 3a34 3632 onTree-inl.h:462 │ │ │ │ -00032330: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    . │ │ │ │ -00032360: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 │ │ │ │ -000323c0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -000323d0: 2d2d 2d2d 2de2 8094 204e 6f64 6520 6261 -----... Node ba │ │ │ │ -000323e0: 7365 2063 6c61 7373 202d 2d2d 2d2d 2d2d se class ------- │ │ │ │ -000323f0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00032400: 2de2 8094 3c2f 6469 763e 3c64 6976 2063 -...
    │ │ │ │ -00032420: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ -00032430: 6563 6973 696f 6e54 7265 652e 683a 3836 ecisionTree.h:86 │ │ │ │ -00032440: 3c2f 6469 763e 3c2f 6469 763e 0a3c 2f64
    .
    ..
    .
    . │ │ │ │ +0000c350: 0a3c 6469 7620 6964 3d22 6e61 762d 7061 ... │ │ │ │ +0000c5b0: 3c2f 6874 6d6c 3e0a . │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,1242 +1,352 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DecisionTree-inl.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 │ │ │ │ │ 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_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36#include │ │ │ │ │ -37 │ │ │ │ │ -38namespace _g_t_s_a_m { │ │ │ │ │ -39 │ │ │ │ │ -40 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -41 // Node │ │ │ │ │ -42 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -43#ifdef DT_DEBUG_MEMORY │ │ │ │ │ -44 template │ │ │ │ │ -45 int DecisionTree::Node::nrNodes = 0; │ │ │ │ │ -46#endif │ │ │ │ │ -47 │ │ │ │ │ -48 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -49 // Leaf │ │ │ │ │ -50 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -51 template │ │ │ │ │ -_5_2 struct _D_e_c_i_s_i_o_n_T_r_e_e::_L_e_a_f : public _D_e_c_i_s_i_o_n_T_r_e_e::Node { │ │ │ │ │ -_5_4 Y _c_o_n_s_t_a_n_t__; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -_5_9 size_t _n_r_A_s_s_i_g_n_m_e_n_t_s__; │ │ │ │ │ -60 │ │ │ │ │ -_6_2 _L_e_a_f() {} │ │ │ │ │ -63 │ │ │ │ │ -_6_5 _L_e_a_f(const Y& constant, size_t nrAssignments = 1) │ │ │ │ │ -66 : constant_(constant), nrAssignments_(nrAssignments) {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 const Y& _c_o_n_s_t_a_n_t() const { │ │ │ │ │ -70 return constant_; │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -_7_4 size_t _n_r_A_s_s_i_g_n_m_e_n_t_s() const { return nrAssignments_; } │ │ │ │ │ -75 │ │ │ │ │ -_7_7 bool _s_a_m_e_L_e_a_f(const _L_e_a_f& q) const override { │ │ │ │ │ -78 return constant_ == q._c_o_n_s_t_a_n_t__; │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -_8_2 bool _s_a_m_e_L_e_a_f(const _N_o_d_e& q) const override { │ │ │ │ │ -83 return (q.isLeaf() && q.sameLeaf(*this)); │ │ │ │ │ +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 bool _e_q_u_a_l_s(const _N_o_d_e& q, const CompareFunc& compare) const override { │ │ │ │ │ -88 const _L_e_a_f* other = dynamic_cast(&q); │ │ │ │ │ -89 if (!other) return false; │ │ │ │ │ -90 return compare(this->constant_, other->_c_o_n_s_t_a_n_t__); │ │ │ │ │ +_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 │ │ │ │ │ -_9_4 void _p_r_i_n_t(const std::string& s, const LabelFormatter& labelFormatter, │ │ │ │ │ -95 const ValueFormatter& valueFormatter) const override { │ │ │ │ │ -96 std::cout << s << " Leaf " << valueFormatter(constant_) << std::endl; │ │ │ │ │ -97 } │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 void _d_o_t(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ -101 const ValueFormatter& valueFormatter, │ │ │ │ │ -102 bool showZero) const override { │ │ │ │ │ -103 std::string value = valueFormatter(constant_); │ │ │ │ │ -104 if (showZero || value.compare("0")) │ │ │ │ │ -105 os << "\"" << this->id() << "\" [label=\"" << value │ │ │ │ │ -106 << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; │ │ │ │ │ -107 } │ │ │ │ │ +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_1_0 const Y& _o_p_e_r_a_t_o_r_(_)(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const override { │ │ │ │ │ -111 return constant_; │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 _N_o_d_e_P_t_r _a_p_p_l_y(const _U_n_a_r_y& op) const override { │ │ │ │ │ -116 _N_o_d_e_P_t_r f(new _L_e_a_f(op(constant_), nrAssignments_)); │ │ │ │ │ -117 return f; │ │ │ │ │ -118 } │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 _N_o_d_e_P_t_r _a_p_p_l_y(const UnaryAssignment& op, │ │ │ │ │ -122 const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment) const override { │ │ │ │ │ -123 _N_o_d_e_P_t_r f(new _L_e_a_f(op(assignment, constant_), nrAssignments_)); │ │ │ │ │ -124 return f; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127 // Apply binary operator "h = f op g" on Leaf node │ │ │ │ │ -128 // Note op is not assumed commutative so we need to keep track of order │ │ │ │ │ -129 // Simply calls apply on argument to call correct virtual method: │ │ │ │ │ -130 // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below) │ │ │ │ │ -131 // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice) │ │ │ │ │ -132 _N_o_d_e_P_t_r apply_f_op_g(const _N_o_d_e& g, const Binary& op) const override { │ │ │ │ │ -133 return g.apply_g_op_fL(*this, op); │ │ │ │ │ -134 } │ │ │ │ │ +_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 // Applying binary operator to two leaves results in a leaf │ │ │ │ │ -137 _N_o_d_e_P_t_r apply_g_op_fL(const Leaf& fL, const Binary& op) const override { │ │ │ │ │ -138 // fL op gL │ │ │ │ │ -139 _N_o_d_e_P_t_r h(new Leaf(op(fL.constant_, constant_), nrAssignments_)); │ │ │ │ │ -140 return h; │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -143 // If second argument is a Choice node, call it's apply with leaf as second │ │ │ │ │ -144 _N_o_d_e_P_t_r apply_g_op_fC(const Choice& fC, const Binary& op) const override { │ │ │ │ │ -145 return fC.apply_fC_op_gL(*this, op); // operand order back to normal │ │ │ │ │ -146 } │ │ │ │ │ -147 │ │ │ │ │ -_1_4_9 _N_o_d_e_P_t_r _c_h_o_o_s_e(const L& label, size_t index) const override { │ │ │ │ │ -150 return _N_o_d_e_P_t_r(new _L_e_a_f(constant(), nrAssignments())); │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -153 bool isLeaf() const override { return true; } │ │ │ │ │ -_1_5_4 │ │ │ │ │ -155 private: │ │ │ │ │ -156 using Base = _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e; │ │ │ │ │ -157 │ │ │ │ │ -159 friend class boost::serialization::access; │ │ │ │ │ -160 template │ │ │ │ │ -161 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -162 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -163 ar& BOOST_SERIALIZATION_NVP(constant_); │ │ │ │ │ -164 ar& BOOST_SERIALIZATION_NVP(nrAssignments_); │ │ │ │ │ -165 } │ │ │ │ │ -166 }; // Leaf │ │ │ │ │ -_1_6_7 │ │ │ │ │ -168 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -169 // Choice │ │ │ │ │ -170 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -171 template │ │ │ │ │ -_1_7_2 struct _D_e_c_i_s_i_o_n_T_r_e_e::_C_h_o_i_c_e: public _D_e_c_i_s_i_o_n_T_r_e_e::Node { │ │ │ │ │ -_1_7_4 L _l_a_b_e_l__; │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 std::vector _b_r_a_n_c_h_e_s__; │ │ │ │ │ -178 │ │ │ │ │ -_1_7_9 private: │ │ │ │ │ -184 size_t allSame_; │ │ │ │ │ -185 │ │ │ │ │ -186 using ChoicePtr = boost::shared_ptr; │ │ │ │ │ -187 │ │ │ │ │ -_1_8_8 public: │ │ │ │ │ -_1_9_0 _C_h_o_i_c_e() {} │ │ │ │ │ -_1_9_1 │ │ │ │ │ -192 ~Choice() override { │ │ │ │ │ -193#ifdef DT_DEBUG_MEMORY │ │ │ │ │ -_1_9_4 std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() │ │ │ │ │ -195 << std::std::endl; │ │ │ │ │ -196#endif │ │ │ │ │ -_1_9_7 } │ │ │ │ │ -198 │ │ │ │ │ -_2_0_0 static _N_o_d_e_P_t_r _U_n_i_q_u_e(const ChoicePtr& f) { │ │ │ │ │ -_2_0_1#ifndef GTSAM_DT_NO_PRUNING │ │ │ │ │ -202 if (f->allSame_) { │ │ │ │ │ -203 assert(f->branches().size() > 0); │ │ │ │ │ -_2_0_4 _N_o_d_e_P_t_r f0 = f->branches_[0]; │ │ │ │ │ -205 │ │ │ │ │ -206 size_t nrAssignments = 0; │ │ │ │ │ -207 for(auto branch: f->branches()) { │ │ │ │ │ -208 assert(branch->isLeaf()); │ │ │ │ │ -209 nrAssignments += │ │ │ │ │ -210 boost::dynamic_pointer_cast(branch)->nrAssignments(); │ │ │ │ │ -211 } │ │ │ │ │ -212 _N_o_d_e_P_t_r newLeaf( │ │ │ │ │ -213 new Leaf(boost::dynamic_pointer_cast(f0)->constant(), │ │ │ │ │ -214 nrAssignments)); │ │ │ │ │ -_2_1_5 return newLeaf; │ │ │ │ │ -216 } else │ │ │ │ │ -217#endif │ │ │ │ │ -218 return f; │ │ │ │ │ -219 } │ │ │ │ │ -220 │ │ │ │ │ -221 bool isLeaf() const override { return false; } │ │ │ │ │ -222 │ │ │ │ │ -_2_2_4 _C_h_o_i_c_e(const L& label, size_t count) : │ │ │ │ │ -225 label_(label), allSame_(true) { │ │ │ │ │ -226 branches_.reserve(count); │ │ │ │ │ -227 } │ │ │ │ │ -_2_2_8 │ │ │ │ │ -_2_3_0 _C_h_o_i_c_e(const _C_h_o_i_c_e& f, const _C_h_o_i_c_e& g, const Binary& op) : │ │ │ │ │ -231 allSame_(true) { │ │ │ │ │ -232 // Choose what to do based on label │ │ │ │ │ -233 if (f._l_a_b_e_l() > g._l_a_b_e_l()) { │ │ │ │ │ -234 // f higher than g │ │ │ │ │ -235 label_ = f._l_a_b_e_l(); │ │ │ │ │ -236 size_t count = f.nrChoices(); │ │ │ │ │ -237 branches_.reserve(count); │ │ │ │ │ -238 for (size_t i = 0; i < count; i++) │ │ │ │ │ -239 push_back(f._b_r_a_n_c_h_e_s__[i]->apply_f_op_g(g, op)); │ │ │ │ │ -240 } else if (g._l_a_b_e_l() > f._l_a_b_e_l()) { │ │ │ │ │ -241 // f lower than g │ │ │ │ │ -_2_4_2 label_ = g._l_a_b_e_l(); │ │ │ │ │ -243 size_t count = g.nrChoices(); │ │ │ │ │ -244 branches_.reserve(count); │ │ │ │ │ -245 for (size_t i = 0; i < count; i++) │ │ │ │ │ -246 push_back(g._b_r_a_n_c_h_e_s__[i]->apply_g_op_fC(f, op)); │ │ │ │ │ -247 } else { │ │ │ │ │ -248 // f same level as g │ │ │ │ │ -249 label_ = f._l_a_b_e_l(); │ │ │ │ │ -250 size_t count = f.nrChoices(); │ │ │ │ │ -251 branches_.reserve(count); │ │ │ │ │ -252 for (size_t i = 0; i < count; i++) │ │ │ │ │ -253 push_back(f._b_r_a_n_c_h_e_s__[i]->apply_f_op_g(*g._b_r_a_n_c_h_e_s__[i], op)); │ │ │ │ │ -254 } │ │ │ │ │ -255 } │ │ │ │ │ -256 │ │ │ │ │ -_2_5_8 const L& _l_a_b_e_l() const { │ │ │ │ │ -259 return label_; │ │ │ │ │ -_2_6_0 } │ │ │ │ │ -261 │ │ │ │ │ -262 size_t nrChoices() const { │ │ │ │ │ -_2_6_3 return branches_.size(); │ │ │ │ │ -264 } │ │ │ │ │ -265 │ │ │ │ │ -266 const std::vector& branches() const { │ │ │ │ │ -267 return branches_; │ │ │ │ │ -268 } │ │ │ │ │ -269 │ │ │ │ │ -_2_7_1 void _p_u_s_h___b_a_c_k(const _N_o_d_e_P_t_r& node) { │ │ │ │ │ -272 // allSame_ is restricted to leaf nodes in a decision tree │ │ │ │ │ -273 if (allSame_ && !branches_.empty()) { │ │ │ │ │ -274 allSame_ = node->sameLeaf(*branches_.back()); │ │ │ │ │ -275 } │ │ │ │ │ -276 branches_.push_back(node); │ │ │ │ │ -277 } │ │ │ │ │ -278 │ │ │ │ │ -_2_8_0 void _p_r_i_n_t(const std::string& s, const LabelFormatter& labelFormatter, │ │ │ │ │ -281 const ValueFormatter& valueFormatter) const override { │ │ │ │ │ -282 std::cout << s << " Choice("; │ │ │ │ │ -283 std::cout << labelFormatter(label_) << ") " << std::endl; │ │ │ │ │ -284 for (size_t i = 0; i < branches_.size(); i++) │ │ │ │ │ -285 branches_[i]->_p_r_i_n_t((boost::format("%s %d") % s % i).str(), │ │ │ │ │ -286 labelFormatter, valueFormatter); │ │ │ │ │ -287 } │ │ │ │ │ -288 │ │ │ │ │ -_2_9_0 void _d_o_t(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ -291 const ValueFormatter& valueFormatter, │ │ │ │ │ -292 bool showZero) const override { │ │ │ │ │ -293 os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_ │ │ │ │ │ -294 << "\"]\n"; │ │ │ │ │ -295 size_t B = branches_.size(); │ │ │ │ │ -296 for (size_t i = 0; i < B; i++) { │ │ │ │ │ -_2_9_7 const _N_o_d_e_P_t_r& branch = branches_[i]; │ │ │ │ │ -298 │ │ │ │ │ -299 // Check if zero │ │ │ │ │ -300 if (!showZero) { │ │ │ │ │ -301 const Leaf* leaf = dynamic_cast(branch.get()); │ │ │ │ │ -302 if (leaf && valueFormatter(leaf->constant()).compare("0")) continue; │ │ │ │ │ -303 } │ │ │ │ │ -304 │ │ │ │ │ -305 os << "\"" << this->id() << "\" -> \"" << branch->id() << "\""; │ │ │ │ │ -306 if (B == 2 && i == 0) os << " [style=dashed]"; │ │ │ │ │ -307 os << std::endl; │ │ │ │ │ -308 branch->dot(os, labelFormatter, valueFormatter, showZero); │ │ │ │ │ -309 } │ │ │ │ │ -310 } │ │ │ │ │ -311 │ │ │ │ │ -_3_1_3 bool _s_a_m_e_L_e_a_f(const _L_e_a_f& q) const override { │ │ │ │ │ -_3_1_4 return false; │ │ │ │ │ -315 } │ │ │ │ │ -316 │ │ │ │ │ -_3_1_8 bool _s_a_m_e_L_e_a_f(const _N_o_d_e& q) const override { │ │ │ │ │ -319 return (q.isLeaf() && q.sameLeaf(*this)); │ │ │ │ │ -320 } │ │ │ │ │ -321 │ │ │ │ │ -_3_2_3 bool _e_q_u_a_l_s(const _N_o_d_e& q, const CompareFunc& compare) const override { │ │ │ │ │ -324 const _C_h_o_i_c_e* other = dynamic_cast(&q); │ │ │ │ │ -325 if (!other) return false; │ │ │ │ │ -326 if (this->label_ != other->_l_a_b_e_l__) return false; │ │ │ │ │ -327 if (branches_.size() != other->_b_r_a_n_c_h_e_s__.size()) return false; │ │ │ │ │ -328 // we don't care about shared pointers being equal here │ │ │ │ │ -329 for (size_t i = 0; i < branches_.size(); i++) │ │ │ │ │ -330 if (!(branches_[i]->_e_q_u_a_l_s(*(other->_b_r_a_n_c_h_e_s__[i]), compare))) │ │ │ │ │ -331 return false; │ │ │ │ │ -332 return true; │ │ │ │ │ -333 } │ │ │ │ │ -334 │ │ │ │ │ -_3_3_6 const Y& _o_p_e_r_a_t_o_r_(_)(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const override { │ │ │ │ │ -337#ifndef NDEBUG │ │ │ │ │ -_3_3_8 typename _A_s_s_i_g_n_m_e_n_t_<_L_>_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r it = x.find(label_); │ │ │ │ │ -339 if (it == x.end()) { │ │ │ │ │ -340 std::cout << "Trying to find value for " << label_ << std::endl; │ │ │ │ │ -_3_4_1 throw std::invalid_argument( │ │ │ │ │ -342 "DecisionTree::operator(): value undefined for a label"); │ │ │ │ │ -343 } │ │ │ │ │ -344#endif │ │ │ │ │ -345 size_t index = x.at(label_); │ │ │ │ │ -346 _N_o_d_e_P_t_r child = branches_[index]; │ │ │ │ │ -347 return (*child)(x); │ │ │ │ │ -348 } │ │ │ │ │ -349 │ │ │ │ │ -_3_5_1 Choice(const L& label, const Choice& f, const _U_n_a_r_y& op) : │ │ │ │ │ -352 label_(label), allSame_(true) { │ │ │ │ │ -353 branches_.reserve(f.branches_.size()); // reserve space │ │ │ │ │ -_3_5_4 for (const _N_o_d_e_P_t_r& branch : f.branches_) { │ │ │ │ │ -355 push_back(branch->apply(op)); │ │ │ │ │ -356 } │ │ │ │ │ -357 } │ │ │ │ │ -358 │ │ │ │ │ -_3_6_9 _C_h_o_i_c_e(const L& label, const _C_h_o_i_c_e& f, const UnaryAssignment& op, │ │ │ │ │ -370 const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment) │ │ │ │ │ -371 : label_(label), allSame_(true) { │ │ │ │ │ -372 branches_.reserve(f._b_r_a_n_c_h_e_s__.size()); // reserve space │ │ │ │ │ -_3_7_3 │ │ │ │ │ -374 _A_s_s_i_g_n_m_e_n_t_<_L_> assignment_ = assignment; │ │ │ │ │ -375 │ │ │ │ │ -376 for (size_t i = 0; i < f._b_r_a_n_c_h_e_s__.size(); i++) { │ │ │ │ │ -_3_7_7 assignment_[label_] = i; // Set assignment for label to i │ │ │ │ │ -378 │ │ │ │ │ -379 const _N_o_d_e_P_t_r branch = f._b_r_a_n_c_h_e_s__[i]; │ │ │ │ │ -380 push_back(branch->apply(op, assignment_)); │ │ │ │ │ -_3_8_1 │ │ │ │ │ -382 // Remove the assignment so we are backtracking │ │ │ │ │ -383 auto assignment_it = assignment_.find(label_); │ │ │ │ │ -384 assignment_.erase(assignment_it); │ │ │ │ │ -385 } │ │ │ │ │ -386 } │ │ │ │ │ -387 │ │ │ │ │ -_3_8_9 _N_o_d_e_P_t_r _a_p_p_l_y(const _U_n_a_r_y& op) const override { │ │ │ │ │ -390 auto r = boost::make_shared(label_, *this, op); │ │ │ │ │ -391 return Unique(r); │ │ │ │ │ -392 } │ │ │ │ │ -393 │ │ │ │ │ -_3_9_5 _N_o_d_e_P_t_r _a_p_p_l_y(const UnaryAssignment& op, │ │ │ │ │ -396 const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment) const override { │ │ │ │ │ -397 auto r = boost::make_shared(label_, *this, op, assignment); │ │ │ │ │ -398 return Unique(r); │ │ │ │ │ -399 } │ │ │ │ │ -400 │ │ │ │ │ -401 // Apply binary operator "h = f op g" on Choice node │ │ │ │ │ -402 // Note op is not assumed commutative so we need to keep track of order │ │ │ │ │ -403 // Simply calls apply on argument to call correct virtual method: │ │ │ │ │ -404 // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf) │ │ │ │ │ -405 // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below) │ │ │ │ │ -406 _N_o_d_e_P_t_r apply_f_op_g(const _N_o_d_e& g, const Binary& op) const override { │ │ │ │ │ -407 return g.apply_g_op_fC(*this, op); │ │ │ │ │ -408 } │ │ │ │ │ -409 │ │ │ │ │ -410 // If second argument of binary op is Leaf node, recurse on branches │ │ │ │ │ -411 _N_o_d_e_P_t_r apply_g_op_fL(const Leaf& fL, const Binary& op) const override { │ │ │ │ │ -412 auto h = boost::make_shared(label(), nrChoices()); │ │ │ │ │ -413 for (auto&& branch : branches_) │ │ │ │ │ -414 h->push_back(fL.apply_f_op_g(*branch, op)); │ │ │ │ │ -415 return Unique(h); │ │ │ │ │ -416 } │ │ │ │ │ -417 │ │ │ │ │ -418 // If second argument of binary op is Choice, call constructor │ │ │ │ │ -419 _N_o_d_e_P_t_r apply_g_op_fC(const Choice& fC, const Binary& op) const override { │ │ │ │ │ -420 auto h = boost::make_shared(fC, *this, op); │ │ │ │ │ -421 return Unique(h); │ │ │ │ │ -422 } │ │ │ │ │ -423 │ │ │ │ │ -424 // If second argument of binary op is Leaf │ │ │ │ │ -425 template │ │ │ │ │ -426 _N_o_d_e_P_t_r apply_fC_op_gL(const Leaf& gL, OP op) const { │ │ │ │ │ -427 auto h = boost::make_shared(label(), nrChoices()); │ │ │ │ │ -428 for (auto&& branch : branches_) │ │ │ │ │ -429 h->push_back(branch->apply_f_op_g(gL, op)); │ │ │ │ │ -430 return Unique(h); │ │ │ │ │ -431 } │ │ │ │ │ -432 │ │ │ │ │ -_4_3_4 _N_o_d_e_P_t_r _c_h_o_o_s_e(const L& label, size_t index) const override { │ │ │ │ │ -435 if (label_ == label) return branches_[index]; // choose branch │ │ │ │ │ -436 │ │ │ │ │ -437 // second case, not label of interest, just recurse │ │ │ │ │ -438 auto r = boost::make_shared(label_, branches_.size()); │ │ │ │ │ -439 for (auto&& branch : branches_) │ │ │ │ │ -440 r->push_back(branch->choose(label, index)); │ │ │ │ │ -441 return Unique(r); │ │ │ │ │ -442 } │ │ │ │ │ -443 │ │ │ │ │ -444 private: │ │ │ │ │ -445 using Base = _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e; │ │ │ │ │ -446 │ │ │ │ │ -448 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 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -452 ar& BOOST_SERIALIZATION_NVP(label_); │ │ │ │ │ -453 ar& BOOST_SERIALIZATION_NVP(branches_); │ │ │ │ │ -454 ar& BOOST_SERIALIZATION_NVP(allSame_); │ │ │ │ │ -455 } │ │ │ │ │ -456 }; // Choice │ │ │ │ │ -457 │ │ │ │ │ -458 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -459 // DecisionTree │ │ │ │ │ -460 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -461 template │ │ │ │ │ -_4_6_2 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e() { │ │ │ │ │ -463 } │ │ │ │ │ -464 │ │ │ │ │ -465 template │ │ │ │ │ -466 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const NodePtr& root) : │ │ │ │ │ -467 root_(root) { │ │ │ │ │ -468 } │ │ │ │ │ -469 │ │ │ │ │ -470 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -471 template │ │ │ │ │ -_4_7_2 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const Y& y) { │ │ │ │ │ -473 root_ = _N_o_d_e_P_t_r(new _L_e_a_f(y)); │ │ │ │ │ -474 } │ │ │ │ │ -475 │ │ │ │ │ -476 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -477 template │ │ │ │ │ -_4_7_8 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const L& label, const Y& y1, const Y& y2) │ │ │ │ │ -{ │ │ │ │ │ -479 auto a = boost::make_shared(label, 2); │ │ │ │ │ -480 _N_o_d_e_P_t_r l1(new _L_e_a_f(y1)), l2(new _L_e_a_f(y2)); │ │ │ │ │ -481 a->push_back(l1); │ │ │ │ │ -482 a->push_back(l2); │ │ │ │ │ -483 root_ = Choice::Unique(a); │ │ │ │ │ -484 } │ │ │ │ │ -485 │ │ │ │ │ -486 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -487 template │ │ │ │ │ -_4_8_8 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const _L_a_b_e_l_C& labelC, const Y& y1, │ │ │ │ │ -489 const Y& y2) { │ │ │ │ │ -490 if (labelC.second != 2) throw std::invalid_argument( │ │ │ │ │ -491 "DecisionTree: binary constructor called with non-binary label"); │ │ │ │ │ -492 auto a = boost::make_shared(labelC.first, 2); │ │ │ │ │ -493 _N_o_d_e_P_t_r l1(new _L_e_a_f(y1)), l2(new _L_e_a_f(y2)); │ │ │ │ │ -494 a->push_back(l1); │ │ │ │ │ -495 a->push_back(l2); │ │ │ │ │ -496 root_ = Choice::Unique(a); │ │ │ │ │ -497 } │ │ │ │ │ -498 │ │ │ │ │ -499 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -500 template │ │ │ │ │ -_5_0_1 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, │ │ │ │ │ -502 const std::vector& ys) { │ │ │ │ │ -503 // call recursive Create │ │ │ │ │ -504 root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); │ │ │ │ │ -505 } │ │ │ │ │ -506 │ │ │ │ │ -507 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -508 template │ │ │ │ │ -_5_0_9 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, │ │ │ │ │ -510 const std::string& table) { │ │ │ │ │ -511 // Convert std::string to values of type Y │ │ │ │ │ -512 std::vector ys; │ │ │ │ │ -513 std::istringstream iss(table); │ │ │ │ │ -514 copy(std::istream_iterator(iss), std::istream_iterator(), │ │ │ │ │ -515 back_inserter(ys)); │ │ │ │ │ -516 │ │ │ │ │ -517 // now call recursive Create │ │ │ │ │ -518 root_ = create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); │ │ │ │ │ -519 } │ │ │ │ │ -520 │ │ │ │ │ -521 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -522 template │ │ │ │ │ -_5_2_3 template _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e( │ │ │ │ │ -524 Iterator begin, Iterator end, const L& label) { │ │ │ │ │ -525 root_ = compose(begin, end, label); │ │ │ │ │ -526 } │ │ │ │ │ -527 │ │ │ │ │ -528 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -529 template │ │ │ │ │ -_5_3_0 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_D_e_c_i_s_i_o_n_T_r_e_e(const L& label, │ │ │ │ │ -531 const _D_e_c_i_s_i_o_n_T_r_e_e& f0, const _D_e_c_i_s_i_o_n_T_r_e_e& f1) { │ │ │ │ │ -532 const std::vector functions{f0, f1}; │ │ │ │ │ -533 root_ = compose(functions.begin(), functions.end(), label); │ │ │ │ │ -534 } │ │ │ │ │ -535 │ │ │ │ │ -536 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -537 template │ │ │ │ │ -538 template │ │ │ │ │ -_5_3_9 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_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, │ │ │ │ │ -540 Func Y_of_X) { │ │ │ │ │ -541 // Define functor for identity mapping of node label. │ │ │ │ │ -542 auto L_of_L = [](const L& label) { return label; }; │ │ │ │ │ -543 root_ = convertFrom(other._r_o_o_t__, L_of_L, Y_of_X); │ │ │ │ │ -544 } │ │ │ │ │ -545 │ │ │ │ │ -546 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -547 template │ │ │ │ │ -548 template │ │ │ │ │ -_5_4_9 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_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, │ │ │ │ │ -550 const std::map& map, Func Y_of_X) { │ │ │ │ │ -551 auto L_of_M = [&map](const M& label) -> L { return map.at(label); }; │ │ │ │ │ -552 root_ = convertFrom(other._r_o_o_t__, L_of_M, Y_of_X); │ │ │ │ │ -553 } │ │ │ │ │ -554 │ │ │ │ │ -555 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -556 // Called by two constructors above. │ │ │ │ │ -557 // Takes a label and a corresponding range of decision trees, and creates a │ │ │ │ │ -558 // new decision tree. However, the order of the labels needs to be │ │ │ │ │ -respected, │ │ │ │ │ -559 // so we cannot just create a root Choice node on the label: if the label │ │ │ │ │ -is │ │ │ │ │ -560 // not the highest label, we need a complicated/ expensive recursive call. │ │ │ │ │ -561 template │ │ │ │ │ -562 template │ │ │ │ │ -563 typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_c_o_m_p_o_s_e( │ │ │ │ │ -564 Iterator begin, Iterator end, const L& label) const { │ │ │ │ │ -565 // find highest label among branches │ │ │ │ │ -566 boost::optional highestLabel; │ │ │ │ │ -567 size_t nrChoices = 0; │ │ │ │ │ -568 for (Iterator it = begin; it != end; it++) { │ │ │ │ │ -569 if (it->root_->isLeaf()) │ │ │ │ │ -570 continue; │ │ │ │ │ -571 boost::shared_ptr c = │ │ │ │ │ -572 boost::dynamic_pointer_cast(it->root_); │ │ │ │ │ -573 if (!highestLabel || c->label() > *highestLabel) { │ │ │ │ │ -574 highestLabel.reset(c->label()); │ │ │ │ │ -575 nrChoices = c->nrChoices(); │ │ │ │ │ -576 } │ │ │ │ │ -577 } │ │ │ │ │ -578 │ │ │ │ │ -579 // if label is already in correct order, just put together a choice on │ │ │ │ │ -label │ │ │ │ │ -580 if (!nrChoices || !highestLabel || label > *highestLabel) { │ │ │ │ │ -581 auto choiceOnLabel = boost::make_shared(label, end - begin); │ │ │ │ │ -582 for (Iterator it = begin; it != end; it++) │ │ │ │ │ -583 choiceOnLabel->push_back(it->root_); │ │ │ │ │ -584 return Choice::Unique(choiceOnLabel); │ │ │ │ │ -585 } else { │ │ │ │ │ -586 // Set up a new choice on the highest label │ │ │ │ │ -587 auto choiceOnHighestLabel = │ │ │ │ │ -588 boost::make_shared(*highestLabel, nrChoices); │ │ │ │ │ -589 // now, for all possible values of highestLabel │ │ │ │ │ -590 for (size_t index = 0; index < nrChoices; index++) { │ │ │ │ │ -591 // make a new set of functions for composing by iterating over the given │ │ │ │ │ -592 // functions, and selecting the appropriate branch. │ │ │ │ │ -593 std::vector functions; │ │ │ │ │ -594 for (Iterator it = begin; it != end; it++) { │ │ │ │ │ -595 // by restricting the input functions to value i for labelBelow │ │ │ │ │ -596 DecisionTree chosen = it->choose(*highestLabel, index); │ │ │ │ │ -597 functions.push_back(chosen); │ │ │ │ │ -598 } │ │ │ │ │ -599 // We then recurse, for all values of the highest label │ │ │ │ │ -600 NodePtr fi = compose(functions.begin(), functions.end(), label); │ │ │ │ │ -601 choiceOnHighestLabel->push_back(fi); │ │ │ │ │ -602 } │ │ │ │ │ -603 return Choice::Unique(choiceOnHighestLabel); │ │ │ │ │ -604 } │ │ │ │ │ -605 } │ │ │ │ │ -606 │ │ │ │ │ -607 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -608 // "create" is a bit of a complicated thing, but very useful. │ │ │ │ │ -609 // It takes a range of labels and a corresponding range of values, │ │ │ │ │ -610 // and creates a decision tree, as follows: │ │ │ │ │ -611 // - if there is only one label, creates a choice node with values in │ │ │ │ │ -leaves │ │ │ │ │ -612 // - otherwise, it evenly splits up the range of values and creates a tree │ │ │ │ │ -for │ │ │ │ │ -613 // each sub-range, and assigns that tree to first label's choices │ │ │ │ │ -614 // Example: │ │ │ │ │ -615 // create([B A],[1 2 3 4]) would call │ │ │ │ │ -616 // create([A],[1 2]) │ │ │ │ │ -617 // create([A],[3 4]) │ │ │ │ │ -618 // and produce │ │ │ │ │ -619 // B=0 │ │ │ │ │ -620 // A=0: 1 │ │ │ │ │ -621 // A=1: 2 │ │ │ │ │ -622 // B=1 │ │ │ │ │ -623 // A=0: 3 │ │ │ │ │ -624 // A=1: 4 │ │ │ │ │ -625 // Note, through the magic of "compose", create([A B],[1 3 2 4]) will │ │ │ │ │ -produce │ │ │ │ │ -626 // exactly the same tree as above: the highest label is always the root. │ │ │ │ │ -627 // However, it will be *way* faster if labels are given highest to lowest. │ │ │ │ │ -628 template │ │ │ │ │ -629 template │ │ │ │ │ -_6_3_0 typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_c_r_e_a_t_e( │ │ │ │ │ -631 It begin, It end, ValueIt beginY, ValueIt endY) const { │ │ │ │ │ -632 // get crucial counts │ │ │ │ │ -633 size_t nrChoices = begin->second; │ │ │ │ │ -634 size_t size = endY - beginY; │ │ │ │ │ -635 │ │ │ │ │ -636 // Find the next key to work on │ │ │ │ │ -637 It labelC = begin + 1; │ │ │ │ │ -638 if (labelC == end) { │ │ │ │ │ -639 // Base case: only one key left │ │ │ │ │ -640 // Create a simple choice node with values as leaves. │ │ │ │ │ -641 if (size != nrChoices) { │ │ │ │ │ -642 std::cout << "Trying to create DD on " << begin->first << std::endl; │ │ │ │ │ -643 std::cout << boost::format( │ │ │ │ │ -644 "DecisionTree::create: expected %d values but got %d " │ │ │ │ │ -645 "instead") % │ │ │ │ │ -646 nrChoices % size │ │ │ │ │ -647 << std::endl; │ │ │ │ │ -648 throw std::invalid_argument("DecisionTree::create invalid argument"); │ │ │ │ │ -649 } │ │ │ │ │ -650 auto choice = boost::make_shared(begin->first, endY - beginY); │ │ │ │ │ -651 for (ValueIt y = beginY; y != endY; y++) │ │ │ │ │ -652 choice->push_back(_N_o_d_e_P_t_r(new _L_e_a_f(*y))); │ │ │ │ │ -653 return Choice::Unique(choice); │ │ │ │ │ -654 } │ │ │ │ │ -655 │ │ │ │ │ -656 // Recursive case: perform "Shannon expansion" │ │ │ │ │ -657 // Creates one tree (i.e.,function) for each choice of current key │ │ │ │ │ -658 // by calling create recursively, and then puts them all together. │ │ │ │ │ -659 std::vector functions; │ │ │ │ │ -660 size_t _s_p_l_i_t = size / nrChoices; │ │ │ │ │ -661 for (size_t i = 0; i < nrChoices; i++, beginY += _s_p_l_i_t) { │ │ │ │ │ -662 _N_o_d_e_P_t_r f = create(labelC, end, beginY, beginY + _s_p_l_i_t); │ │ │ │ │ -663 functions.emplace_back(f); │ │ │ │ │ -664 } │ │ │ │ │ -665 return compose(functions.begin(), functions.end(), begin->first); │ │ │ │ │ -666 } │ │ │ │ │ -667 │ │ │ │ │ -668 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -669 template │ │ │ │ │ -670 template │ │ │ │ │ -_6_7_1 typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_c_o_n_v_e_r_t_F_r_o_m( │ │ │ │ │ -672 const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>_:_:_N_o_d_e_P_t_r& f, │ │ │ │ │ -673 std::function L_of_M, │ │ │ │ │ -674 std::function Y_of_X) const { │ │ │ │ │ -675 using LY = _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>; │ │ │ │ │ -676 │ │ │ │ │ -677 // Ugliness below because apparently we can't have templated virtual │ │ │ │ │ -678 // functions. │ │ │ │ │ -679 // If leaf, apply unary conversion "op" and create a unique leaf. │ │ │ │ │ -680 using MXLeaf = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>_:_:_L_e_a_f; │ │ │ │ │ -681 if (auto leaf = boost::dynamic_pointer_cast(f)) { │ │ │ │ │ -682 return _N_o_d_e_P_t_r(new _L_e_a_f(Y_of_X(leaf->constant()), leaf->nrAssignments())); │ │ │ │ │ -683 } │ │ │ │ │ -684 │ │ │ │ │ -685 // Check if Choice │ │ │ │ │ -686 using MXChoice = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>_:_:_C_h_o_i_c_e; │ │ │ │ │ -687 auto choice = boost::dynamic_pointer_cast(f); │ │ │ │ │ -688 if (!choice) throw std::invalid_argument( │ │ │ │ │ -689 "DecisionTree::convertFrom: Invalid NodePtr"); │ │ │ │ │ -690 │ │ │ │ │ -691 // get new label │ │ │ │ │ -692 const M oldLabel = choice->label(); │ │ │ │ │ -693 const L newLabel = L_of_M(oldLabel); │ │ │ │ │ -694 │ │ │ │ │ -695 // put together via Shannon expansion otherwise not sorted. │ │ │ │ │ -696 std::vector functions; │ │ │ │ │ -697 for (auto&& branch : choice->branches()) { │ │ │ │ │ -698 functions.emplace_back(convertFrom(branch, L_of_M, Y_of_X)); │ │ │ │ │ -699 } │ │ │ │ │ -700 return LY::compose(functions.begin(), functions.end(), newLabel); │ │ │ │ │ -701 } │ │ │ │ │ -702 │ │ │ │ │ -703 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -714 template │ │ │ │ │ -_7_1_5 struct _V_i_s_i_t { │ │ │ │ │ -716 using F = std::function; │ │ │ │ │ -_7_1_7 explicit _V_i_s_i_t(F _f) : _f(_f) {} │ │ │ │ │ -_7_1_8 F _f; │ │ │ │ │ -719 │ │ │ │ │ -_7_2_1 void _o_p_e_r_a_t_o_r_(_)(const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r& node) const { │ │ │ │ │ -722 using Leaf = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_L_e_a_f; │ │ │ │ │ -723 if (auto leaf = boost::dynamic_pointer_cast(node)) │ │ │ │ │ -724 return _f(leaf->constant()); │ │ │ │ │ -725 │ │ │ │ │ -726 using Choice = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_C_h_o_i_c_e; │ │ │ │ │ -727 auto choice = boost::dynamic_pointer_cast(node); │ │ │ │ │ -728 if (!choice) │ │ │ │ │ -729 throw std::invalid_argument("DecisionTree::Visit: Invalid NodePtr"); │ │ │ │ │ -730 for (auto&& branch : choice->branches()) (*this)(branch); // recurse! │ │ │ │ │ -731 } │ │ │ │ │ -732 }; │ │ │ │ │ -733 │ │ │ │ │ -734 template │ │ │ │ │ -735 template │ │ │ │ │ -_7_3_6 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_v_i_s_i_t(Func f) const { │ │ │ │ │ -737 _V_i_s_i_t_<_L_,_ _Y_> visit(f); │ │ │ │ │ -738 visit(root_); │ │ │ │ │ -739 } │ │ │ │ │ -740 │ │ │ │ │ -741 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -751 template │ │ │ │ │ -_7_5_2 struct _V_i_s_i_t_L_e_a_f { │ │ │ │ │ -753 using F = std::function_:_:_L_e_a_f&)>; │ │ │ │ │ -_7_5_4 explicit _V_i_s_i_t_L_e_a_f(F _f) : _f(_f) {} │ │ │ │ │ -_7_5_5 F _f; │ │ │ │ │ -756 │ │ │ │ │ -_7_5_8 void _o_p_e_r_a_t_o_r_(_)(const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r& node) const { │ │ │ │ │ -759 using Leaf = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_L_e_a_f; │ │ │ │ │ -760 if (auto leaf = boost::dynamic_pointer_cast(node)) │ │ │ │ │ -761 return _f(*leaf); │ │ │ │ │ -762 │ │ │ │ │ -763 using Choice = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_C_h_o_i_c_e; │ │ │ │ │ -764 auto choice = boost::dynamic_pointer_cast(node); │ │ │ │ │ -765 if (!choice) │ │ │ │ │ -766 throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr"); │ │ │ │ │ -767 for (auto&& branch : choice->branches()) (*this)(branch); // recurse! │ │ │ │ │ -768 } │ │ │ │ │ -769 }; │ │ │ │ │ -770 │ │ │ │ │ -771 template │ │ │ │ │ -772 template │ │ │ │ │ -_7_7_3 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_v_i_s_i_t_L_e_a_f(Func f) const { │ │ │ │ │ -774 _V_i_s_i_t_L_e_a_f_<_L_,_ _Y_> visit(f); │ │ │ │ │ -775 visit(root_); │ │ │ │ │ -776 } │ │ │ │ │ -777 │ │ │ │ │ -778 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -785 template │ │ │ │ │ -_7_8_6 struct _V_i_s_i_t_W_i_t_h { │ │ │ │ │ -787 using F = std::function&, const Y&)>; │ │ │ │ │ -_7_8_8 explicit _V_i_s_i_t_W_i_t_h(F _f) : _f(_f) {} │ │ │ │ │ -_7_8_9 _A_s_s_i_g_n_m_e_n_t_<_L_> _a_s_s_i_g_n_m_e_n_t; │ │ │ │ │ -_7_9_0 F _f; │ │ │ │ │ -791 │ │ │ │ │ -_7_9_3 void _o_p_e_r_a_t_o_r_(_)(const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_N_o_d_e_P_t_r& node) { │ │ │ │ │ -794 using Leaf = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_L_e_a_f; │ │ │ │ │ -795 if (auto leaf = boost::dynamic_pointer_cast(node)) │ │ │ │ │ -796 return _f(_a_s_s_i_g_n_m_e_n_t, leaf->constant()); │ │ │ │ │ -797 │ │ │ │ │ -798 using Choice = typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_C_h_o_i_c_e; │ │ │ │ │ -799 auto choice = boost::dynamic_pointer_cast(node); │ │ │ │ │ -800 if (!choice) │ │ │ │ │ -801 throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); │ │ │ │ │ -802 for (size_t i = 0; i < choice->nrChoices(); i++) { │ │ │ │ │ -803 _a_s_s_i_g_n_m_e_n_t[choice->label()] = i; // Set assignment for label to i │ │ │ │ │ -804 │ │ │ │ │ -805 (*this)(choice->branches()[i]); // recurse! │ │ │ │ │ -806 │ │ │ │ │ -807 // Remove the choice so we are backtracking │ │ │ │ │ -808 auto choice_it = _a_s_s_i_g_n_m_e_n_t.find(choice->label()); │ │ │ │ │ -809 _a_s_s_i_g_n_m_e_n_t.erase(choice_it); │ │ │ │ │ -810 } │ │ │ │ │ -811 } │ │ │ │ │ -812 }; │ │ │ │ │ -813 │ │ │ │ │ -814 template │ │ │ │ │ -815 template │ │ │ │ │ -_8_1_6 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_v_i_s_i_t_W_i_t_h(Func f) const { │ │ │ │ │ -817 _V_i_s_i_t_W_i_t_h_<_L_,_ _Y_> visit(f); │ │ │ │ │ -818 visit(root_); │ │ │ │ │ -819 } │ │ │ │ │ -820 │ │ │ │ │ -821 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -822 template │ │ │ │ │ -_8_2_3 size_t _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_n_r_L_e_a_v_e_s() const { │ │ │ │ │ -824 size_t total = 0; │ │ │ │ │ -825 visit([&total](const Y& node) { total += 1; }); │ │ │ │ │ -826 return total; │ │ │ │ │ -827 } │ │ │ │ │ -828 │ │ │ │ │ -829 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -830 // fold is just done with a visit │ │ │ │ │ -831 template │ │ │ │ │ -832 template │ │ │ │ │ -_8_3_3 X _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_f_o_l_d(Func f, X x0) const { │ │ │ │ │ -834 visit([&](const Y& y) { x0 = f(y, x0); }); │ │ │ │ │ -835 return x0; │ │ │ │ │ -836 } │ │ │ │ │ -837 │ │ │ │ │ -838 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -852 template │ │ │ │ │ -_8_5_3 std::set _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_l_a_b_e_l_s() const { │ │ │ │ │ -854 std::set unique; │ │ │ │ │ -855 auto f = [&](const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment, const Y&) { │ │ │ │ │ -856 for (auto&& kv : assignment) { │ │ │ │ │ -857 unique.insert(kv.first); │ │ │ │ │ -858 } │ │ │ │ │ -859 }; │ │ │ │ │ -860 visitWith(f); │ │ │ │ │ -861 return unique; │ │ │ │ │ -862 } │ │ │ │ │ -863 │ │ │ │ │ -864/ │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -865 template │ │ │ │ │ -866 bool _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_e_q_u_a_l_s(const _D_e_c_i_s_i_o_n_T_r_e_e& other, │ │ │ │ │ -867 const CompareFunc& compare) const { │ │ │ │ │ -868 return root_->equals(*other._r_o_o_t__, compare); │ │ │ │ │ -869 } │ │ │ │ │ -870 │ │ │ │ │ -871 template │ │ │ │ │ -_8_7_2 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ -873 const LabelFormatter& labelFormatter, │ │ │ │ │ -874 const ValueFormatter& valueFormatter) const { │ │ │ │ │ -875 root_->print(s, labelFormatter, valueFormatter); │ │ │ │ │ -876 } │ │ │ │ │ -877 │ │ │ │ │ -878 template │ │ │ │ │ -_8_7_9 bool _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_o_p_e_r_a_t_o_r_=_=(const _D_e_c_i_s_i_o_n_T_r_e_e& other) const { │ │ │ │ │ -880 return root_->equals(*other._r_o_o_t__); │ │ │ │ │ -881 } │ │ │ │ │ -882 │ │ │ │ │ -883 template │ │ │ │ │ -_8_8_4 const Y& _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_o_p_e_r_a_t_o_r_(_)(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const { │ │ │ │ │ -885 return root_->operator ()(x); │ │ │ │ │ -886 } │ │ │ │ │ -887 │ │ │ │ │ -888 template │ │ │ │ │ -_8_8_9 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_a_p_p_l_y(const _U_n_a_r_y& op) const { │ │ │ │ │ -890 // It is unclear what should happen if tree is empty: │ │ │ │ │ -891 if (empty()) { │ │ │ │ │ -892 throw std::runtime_error( │ │ │ │ │ -893 "DecisionTree::apply(unary op) undefined for empty tree."); │ │ │ │ │ -894 } │ │ │ │ │ -895 return _D_e_c_i_s_i_o_n_T_r_e_e(root_->apply(op)); │ │ │ │ │ -896 } │ │ │ │ │ -897 │ │ │ │ │ -899 template │ │ │ │ │ -_9_0_0 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_a_p_p_l_y( │ │ │ │ │ -901 const UnaryAssignment& op) const { │ │ │ │ │ -902 // It is unclear what should happen if tree is empty: │ │ │ │ │ -903 if (empty()) { │ │ │ │ │ -904 throw std::runtime_error( │ │ │ │ │ -905 "DecisionTree::apply(unary op) undefined for empty tree."); │ │ │ │ │ -906 } │ │ │ │ │ -907 _A_s_s_i_g_n_m_e_n_t_<_L_> assignment; │ │ │ │ │ -908 return _D_e_c_i_s_i_o_n_T_r_e_e(root_->apply(op, assignment)); │ │ │ │ │ -909 } │ │ │ │ │ -910 │ │ │ │ │ -911 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -912 template │ │ │ │ │ -_9_1_3 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _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& g, │ │ │ │ │ -914 const Binary& op) const { │ │ │ │ │ -915 // It is unclear what should happen if either tree is empty: │ │ │ │ │ -916 if (empty() || g._e_m_p_t_y()) { │ │ │ │ │ -917 throw std::runtime_error( │ │ │ │ │ -918 "DecisionTree::apply(binary op) undefined for empty trees."); │ │ │ │ │ -919 } │ │ │ │ │ -920 // apply the operaton on the root of both diagrams │ │ │ │ │ -921 _N_o_d_e_P_t_r h = root_->apply_f_op_g(*g._r_o_o_t__, op); │ │ │ │ │ -922 // create a new class with the resulting root "h" │ │ │ │ │ -923 _D_e_c_i_s_i_o_n_T_r_e_e result(h); │ │ │ │ │ -924 return result; │ │ │ │ │ -925 } │ │ │ │ │ -926 │ │ │ │ │ -927 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -928 // The way this works: │ │ │ │ │ -929 // We have an ADT, picture it as a tree. │ │ │ │ │ -930 // At a certain depth, we have a branch on "label". │ │ │ │ │ -931 // The function "choose(label,index)" will return a tree of one less depth, │ │ │ │ │ -932 // where there is no more branch on "label": only the subtree under that │ │ │ │ │ -933 // branch point corresponding to the value "index" is left instead. │ │ │ │ │ -934 // The function below get all these smaller trees and "ops" them together. │ │ │ │ │ -935 // This implements marginalization in Darwiche09book, pg 330 │ │ │ │ │ -936 template │ │ │ │ │ -_9_3_7 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_c_o_m_b_i_n_e(const L& label, │ │ │ │ │ -938 size_t cardinality, const Binary& op) const { │ │ │ │ │ -939 _D_e_c_i_s_i_o_n_T_r_e_e result = choose(label, 0); │ │ │ │ │ -940 for (size_t index = 1; index < cardinality; index++) { │ │ │ │ │ -941 _D_e_c_i_s_i_o_n_T_r_e_e chosen = choose(label, index); │ │ │ │ │ -942 result = result.apply(chosen, op); │ │ │ │ │ -943 } │ │ │ │ │ -944 return result; │ │ │ │ │ -945 } │ │ │ │ │ -946 │ │ │ │ │ -947 / │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -948 template │ │ │ │ │ -_9_4_9 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ -950 const LabelFormatter& labelFormatter, │ │ │ │ │ -951 const ValueFormatter& valueFormatter, │ │ │ │ │ -952 bool showZero) const { │ │ │ │ │ -953 os << "digraph G {\n"; │ │ │ │ │ -954 root_->dot(os, labelFormatter, valueFormatter, showZero); │ │ │ │ │ -955 os << " [ordering=out]}" << std::endl; │ │ │ │ │ -956 } │ │ │ │ │ -957 │ │ │ │ │ -958 template │ │ │ │ │ -_9_5_9 void _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_d_o_t(const std::string& name, │ │ │ │ │ -960 const LabelFormatter& labelFormatter, │ │ │ │ │ -961 const ValueFormatter& valueFormatter, │ │ │ │ │ -962 bool showZero) const { │ │ │ │ │ -963 std::ofstream os((name + ".dot").c_str()); │ │ │ │ │ -964 _d_o_t(os, labelFormatter, valueFormatter, showZero); │ │ │ │ │ -965 int result = │ │ │ │ │ -966 system(("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null") │ │ │ │ │ -967 .c_str()); │ │ │ │ │ -968 if (result == -1) │ │ │ │ │ -969 throw std::runtime_error("DecisionTree::dot system call failed"); │ │ │ │ │ -970 } │ │ │ │ │ -971 │ │ │ │ │ -972 template │ │ │ │ │ -_9_7_3 std::string _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_d_o_t(const LabelFormatter& labelFormatter, │ │ │ │ │ -974 const ValueFormatter& valueFormatter, │ │ │ │ │ -975 bool showZero) const { │ │ │ │ │ -976 std::stringstream ss; │ │ │ │ │ -977 _d_o_t(ss, labelFormatter, valueFormatter, showZero); │ │ │ │ │ -978 return ss.str(); │ │ │ │ │ -979 } │ │ │ │ │ -980 │ │ │ │ │ -981/ │ │ │ │ │ -******************************************************************************/ │ │ │ │ │ -982 │ │ │ │ │ -983 } // namespace gtsam │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ -Decision Tree for use in DiscreteFactors. │ │ │ │ │ +136 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ +137 │ │ │ │ │ +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_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_L_e_a_f_:_:_c_h_o_o_s_e │ │ │ │ │ -NodePtr choose(const L &label, size_t index) const override │ │ │ │ │ -choose a branch, create new memory ! │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -const Y & operator()(const Assignment< L > &x) const override │ │ │ │ │ -evaluate │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_a_p_p_l_y │ │ │ │ │ -NodePtr apply(const UnaryAssignment &op, const Assignment< L > &assignment) │ │ │ │ │ -const override │ │ │ │ │ -Apply unary operator with assignment. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Node &q, const CompareFunc &compare) const override │ │ │ │ │ -equality up to tolerance │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_c_o_n_s_t_a_n_t__ │ │ │ │ │ -Y constant_ │ │ │ │ │ -constant stored in this leaf │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_a_p_p_l_y │ │ │ │ │ -NodePtr apply(const Unary &op) const override │ │ │ │ │ -apply unary operator │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_s_a_m_e_L_e_a_f │ │ │ │ │ -bool sameLeaf(const Leaf &q) const override │ │ │ │ │ -Leaf-Leaf equality. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_L_e_a_f │ │ │ │ │ -Leaf(const Y &constant, size_t nrAssignments=1) │ │ │ │ │ -Constructor from constant. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_n_r_A_s_s_i_g_n_m_e_n_t_s__ │ │ │ │ │ -size_t nrAssignments_ │ │ │ │ │ -The number of assignments contained within this leaf. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter, bool showZero) const override │ │ │ │ │ -Write graphviz format to stream os. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_L_e_a_f │ │ │ │ │ -Leaf() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_s_a_m_e_L_e_a_f │ │ │ │ │ -bool sameLeaf(const Node &q) const override │ │ │ │ │ -polymorphic equality: is q a leaf and is it the same as this leaf? │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_c_o_n_s_t_a_n_t │ │ │ │ │ -const Y & constant() const │ │ │ │ │ -Return the constant. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f_:_:_n_r_A_s_s_i_g_n_m_e_n_t_s │ │ │ │ │ -size_t nrAssignments() const │ │ │ │ │ -Return the number of assignments contained within this leaf. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:74 │ │ │ │ │ -_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_:_:_C_h_o_i_c_e_:_:_a_p_p_l_y │ │ │ │ │ -NodePtr apply(const Unary &op) const override │ │ │ │ │ -apply unary operator. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:389 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_C_h_o_i_c_e │ │ │ │ │ -Choice(const L &label, const Choice &f, const UnaryAssignment &op, const │ │ │ │ │ -Assignment< L > &assignment) │ │ │ │ │ -Constructor which accepts a UnaryAssignment op and the corresponding │ │ │ │ │ -assignment. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_l_a_b_e_l │ │ │ │ │ -const L & label() const │ │ │ │ │ -Return the label of this choice node. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:258 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter) const override │ │ │ │ │ -print (as a tree). │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:280 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_a_p_p_l_y │ │ │ │ │ -NodePtr apply(const UnaryAssignment &op, const Assignment< L > &assignment) │ │ │ │ │ -const override │ │ │ │ │ -Apply unary operator with assignment. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:395 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_l_a_b_e_l__ │ │ │ │ │ -L label_ │ │ │ │ │ -the label of the variable on which we split │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_s_a_m_e_L_e_a_f │ │ │ │ │ -bool sameLeaf(const Node &q) const override │ │ │ │ │ -polymorphic equality: if q is a leaf, could be... │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:318 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_C_h_o_i_c_e │ │ │ │ │ -Choice(const Choice &f, const Choice &g, const Binary &op) │ │ │ │ │ -Construct from applying binary op to two Choice nodes. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -void push_back(const NodePtr &node) │ │ │ │ │ -add a branch: TODO merge into constructor │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:271 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_b_r_a_n_c_h_e_s__ │ │ │ │ │ -std::vector< NodePtr > branches_ │ │ │ │ │ -The children of this Choice node. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_C_h_o_i_c_e │ │ │ │ │ -Choice() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -const Y & operator()(const Assignment< L > &x) const override │ │ │ │ │ -evaluate │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:336 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_C_h_o_i_c_e │ │ │ │ │ -Choice(const L &label, size_t count) │ │ │ │ │ -Constructor, given choice label and mandatory expected branch count. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:224 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_c_h_o_o_s_e │ │ │ │ │ -NodePtr choose(const L &label, size_t index) const override │ │ │ │ │ -choose a branch, recursively │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:434 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter, bool showZero) const override │ │ │ │ │ -output to graphviz (as a a graph) │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:290 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_s_a_m_e_L_e_a_f │ │ │ │ │ -bool sameLeaf(const Leaf &q) const override │ │ │ │ │ -Choice-Leaf equality: always false. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:313 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_U_n_i_q_u_e │ │ │ │ │ -static NodePtr Unique(const ChoicePtr &f) │ │ │ │ │ -If all branches of a choice node f are the same, just return a branch. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:200 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Node &q, const CompareFunc &compare) const override │ │ │ │ │ -equality │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:323 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t │ │ │ │ │ -Functor performing depth-first visit to each leaf with the leaf value as the │ │ │ │ │ -argument. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:715 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_:_:_f │ │ │ │ │ -F f │ │ │ │ │ -folding function object. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:718 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -void operator()(const typename DecisionTree< L, Y >::NodePtr &node) const │ │ │ │ │ -Do a depth-first visit on the tree rooted at node. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:721 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_:_:_V_i_s_i_t │ │ │ │ │ -Visit(F f) │ │ │ │ │ -Construct from folding function. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:717 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_L_e_a_f │ │ │ │ │ -Functor performing depth-first visit to each leaf with the Leaf object passed │ │ │ │ │ -as an argument. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:752 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_L_e_a_f_:_:_V_i_s_i_t_L_e_a_f │ │ │ │ │ -VisitLeaf(F f) │ │ │ │ │ -Construct from folding function. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:754 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_L_e_a_f_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -void operator()(const typename DecisionTree< L, Y >::NodePtr &node) const │ │ │ │ │ -Do a depth-first visit on the tree rooted at node. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:758 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_L_e_a_f_:_:_f │ │ │ │ │ -F f │ │ │ │ │ -folding function object. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:755 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_W_i_t_h │ │ │ │ │ -Functor performing depth-first visit to each leaf with the leaf's Assignment │ │ │ │ │ -and value passed as a... │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:786 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_W_i_t_h_:_:_V_i_s_i_t_W_i_t_h │ │ │ │ │ -VisitWith(F f) │ │ │ │ │ -Construct from folding function. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:788 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_W_i_t_h_:_:_a_s_s_i_g_n_m_e_n_t │ │ │ │ │ -Assignment< L > assignment │ │ │ │ │ -Assignment, mutating through recursion. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:789 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_W_i_t_h_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -void operator()(const typename DecisionTree< L, Y >::NodePtr &node) │ │ │ │ │ -Do a depth-first visit on the tree rooted at node. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:793 │ │ │ │ │ -_g_t_s_a_m_:_:_V_i_s_i_t_W_i_t_h_:_:_f │ │ │ │ │ -F f │ │ │ │ │ -folding function object. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:790 │ │ │ │ │ -_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_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_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_:_:_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_:_:_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 │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * DDeecciissiioonnTTrreeee--iinnll..hh │ │ │ │ │ + * _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/a00203.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteJunctionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteDistribution.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,32 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    DiscreteJunctionTree.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    DiscreteDistribution.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::DiscreteJunctionTree
     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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Typedefs

    │ │ │ │ -using gtsam::DiscreteCluster = DiscreteJunctionTree::Cluster
     typedef for wrapper:
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Mar 29, 2013
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Date
    December 2021
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 | _T_y_p_e_d_e_f_s │ │ │ │ │ -DiscreteJunctionTree.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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteDistribution.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::DDiissccrreetteeCClluusstteerr = _D_i_s_c_r_e_t_e_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 │ │ │ │ │ + December 2021 │ │ │ │ │ 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_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_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/a00206.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/AlgebraicDecisionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,36 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    DiscreteValues.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    AlgebraicDecisionTree.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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
    January, 2022
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Feb 20, 2022
    │ │ │ │ +
    Author
    Mike Sheffler
    │ │ │ │ +
    │ │ │ │ +Duy-Nguyen Ta
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -DiscreteValues.cpp File Reference │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ - January, 2022 │ │ │ │ │ + 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_V_a_l_u_e_s_._c_p_p │ │ │ │ │ + * _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/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/Signature.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,83 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    DiscreteFactor.cpp File Reference
    │ │ │ │ +Typedefs | │ │ │ │ +Functions | │ │ │ │ +Variables
    │ │ │ │ +
    Signature.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    discrete factor │ │ │ │ +

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

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

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    │ │ │ │ +typedef string::const_iterator gtsam::parser::It
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

    │ │ │ │ Functions

    std::vector< double > gtsam::expNormalize (const std::vector< double > &logProbs)
     Normalize a set of log probabilities.
     
    │ │ │ │ +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

    │ │ │ │ -

    discrete factor

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

    signatures for conditional densities

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Feb 27, 2011
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,57 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 │ │ │ │ │ +typedef string::const_iterator  ggttssaamm::::ppaarrsseerr::::IItt │ │ │ │ │ +  │ │ │ │ │ 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. │ │ │ │ │ +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 ********** │ │ │ │ │ -discrete factor │ │ │ │ │ - Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ +signatures for conditional densities │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ 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_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _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/a00209.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00209 = [ │ │ │ │ │ - ["expNormalize", "a00209.html#ae1c1177c8725296b34601409322400ea", null] │ │ │ │ │ + ["operator%", "a00209.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ + ["operator%", "a00209.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ + ["operator|", "a00209.html#aa6c58dfb2fc9398a026e3268ba2edfad", 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    44 Base;
    │ │ │ │ -
    45 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    46 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ - │ │ │ │ -
    48 virtual ~DiscreteBayesTreeClique() {}
    │ │ │ │ - │ │ │ │ -
    50 const boost::shared_ptr<DiscreteConditional>& conditional)
    │ │ │ │ -
    51 : Base(conditional) {}
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    50 typedef boost::shared_ptr<DecisionTreeFactor> shared_ptr;
    │ │ │ │ + │ │ │ │
    52
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ + │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ -
    78
    │ │ │ │ -
    79 public:
    │ │ │ │ -
    80 typedef DiscreteBayesTree This;
    │ │ │ │ -
    81 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    82
    │ │ │ │ -
    85
    │ │ │ │ - │ │ │ │ +
    64 DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
    │ │ │ │ +
    65
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ -
    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)
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ +
    Algebraic Decision Trees.
    │ │ │ │ + │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │ +
    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
    │ │ │ │ +
    string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of markdown.
    Definition DiscreteValues.cpp:129
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
    free versions of apply
    Definition DecisionTree.h:413
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A clique in a DiscreteBayesTree.
    Definition DiscreteBayesTree.h:40
    │ │ │ │ -
    void printSignature(const std::string &s="Clique: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print index signature only
    Definition DiscreteBayesTree.h:54
    │ │ │ │ -
    A Bayes tree representing a Discrete density.
    Definition DiscreteBayesTree.h:75
    │ │ │ │ -
    DiscreteBayesTree()
    Default constructor, creates an empty Bayes tree.
    Definition DiscreteBayesTree.h:86
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ +
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ +
    shared_ptr sum(const Ordering &keys) const
    Create new factor by summing all values with the same separator values.
    Definition DecisionTreeFactor.h:173
    │ │ │ │ +
    double operator()(const DiscreteValues &values) const override
    Evaluate probability density, sugar.
    Definition DecisionTreeFactor.h:143
    │ │ │ │ +
    DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override
    multiply two factors
    Definition DecisionTreeFactor.h:151
    │ │ │ │ +
    shared_ptr max(size_t nrFrontals) const
    Create new factor by maximizing over all values with the same separator.
    Definition DecisionTreeFactor.h:178
    │ │ │ │ +
    shared_ptr max(const Ordering &keys) const
    Create new factor by maximizing over all values with the same separator.
    Definition DecisionTreeFactor.h:183
    │ │ │ │ +
    double evaluate(const DiscreteValues &values) const
    Calculate probability for given values x, is just look up in AlgebraicDecisionTree.
    Definition DecisionTreeFactor.h:138
    │ │ │ │ +
    DiscreteFactor Base
    Typedef to base class.
    Definition DecisionTreeFactor.h:49
    │ │ │ │ +
    shared_ptr sum(size_t nrFrontals) const
    Create new factor by summing all values with the same separator values.
    Definition DecisionTreeFactor.h:168
    │ │ │ │ +
    DecisionTreeFactor(const DiscreteKey &key, SOURCE table)
    Single-key specialization.
    Definition DecisionTreeFactor.h:110
    │ │ │ │ +
    DecisionTreeFactor toDecisionTreeFactor() const override
    Convert into a decisiontree.
    Definition DecisionTreeFactor.h:165
    │ │ │ │ +
    DecisionTreeFactor operator/(const DecisionTreeFactor &f) const
    divide by factor f (safely)
    Definition DecisionTreeFactor.h:160
    │ │ │ │ +
    DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row)
    Single-key specialization, with vector of doubles.
    Definition DecisionTreeFactor.h:114
    │ │ │ │ +
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ +
    Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
    Definition DiscreteFactor.h:38
    │ │ │ │ +
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    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_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_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_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 │ │ │ │ │ _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/DiscreteValues.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,58 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    DiscreteConditional.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    DiscreteValues.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::DiscreteConditional
     Discrete Conditional Density Derives from DecisionTreeFactor. More...
    class  gtsam::DiscreteValues
     A map from keys to values. More...
     
    struct  gtsam::traits< DiscreteConditional >
    struct  gtsam::traits< DiscreteValues >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

    │ │ │ │ +std::vector< DiscreteValuesgtsam::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 14, 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 │ │ │ │ │ -DiscreteConditional.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_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_._._. │ │ │ │ │ + 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_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ +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 14, 2011 │ │ │ │ │ + Dec 13, 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_C_o_n_d_i_t_i_o_n_a_l_._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/a00215.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00215 = [ │ │ │ │ │ - ["gtsam::traits< DiscreteConditional >", "a02836.html", null] │ │ │ │ │ + ["gtsam::traits< DiscreteValues >", "a02904.html", null], │ │ │ │ │ + ["cartesianProduct", "a00215.html#a55a86a4657d82f6ff048a10d058c529f", null], │ │ │ │ │ + ["html", "a00215.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ + ["markdown", "a00215.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00215_source.html │ │ │ │ @@ -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/discrete/DiscreteValues.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,228 +98,136 @@ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DiscreteConditional.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/inference/Conditional-inst.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25#include <boost/make_shared.hpp>
    │ │ │ │ -
    26#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    27#include <string>
    │ │ │ │ -
    28#include <vector>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <gtsam/inference/Key.h>
    │ │ │ │ +
    23
    │ │ │ │ +
    24#include <map>
    │ │ │ │ +
    25#include <string>
    │ │ │ │ +
    26#include <vector>
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    44 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    48
    │ │ │ │ - │ │ │ │ -
    50
    │ │ │ │ -
    53
    │ │ │ │ - │ │ │ │ -
    56
    │ │ │ │ -
    58 DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    64 DiscreteConditional(size_t nFrontals, const DiscreteKeys& keys,
    │ │ │ │ -
    65 const ADT& potentials);
    │ │ │ │ +
    61 bool equals(const DiscreteValues& x, double tol = 1e-9) const;
    │ │ │ │ +
    62
    │ │ │ │
    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)) {}
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ +
    79 DiscreteValues& update(const DiscreteValues& values);
    │ │ │ │
    80
    │ │ │ │ -
    │ │ │ │ -
    90 DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
    │ │ │ │ -
    91 const std::string& spec)
    │ │ │ │ -
    92 : DiscreteConditional(Signature(key, parents, spec)) {}
    │ │ │ │ +
    │ │ │ │ +
    85 static std::vector<DiscreteValues> CartesianProduct(
    │ │ │ │ +
    86 const DiscreteKeys& keys) {
    │ │ │ │ +
    87 return Base::CartesianProduct<DiscreteValues>(keys);
    │ │ │ │ +
    88 }
    │ │ │ │
    │ │ │ │ +
    89
    │ │ │ │
    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
    │ │ │ │ +
    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) {
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    signatures for conditional densities
    │ │ │ │ - │ │ │ │ -
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │ +
    138// traits
    │ │ │ │ +
    139template <>
    │ │ │ │ +
    140struct traits<DiscreteValues> : public Testable<DiscreteValues> {};
    │ │ │ │ +
    141
    │ │ │ │ +
    142} // namespace gtsam
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ +
    An assignment from labels to a discrete value index (size_t)
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │
    string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of markdown.
    Definition DiscreteValues.cpp:129
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys)
    Free version of CartesianProduct.
    Definition DiscreteValues.h:124
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ - │ │ │ │ -
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ -
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ -
    DiscreteConditional(const DiscreteKey &key, const std::string &spec)
    No-parent specialization; can also use DiscreteDistribution.
    Definition DiscreteConditional.h:95
    │ │ │ │ -
    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
    │ │ │ │ -
    double evaluate(const DiscreteValues &values) const
    Evaluate, just look up in AlgebraicDecisonTree.
    Definition DiscreteConditional.h:163
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition DiscreteConditional.h:44
    │ │ │ │ -
    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
    │ │ │ │ -
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition DiscreteConditional.h:47
    │ │ │ │ -
    double logProbability(const HybridValues &x) const override
    < HybridValues version
    Definition DiscreteConditional.h:253
    │ │ │ │ -
    DiscreteConditional This
    Typedef to this class.
    Definition DiscreteConditional.h:43
    │ │ │ │ -
    DecisionTreeFactor BaseFactor
    Typedef to our factor base class.
    Definition DiscreteConditional.h:45
    │ │ │ │ -
    DiscreteConditional()
    Default constructor needed for serialization.
    Definition DiscreteConditional.h:55
    │ │ │ │ -
    double logProbability(const DiscreteValues &x) const
    Log-probability is just -error(x).
    Definition DiscreteConditional.h:151
    │ │ │ │ -
    void printSignature(const std::string &s="Discrete Conditional: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print index signature only
    Definition DiscreteConditional.h:156
    │ │ │ │ -
    double logNormalizationConstant() const override
    logNormalizationConstant K is just zero, such that logProbability(x) = log(evaluate(x)) = - error(x) ...
    Definition DiscreteConditional.h:262
    │ │ │ │ +
    An assignment from labels to value index (size_t).
    Definition Assignment.h:37
    │ │ │ │
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    Signature for a discrete conditional density, used to construct conditionals.
    Definition Signature.h:54
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    the error.
    │ │ │ │ +
    std::map< Key, std::vector< std::string > > Names
    Translation table from values to strings.
    Definition DiscreteValues.h:95
    │ │ │ │ +
    static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
    Return a vector of DiscreteValues, one for each possible combination of values.
    Definition DiscreteValues.h:85
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,286 +1,168 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteConditional.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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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); │ │ │ │ │ +_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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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)) {} │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -_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)) {} │ │ │ │ │ +_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 _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 = │ │ │ │ │ +_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, │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ _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_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_:_:_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_:_:_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_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_C_o_n_d_i_t_i_o_n_a_l_._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/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/DiscreteMarginals.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DiscreteEliminationTree.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 │ │ │ │ │ -DiscreteEliminationTree.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_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_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/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/Assignment.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,73 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    DecisionTree.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    Assignment.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    An assignment from labels to a discrete value index (size_t) │ │ │ │ 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 > >
    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

    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

    │ │ │ │ -

    Decision Tree for use in DiscreteFactors.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Can Erdogan
    │ │ │ │ -
    Date
    Jan 30, 2012
    │ │ │ │ +

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

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Feb 5, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,59 +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 │ │ │ │ │ -DecisionTree.h File Reference │ │ │ │ │ -Decision Tree for use in DiscreteFactors. _M_o_r_e_._._. │ │ │ │ │ +_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_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_ _>_ _> │ │ │ │ │ +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 │ │ │ │ │ -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 ********** │ │ │ │ │ -Decision Tree for use in DiscreteFactors. │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Can Erdogan │ │ │ │ │ Date │ │ │ │ │ - Jan 30, 2012 │ │ │ │ │ + Feb 5, 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 │ │ │ │ │ + * _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/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/Assignment.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,333 +98,110 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DecisionTree.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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ -
    22#include <gtsam/base/Testable.h>
    │ │ │ │ -
    23#include <gtsam/base/types.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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    62 protected:
    │ │ │ │ -
    │ │ │ │ -
    64 static bool DefaultCompare(const Y& a, const Y& b) {
    │ │ │ │ -
    65 return a == b;
    │ │ │ │ -
    66 }
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    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#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:
    │ │ │ │ - │ │ │ │ -
    136 template <class ARCHIVE>
    │ │ │ │ -
    137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {}
    │ │ │ │ -
    138 };
    │ │ │ │ -
    │ │ │ │ -
    141 public:
    │ │ │ │ -
    143 using NodePtr = typename Node::Ptr;
    │ │ │ │ -
    144
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    414 const typename DecisionTree<L, Y>::Unary& op) {
    │ │ │ │ -
    415 return f.apply(op);
    │ │ │ │ -
    416 }
    │ │ │ │ +
    110}; // Assignment
    │ │ │ │
    │ │ │ │ -
    417
    │ │ │ │ -
    419 template<typename L, typename Y>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    421 const typename DecisionTree<L, Y>::UnaryAssignment& op) {
    │ │ │ │ -
    422 return f.apply(op);
    │ │ │ │ -
    423 }
    │ │ │ │ -
    │ │ │ │ -
    424
    │ │ │ │ -
    426 template<typename L, typename Y>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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; }),
    │ │ │ │ - │ │ │ │ -
    445 [](std::pair<T1, T2> i) { return i.second; }));
    │ │ │ │ -
    446 }
    │ │ │ │ -
    │ │ │ │ -
    447
    │ │ │ │ -
    448} // namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    An assignment from labels to a discrete value index (size_t)
    │ │ │ │ +
    111
    │ │ │ │ +
    112} // namespace gtsam
    │ │ │ │ +
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
    free versions of apply
    Definition DecisionTree.h:413
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    An assignment from labels to value index (size_t).
    Definition Assignment.h:37
    │ │ │ │ -
    Definition DecisionTree-inl.h:52
    │ │ │ │ -
    Definition DecisionTree-inl.h:172
    │ │ │ │ -
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ -
    DecisionTree apply(const Unary &op) const
    apply Unary operation "op" to f
    Definition DecisionTree-inl.h:889
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    DecisionTree combine(const LabelC &labelC, const Binary &op) const
    combine with LabelC for convenience
    Definition DecisionTree.h:368
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual ~DecisionTree()=default
    Make virtual.
    │ │ │ │ -
    static bool DefaultCompare(const Y &a, const Y &b)
    Default method for comparison of two objects of type Y.
    Definition DecisionTree.h:64
    │ │ │ │ -
    typename Node::Ptr NodePtr
    ---------------------— Node base class ------------------------—
    Definition DecisionTree.h:143
    │ │ │ │ -
    std::set< L > labels() const
    Retrieve all unique labels as a set.
    Definition DecisionTree-inl.h:853
    │ │ │ │ -
    bool empty() const
    Check if tree is empty.
    Definition DecisionTree.h:257
    │ │ │ │ -
    void visit(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:736
    │ │ │ │ -
    void visitLeaf(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:773
    │ │ │ │ -
    std::function< Y(const Y &)> Unary
    Handy typedefs for unary and binary function types.
    Definition DecisionTree.h:74
    │ │ │ │ -
    X fold(Func f, X x0) const
    Fold a binary function over the tree, returning accumulator.
    Definition DecisionTree-inl.h:833
    │ │ │ │ -
    NodePtr root_
    A DecisionTree just contains the root. TODO(dellaert): make protected.
    Definition DecisionTree.h:146
    │ │ │ │ -
    void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
    GTSAM-style print.
    Definition DecisionTree-inl.h:872
    │ │ │ │ -
    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
    │ │ │ │ -
    void visitWith(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:816
    │ │ │ │ -
    const Y & operator()(const Assignment< L > &x) const
    evaluate
    Definition DecisionTree-inl.h:884
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition DecisionTree.h:399
    │ │ │ │ -
    bool operator==(const DecisionTree &q) const
    equality
    Definition DecisionTree-inl.h:879
    │ │ │ │ -
    std::pair< L, size_t > LabelC
    A label annotated with cardinality.
    Definition DecisionTree.h:79
    │ │ │ │ -
    size_t nrLeaves() const
    Return the number of leaves in the tree.
    Definition DecisionTree-inl.h:823
    │ │ │ │ -
    DecisionTree()
    Default constructor (for serialization)
    Definition DecisionTree-inl.h:462
    │ │ │ │ -
    ---------------------— Node base class ------------------------—
    Definition DecisionTree.h:86
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition DecisionTree.h:135
    │ │ │ │ +
    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
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,417 +1,111 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DecisionTree.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 │ │ │ │ │ -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#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 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -109 // Unique ID for dot files │ │ │ │ │ -110 const void* id() const { return this; } │ │ │ │ │ +108 return allPossValues; │ │ │ │ │ +109 } │ │ │ │ │ +110}; // Assignment │ │ │ │ │ 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; │ │ │ │ │ -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 │ │ │ │ │ -_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) │ │ │ │ │ +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_:_:_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_:_:_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_e_c_i_s_i_o_n_T_r_e_e_._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/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/DiscreteEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,34 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    AlgebraicDecisionTree.h File Reference
    │ │ │ │ +
    DiscreteEliminationTree.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
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +DiscreteEliminationTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Algebraic Decision Trees. │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Mar 14, 2011 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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_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/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/DiscreteBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,57 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    DiscreteFactorGraph.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    DiscreteBayesTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

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

    │ │ │ │ + │ │ │ │

    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...
    class  gtsam::DiscreteBayesTreeClique
     A clique in a DiscreteBayesTree. More...
     
    struct  gtsam::traits< DiscreteFactorGraph >
     traits 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::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

    │ │ │ │ -
    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,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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteFactorGraph.h 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 │ │ │ │ │ -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_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_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_._._. │ │ │ │ │ +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_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 ********** │ │ │ │ │ - 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_._h │ │ │ │ │ + * _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/a00230.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a00230 = [ │ │ │ │ │ - ["gtsam::EliminationTraits< DiscreteFactorGraph >", "a02860.html", "a02860"], │ │ │ │ │ - ["gtsam::traits< DiscreteFactorGraph >", "a02868.html", null], │ │ │ │ │ - ["EliminateDiscrete", "a00230.html#ga6996e9c84467350b071645649231223a", null], │ │ │ │ │ - ["EliminateForMPE", "a00230.html#ga9bd0b2859a7513198a369f5de3c8cde3", null] │ │ │ │ │ + ["gtsam::DiscreteBayesTreeClique", "a02824.html", "a02824"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00230_source.html │ │ │ │ @@ -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/discrete/DiscreteBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,233 +98,134 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DiscreteFactorGraph.h
    │ │ │ │ +
    DiscreteBayesTree.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <gtsam/base/FastSet.h>
    │ │ │ │ -
    27
    │ │ │ │ -
    28#include <boost/make_shared.hpp>
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │
    29#include <string>
    │ │ │ │ -
    30#include <utility>
    │ │ │ │ -
    31#include <vector>
    │ │ │ │ +
    30
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │
    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
    │ │ │ │ -
    64GTSAM_EXPORT
    │ │ │ │ -
    65std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
    │ │ │ │ -
    66EliminateForMPE(const DiscreteFactorGraph& factors,
    │ │ │ │ -
    67 const Ordering& frontalKeys);
    │ │ │ │ -
    68
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    70{
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    88 const FactorGraphType& graph,
    │ │ │ │ -
    89 boost::optional<const VariableIndex&> variableIndex) {
    │ │ │ │ -
    90 return Ordering::Colamd(*variableIndex);
    │ │ │ │ -
    91 }
    │ │ │ │ -
    │ │ │ │ -
    92};
    │ │ │ │ +
    33// Forward declarations
    │ │ │ │ +
    34class DiscreteConditional;
    │ │ │ │ +
    35class VectorValues;
    │ │ │ │ +
    36
    │ │ │ │ +
    37/* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    39class GTSAM_EXPORT DiscreteBayesTreeClique
    │ │ │ │ +
    40 : public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> {
    │ │ │ │ +
    41 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    44 Base;
    │ │ │ │ +
    45 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    46 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ + │ │ │ │ +
    48 virtual ~DiscreteBayesTreeClique() {}
    │ │ │ │ + │ │ │ │ +
    50 const boost::shared_ptr<DiscreteConditional>& conditional)
    │ │ │ │ +
    51 : Base(conditional) {}
    │ │ │ │ +
    52
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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/* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    74class GTSAM_EXPORT DiscreteBayesTree
    │ │ │ │ +
    75 : public BayesTree<DiscreteBayesTreeClique> {
    │ │ │ │ +
    76 private:
    │ │ │ │ + │ │ │ │ +
    78
    │ │ │ │ +
    79 public:
    │ │ │ │ +
    80 typedef DiscreteBayesTree This;
    │ │ │ │ +
    81 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    82
    │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    99class GTSAM_EXPORT DiscreteFactorGraph
    │ │ │ │ -
    100 : public FactorGraph<DiscreteFactor>,
    │ │ │ │ -
    101 public EliminateableFactorGraph<DiscreteFactorGraph> {
    │ │ │ │ -
    102 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    107 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    108
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    111 using Indices = KeyVector;
    │ │ │ │ -
    112
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    129
    │ │ │ │ - │ │ │ │ -
    132
    │ │ │ │ -
    135
    │ │ │ │ -
    136 bool equals(const This& fg, double tol = 1e-9) const;
    │ │ │ │ -
    137
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Variable elimination algorithms for factor graphs.
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
    Main elimination function for DiscreteFactorGraph.
    Definition DiscreteFactorGraph.cpp:200
    │ │ │ │ -
    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
    │ │ │ │ +
    112};
    │ │ │ │ +
    │ │ │ │ +
    113
    │ │ │ │ +
    114} // namespace gtsam
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of markdown.
    Definition DiscreteValues.cpp:129
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ - │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ -
    A Bayes net made from discrete conditional distributions.
    Definition DiscreteBayesNet.h:38
    │ │ │ │ +
    A clique in a DiscreteBayesTree.
    Definition DiscreteBayesTree.h:40
    │ │ │ │ +
    void printSignature(const std::string &s="Clique: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print index signature only
    Definition DiscreteBayesTree.h:54
    │ │ │ │
    A Bayes tree representing a Discrete density.
    Definition DiscreteBayesTree.h:75
    │ │ │ │ -
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ -
    Elimination tree for discrete factors.
    Definition DiscreteEliminationTree.h:33
    │ │ │ │ -
    Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
    Definition DiscreteFactor.h:38
    │ │ │ │ -
    DiscreteValues::Names Names
    Translation table from values to strings.
    Definition DiscreteFactor.h:106
    │ │ │ │ -
    DiscreteFactorGraph FactorGraphType
    Type of the factor graph (e.g. DiscreteFactorGraph)
    Definition DiscreteFactorGraph.h:72
    │ │ │ │ -
    DiscreteJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition DiscreteFactorGraph.h:77
    │ │ │ │ -
    DiscreteFactor FactorType
    Type of factors in factor graph.
    Definition DiscreteFactorGraph.h:71
    │ │ │ │ -
    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
    │ │ │ │ -
    DiscreteBayesTree BayesTreeType
    Type of Bayes tree.
    Definition DiscreteFactorGraph.h:76
    │ │ │ │ -
    DiscreteBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition DiscreteFactorGraph.h:74
    │ │ │ │ -
    DiscreteConditional ConditionalType
    Type of conditionals from elimination.
    Definition DiscreteFactorGraph.h:73
    │ │ │ │ -
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition DiscreteFactorGraph.h:87
    │ │ │ │ -
    DiscreteEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition DiscreteFactorGraph.h:75
    │ │ │ │ -
    A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
    Definition DiscreteFactorGraph.h:101
    │ │ │ │ -
    DiscreteFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition DiscreteFactorGraph.h:123
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition DiscreteFactorGraph.h:107
    │ │ │ │ -
    DiscreteFactorGraph()
    ‍map from keys to values
    Definition DiscreteFactorGraph.h:114
    │ │ │ │ -
    DiscreteFactorGraph(const FactorGraph< DERIVED_FACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition DiscreteFactorGraph.h:128
    │ │ │ │ -
    DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition DiscreteFactorGraph.h:118
    │ │ │ │ -
    virtual ~DiscreteFactorGraph()
    Destructor.
    Definition DiscreteFactorGraph.h:131
    │ │ │ │ -
    EliminateableFactorGraph< This > BaseEliminateable
    for elimination
    Definition DiscreteFactorGraph.h:106
    │ │ │ │ -
    void add(Args &&... args)
    Add a decision-tree factor.
    Definition DiscreteFactorGraph.h:142
    │ │ │ │ -
    An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition DiscreteJunctionTree.h:52
    │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    A DAG made from lookup tables, as defined above.
    Definition DiscreteLookupDAG.h:77
    │ │ │ │ +
    DiscreteBayesTree()
    Default constructor, creates an empty Bayes tree.
    Definition DiscreteBayesTree.h:86
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ -
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    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
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,352 +1,155 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteFactorGraph.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 <_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 │ │ │ │ │ +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#include │ │ │ │ │ -31#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ 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 │ │ │ │ │ -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); │ │ │ │ │ +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) {} │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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_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}; │ │ │ │ │ +_8_6 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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 = │ │ │ │ │ +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 = │ │ │ │ │ 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 │ │ │ │ │ +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_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._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. │ │ │ │ │ _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, │ │ │ │ │ +_g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ +string html(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 │ │ │ │ │ +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_:_:_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_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_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_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_:_:_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_:_:_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 │ │ │ │ │ * _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_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/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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    DiscreteBayesNet.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    DiscreteValues.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

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

    │ │ │ │ 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 15, 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 │ │ │ │ │ -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_B_a_y_e_s_N_e_t │ │ │ │ │ -  A Bayes net made from discrete conditional distributions. _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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 15, 2011 │ │ │ │ │ + January, 2022 │ │ │ │ │ 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_._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/a00233.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00233 = [ │ │ │ │ │ - ["gtsam::traits< DiscreteBayesNet >", "a02820.html", null] │ │ │ │ │ + ["html", "a00233.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ + ["markdown", "a00233.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ ]; │ │ │ ├── ./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/DiscreteConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,60 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Signature.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    DiscreteConditional.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...
    class  gtsam::DiscreteConditional
     Discrete Conditional Density Derives from DecisionTreeFactor. More...
     
    struct  gtsam::traits< DiscreteConditional >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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);.
     
    │ │ │ │

    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,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 │ │ │ │ │ -Signature.h File Reference │ │ │ │ │ -signatures for conditional densities _M_o_r_e_._._. │ │ │ │ │ +_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_:_:_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_._._. │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ -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);. │ │ │ │ │ -  │ │ │ │ │ ********** 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_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/a00236.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00236 = [ │ │ │ │ │ - ["operator%", "a00236.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ - ["operator%", "a00236.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ - ["operator|", "a00236.html#aa6c58dfb2fc9398a026e3268ba2edfad", null] │ │ │ │ │ + ["gtsam::traits< DiscreteConditional >", "a02836.html", 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/DiscreteConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,112 +98,228 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Signature.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
    │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/inference/Conditional-inst.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ -
    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_;
    │ │ │ │ +
    25#include <boost/make_shared.hpp>
    │ │ │ │ +
    26#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    27#include <string>
    │ │ │ │ +
    28#include <vector>
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    44 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    48
    │ │ │ │ + │ │ │ │ +
    50
    │ │ │ │ +
    53
    │ │ │ │ + │ │ │ │ +
    56
    │ │ │ │ +
    58 DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
    │ │ │ │ +
    59
    │ │ │ │ +
    64 DiscreteConditional(size_t nFrontals, const DiscreteKeys& keys,
    │ │ │ │ +
    65 const ADT& potentials);
    │ │ │ │
    66
    │ │ │ │ -
    68 DiscreteKeys parents_;
    │ │ │ │ +
    68 explicit DiscreteConditional(const Signature& signature);
    │ │ │ │
    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);
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    112 Signature(const DiscreteKey& key);
    │ │ │ │ +
    110 DiscreteConditional(const DecisionTreeFactor& joint,
    │ │ │ │ +
    111 const DecisionTreeFactor& marginal,
    │ │ │ │ +
    112 const Ordering& orderedKeys);
    │ │ │ │
    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);
    │ │ │ │ +
    129 DiscreteConditional operator*(const DiscreteConditional& other) const;
    │ │ │ │ +
    130
    │ │ │ │ +
    132 DiscreteConditional marginal(Key key) const;
    │ │ │ │ +
    133
    │ │ │ │
    137
    │ │ │ │ -
    139 Signature& operator=(const Table& table);
    │ │ │ │ -
    140
    │ │ │ │ -
    142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
    │ │ │ │ -
    143 const Signature& s);
    │ │ │ │ -
    144 };
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    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}
    │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ +
    149
    │ │ │ │ +
    │ │ │ │ +
    151 double logProbability(const DiscreteValues& x) const {
    │ │ │ │ +
    152 return -error(x);
    │ │ │ │ +
    153 }
    │ │ │ │ +
    │ │ │ │ +
    154
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    signatures for conditional densities
    │ │ │ │ + │ │ │ │
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    Signature operator|(const DiscreteKey &key, const DiscreteKey &parent)
    Helper function to create Signature objects example: Signature s = D | E;.
    Definition Signature.cpp:178
    │ │ │ │ -
    Signature operator%(const DiscreteKey &key, const string &parent)
    Helper function to create Signature objects example: Signature s(D % "99/1");.
    Definition Signature.cpp:183
    │ │ │ │ +
    string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of markdown.
    Definition DiscreteValues.cpp:129
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ + │ │ │ │ +
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ +
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ +
    DiscreteConditional(const DiscreteKey &key, const std::string &spec)
    No-parent specialization; can also use DiscreteDistribution.
    Definition DiscreteConditional.h:95
    │ │ │ │ +
    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
    │ │ │ │ +
    double evaluate(const DiscreteValues &values) const
    Evaluate, just look up in AlgebraicDecisonTree.
    Definition DiscreteConditional.h:163
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition DiscreteConditional.h:44
    │ │ │ │ +
    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
    │ │ │ │ +
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition DiscreteConditional.h:47
    │ │ │ │ +
    double logProbability(const HybridValues &x) const override
    < HybridValues version
    Definition DiscreteConditional.h:253
    │ │ │ │ +
    DiscreteConditional This
    Typedef to this class.
    Definition DiscreteConditional.h:43
    │ │ │ │ +
    DecisionTreeFactor BaseFactor
    Typedef to our factor base class.
    Definition DiscreteConditional.h:45
    │ │ │ │ +
    DiscreteConditional()
    Default constructor needed for serialization.
    Definition DiscreteConditional.h:55
    │ │ │ │ +
    double logProbability(const DiscreteValues &x) const
    Log-probability is just -error(x).
    Definition DiscreteConditional.h:151
    │ │ │ │ +
    void printSignature(const std::string &s="Discrete Conditional: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print index signature only
    Definition DiscreteConditional.h:156
    │ │ │ │ +
    double logNormalizationConstant() const override
    logNormalizationConstant K is just zero, such that logProbability(x) = log(evaluate(x)) = - error(x) ...
    Definition DiscreteConditional.h:262
    │ │ │ │
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │
    Signature for a discrete conditional density, used to construct conditionals.
    Definition Signature.h:54
    │ │ │ │ -
    std::vector< double > Row
    Data type for the CPT.
    Definition Signature.h:59
    │ │ │ │ -
    const DiscreteKey & key() const
    the variable key
    Definition Signature.h:115
    │ │ │ │ -
    const DiscreteKeys & parents() const
    the parent keys
    Definition Signature.h:118
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Signature.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 │ │ │ │ │ @@ -15,119 +15,272 @@ │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_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_; │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +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; │ │ │ │ │ +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 _D_i_s_c_r_e_t_e_K_e_y_s parents_; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +_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 │ │ │ │ │ -112 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key); │ │ │ │ │ +110 DiscreteConditional(const DecisionTreeFactor& joint, │ │ │ │ │ +111 const DecisionTreeFactor& marginal, │ │ │ │ │ +112 const Ordering& orderedKeys); │ │ │ │ │ 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); │ │ │ │ │ +129 DiscreteConditional operator*(const DiscreteConditional& other) const; │ │ │ │ │ +130 │ │ │ │ │ +132 DiscreteConditional marginal(Key key) const; │ │ │ │ │ +133 │ │ │ │ │ 137 │ │ │ │ │ -139 Signature& operator=(const Table& table); │ │ │ │ │ -140 │ │ │ │ │ -142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -143 const Signature& s); │ │ │ │ │ -144 }; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _S_i_g_n_a_t_u_r_e_._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/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/DiscreteConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    DiscreteKey.h File Reference
    │ │ │ │ +Variables
    │ │ │ │ +
    DiscreteConditional.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    specialized key for discrete variables │ │ │ │ -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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ -Functions

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

    │ │ │ │ +Variables

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

    Detailed Description

    │ │ │ │ -

    specialized key for discrete variables

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Feb 28, 2011
    │ │ │ │ +
    Date
    Feb 14, 2011
    │ │ │ │ +
    Author
    Duy-Nguyen Ta
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 | _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 | _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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +template class GTSAM_EXPORT  ggttssaamm::::CCoonnddiittiioonnaall<< DDeecciissiioonnTTrreeeeFFaaccttoorr,, │ │ │ │ │ + DDiissccrreetteeCCoonnddiittiioonnaall >> │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - 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_K_e_y_._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/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/DiscreteEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DiscreteBayesTree.cpp File Reference
    │ │ │ │ +
    DiscreteEliminationTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ -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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

    │ │ │ │ -

    DiscreteBayesTree

    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,26 @@ │ │ │ │ │ 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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ -DiscreteBayesTree │ │ │ │ │ + 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_B_a_y_e_s_T_r_e_e_._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_._h │ │ │ │ │ * Generated on Tue Jan 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/DiscreteLookupDAG.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DiscreteLookupDAG.h File Reference
    │ │ │ │ +
    DiscreteLookupDAG.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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

    │ │ │ │ -
    Date
    January, 2022
    │ │ │ │ -
    Author
    Frank dellaert
    │ │ │ │ +
    Date
    Feb 14, 2011
    │ │ │ │ +
    Author
    Duy-Nguyen Ta
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 │ │ │ │ │ - January, 2022 │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ - Frank dellaert │ │ │ │ │ + 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_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/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/DiscreteFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    DecisionTreeFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    DiscreteFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::DecisionTreeFactor
     A discrete probabilistic factor. More...
    class  gtsam::DiscreteFactor
     Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
     
    struct  gtsam::traits< DecisionTreeFactor >
    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
    │ │ │ │
    Author
    Duy-Nguyen Ta
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -DecisionTreeFactor.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 │ │ │ │ │ +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_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_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_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_ _> │ │ │ │ │ +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 │ │ │ │ │ +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 ********** │ │ │ │ │ Date │ │ │ │ │ Feb 14, 2011 │ │ │ │ │ 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_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/a00251.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00251 = [ │ │ │ │ │ - ["gtsam::traits< DecisionTreeFactor >", "a02812.html", null] │ │ │ │ │ + ["gtsam::traits< DiscreteFactor >", "a02856.html", null], │ │ │ │ │ + ["expNormalize", "a00251.html#ae1c1177c8725296b34601409322400ea", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00251_source.html │ │ │ │ @@ -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/discrete/DiscreteFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DecisionTreeFactor.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
    │ │ │ │ @@ -114,225 +114,127 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    50 typedef boost::shared_ptr<DecisionTreeFactor> shared_ptr;
    │ │ │ │ - │ │ │ │ -
    52
    │ │ │ │ -
    53 protected:
    │ │ │ │ -
    54 std::map<Key, size_t> cardinalities_;
    │ │ │ │ -
    55
    │ │ │ │ -
    56 public:
    │ │ │ │ -
    59
    │ │ │ │ - │ │ │ │ -
    62
    │ │ │ │ -
    64 DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    44 typedef boost::shared_ptr<DiscreteFactor> shared_ptr;
    │ │ │ │ +
    45 typedef Factor Base;
    │ │ │ │ +
    46
    │ │ │ │ + │ │ │ │ +
    48
    │ │ │ │ +
    49public:
    │ │ │ │ +
    50
    │ │ │ │ +
    53
    │ │ │ │ + │ │ │ │ +
    56
    │ │ │ │ +
    59 template<typename CONTAINER>
    │ │ │ │ +
    60 DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
    │ │ │ │ +
    61
    │ │ │ │ +
    │ │ │ │ +
    63 virtual ~DiscreteFactor() {
    │ │ │ │ +
    64 }
    │ │ │ │ +
    │ │ │ │
    65
    │ │ │ │ - │ │ │ │ -
    86 const std::vector<double>& table);
    │ │ │ │ -
    87
    │ │ │ │ -
    106 DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
    │ │ │ │ +
    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;
    │ │ │ │ +
    89
    │ │ │ │ +
    94 double error(const HybridValues& c) const override;
    │ │ │ │ +
    95
    │ │ │ │ + │ │ │ │ +
    98
    │ │ │ │ +
    99 virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
    │ │ │ │ +
    100
    │ │ │ │ +
    104
    │ │ │ │ + │ │ │ │
    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)
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    152 return apply(f, ADT::Ring::mul);
    │ │ │ │ -
    153 }
    │ │ │ │ -
    │ │ │ │ -
    154
    │ │ │ │ -
    155 static double safe_div(const double& a, const double& b);
    │ │ │ │ +
    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 size_t cardinality(Key j) const { return cardinalities_.at(j); }
    │ │ │ │ -
    158
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Algebraic Decision Trees.
    │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ - │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │ +
    157}// namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ + │ │ │ │ +
    The base class for all factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of markdown.
    Definition DiscreteValues.cpp:129
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
    free versions of apply
    Definition DecisionTree.h:413
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    std::vector< double > expNormalize(const std::vector< double > &logProbs)
    Normalize a set of log probabilities.
    Definition DiscreteFactor.cpp:42
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ -
    shared_ptr sum(const Ordering &keys) const
    Create new factor by summing all values with the same separator values.
    Definition DecisionTreeFactor.h:173
    │ │ │ │ -
    double operator()(const DiscreteValues &values) const override
    Evaluate probability density, sugar.
    Definition DecisionTreeFactor.h:143
    │ │ │ │ -
    DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override
    multiply two factors
    Definition DecisionTreeFactor.h:151
    │ │ │ │ -
    shared_ptr max(size_t nrFrontals) const
    Create new factor by maximizing over all values with the same separator.
    Definition DecisionTreeFactor.h:178
    │ │ │ │ -
    shared_ptr max(const Ordering &keys) const
    Create new factor by maximizing over all values with the same separator.
    Definition DecisionTreeFactor.h:183
    │ │ │ │ -
    double evaluate(const DiscreteValues &values) const
    Calculate probability for given values x, is just look up in AlgebraicDecisionTree.
    Definition DecisionTreeFactor.h:138
    │ │ │ │ -
    DiscreteFactor Base
    Typedef to base class.
    Definition DecisionTreeFactor.h:49
    │ │ │ │ -
    shared_ptr sum(size_t nrFrontals) const
    Create new factor by summing all values with the same separator values.
    Definition DecisionTreeFactor.h:168
    │ │ │ │ -
    DecisionTreeFactor(const DiscreteKey &key, SOURCE table)
    Single-key specialization.
    Definition DecisionTreeFactor.h:110
    │ │ │ │ -
    DecisionTreeFactor toDecisionTreeFactor() const override
    Convert into a decisiontree.
    Definition DecisionTreeFactor.h:165
    │ │ │ │ -
    DecisionTreeFactor operator/(const DecisionTreeFactor &f) const
    divide by factor f (safely)
    Definition DecisionTreeFactor.h:160
    │ │ │ │ -
    DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row)
    Single-key specialization, with vector of doubles.
    Definition DecisionTreeFactor.h:114
    │ │ │ │ -
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │
    Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
    Definition DiscreteFactor.h:38
    │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
    Render as html table.
    │ │ │ │ +
    Factor Base
    Our base class.
    Definition DiscreteFactor.h:45
    │ │ │ │ +
    void print(const std::string &s="DiscreteFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition DiscreteFactor.h:74
    │ │ │ │ +
    boost::shared_ptr< DiscreteFactor > shared_ptr
    shared_ptr to this class
    Definition DiscreteFactor.h:44
    │ │ │ │ +
    virtual ~DiscreteFactor()
    Virtual destructor.
    Definition DiscreteFactor.h:63
    │ │ │ │ +
    DiscreteValues::Names Names
    Translation table from values to strings.
    Definition DiscreteFactor.h:106
    │ │ │ │ +
    virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0
    equals
    │ │ │ │ +
    DiscreteFactor This
    This class.
    Definition DiscreteFactor.h:43
    │ │ │ │ +
    virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0
    Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor.
    │ │ │ │ +
    virtual double operator()(const DiscreteValues &) const =0
    Find value for given assignment of values to variables.
    │ │ │ │ +
    virtual std::string markdown(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
    Render as markdown table.
    │ │ │ │ +
    DiscreteFactor(const CONTAINER &keys)
    Construct from container of keys.
    Definition DiscreteFactor.h:60
    │ │ │ │ +
    DiscreteFactor()
    Default constructor creates empty factor.
    Definition DiscreteFactor.h:55
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ +
    std::map< Key, std::vector< std::string > > Names
    Translation table from values to strings.
    Definition DiscreteValues.h:95
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DecisionTreeFactor.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 │ │ │ │ │ @@ -16,288 +16,181 @@ │ │ │ │ │ 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(); │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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); │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ 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); │ │ │ │ │ +_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 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, │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_:_:_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_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_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_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.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_F_a_c_t_o_r_._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/a00254.html │ │ │ │ @@ -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/discrete/DiscreteJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,84 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions | │ │ │ │ -Variables
    │ │ │ │ -
    Signature.cpp File Reference
    │ │ │ │ +Typedefs
    │ │ │ │ +
    DiscreteJunctionTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::DiscreteJunctionTree
     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.
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ -

    │ │ │ │ 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
     
    │ │ │ │ +using gtsam::DiscreteCluster = DiscreteJunctionTree::Cluster
     typedef for wrapper:
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    signatures for conditional densities

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Feb 27, 2011
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,57 +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 | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -Signature.cpp File Reference │ │ │ │ │ -signatures 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 │ │ │ │ │ +DiscreteJunctionTree.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_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. │ │ │ │ │   │ │ │ │ │ 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} │ │ │ │ │ -  │ │ │ │ │ -struct gtsam::parser::Grammar  ggttssaamm::::ppaarrsseerr::::ggrraammmmaarr │ │ │ │ │ +using  ggttssaamm::::DDiissccrreetteeCClluusstteerr = _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ +  typedef for wrapper: │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -signatures for conditional densities │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 27, 2011 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _D_i_s_c_r_e_t_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/a00254.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00254 = [ │ │ │ │ │ - ["operator%", "a00254.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ - ["operator%", "a00254.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ - ["operator|", "a00254.html#aa6c58dfb2fc9398a026e3268ba2edfad", null] │ │ │ │ │ + ["DiscreteCluster", "a00254.html#aa8c26fa5afbbfd23795d7d421e543a81", 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/DecisionTreeFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    AlgebraicDecisionTree.cpp File Reference
    │ │ │ │ +
    DecisionTreeFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    discrete factor │ │ │ │ +More...

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

    │ │ │ │ Namespaces

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

    Detailed Description

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

    discrete factor

    │ │ │ │ +
    Date
    Feb 14, 2011
    │ │ │ │ +
    Author
    Duy-Nguyen Ta
    │ │ │ │
    │ │ │ │ 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 │ │ │ │ │ -AlgebraicDecisionTree.cpp File Reference │ │ │ │ │ +DecisionTreeFactor.cpp File Reference │ │ │ │ │ +discrete factor _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +discrete factor │ │ │ │ │ Date │ │ │ │ │ - Feb 20, 2022 │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ - Mike Sheffler │ │ │ │ │ Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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_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/a00260_source.html │ │ │ │ @@ -25,950 +25,12888 @@ │ │ │ │ 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 6973 6372 6574 6545 6c69 6d69 te/DiscreteElimi │ │ │ │ -00000200: 6e61 7469 6f6e 5472 6565 2e68 2053 6f75 nationTree.h Sou │ │ │ │ -00000210: 7263 6520 4669 6c65 3c2f 7469 746c 653e rce File │ │ │ │ -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 . │ │ │ │ +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 3236 305f 736f vTree('a00260_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.
    ..
    .
    │ │ │ │ -00000f80: 0a3c 2f64 6976 3e0a 0a3c 6469 7620 636c .
    ..
    . │ │ │ │ -00000fa0: 3c64 6976 2063 6c61 7373 3d22 6865 6164
    Disc │ │ │ │ -00000fd0: 7265 7465 456c 696d 696e 6174 696f 6e54 reteEliminationT │ │ │ │ -00000fe0: 7265 652e 683c 2f64 6976 3e3c 2f64 6976 ree.h
    .
    .
    .Go to the doc │ │ │ │ -00001040: 756d 656e 7461 7469 6f6e 206f 6620 7468 umentation of th │ │ │ │ -00001050: 6973 2066 696c 652e 3c2f 613e 3c64 6976 is file.
    1< │ │ │ │ -000010c0: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span>/* - │ │ │ │ -000010e0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -000010f0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001100: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001110: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001120: 2d2d 2d2d 2d2d 2d2d 2d2d 2d3c 2f73 7061 -----------
    .
    2
    .
    3< │ │ │ │ -00001200: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span> * G │ │ │ │ -00001220: 5453 414d 2043 6f70 7972 6967 6874 2032 TSAM Copyright 2 │ │ │ │ -00001230: 3031 302c 2047 656f 7267 6961 2054 6563 010, Georgia Tec │ │ │ │ -00001240: 6820 5265 7365 6172 6368 2043 6f72 706f h Research Corpo │ │ │ │ -00001250: 7261 7469 6f6e 2c3c 2f73 7061 6e3e 3c2f ration,.
    │ │ │ │ -000012b0: 2034 3c2f 7370 616e 3e3c 7370 616e 2063 4 │ │ │ │ -000012d0: 2a20 4174 6c61 6e74 612c 2047 656f 7267 * Atlanta, Georg │ │ │ │ -000012e0: 6961 2033 3033 3332 2d30 3431 353c 2f73 ia 30332-0415
    .
    5< │ │ │ │ -00001350: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ -00001360: 656e 7422 3e20 2a20 416c 6c20 5269 6768 ent"> * All Righ │ │ │ │ -00001370: 7473 2052 6573 6572 7665 643c 2f73 7061 ts Reserved
    .
    6 * Authors: F │ │ │ │ -00001400: 7261 6e6b 2044 656c 6c61 6572 742c 2065 rank Dellaert, e │ │ │ │ -00001410: 7420 616c 2e20 2873 6565 2054 4841 4e4b t al. (see THANK │ │ │ │ -00001420: 5320 666f 7220 7468 6520 6675 6c6c 2061 S for the full a │ │ │ │ -00001430: 7574 686f 7220 6c69 7374 293c 2f73 7061 uthor list)
    .
    7
    .
    8< │ │ │ │ -00001510: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span> * S │ │ │ │ -00001530: 6565 204c 4943 454e 5345 2066 6f72 2074 ee LICENSE for t │ │ │ │ -00001540: 6865 206c 6963 656e 7365 2069 6e66 6f72 he license infor │ │ │ │ -00001550: 6d61 7469 6f6e 3c2f 7370 616e 3e3c 2f64 mation.
    │ │ │ │ -000015b0: 393c 2f73 7061 6e3e 3c73 7061 6e20 636c 9
    .< │ │ │ │ -000015f0: 6120 6964 3d22 6c30 3030 3130 2220 6e61 a id="l00010" na │ │ │ │ -00001600: 6d65 3d22 6c30 3030 3130 223e 3c2f 613e me="l00010"> │ │ │ │ -00001610: 3c73 7061 6e20 636c 6173 733d 226c 696e 10 * ------ │ │ │ │ -00001650: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001660: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001670: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001680: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ -00001690: 2d2d 2d2d 202a 2f3c 2f73 7061 6e3e 3c2f ---- */.
    │ │ │ │ -000016f0: 3131 3c2f 7370 616e 3e20 3c2f 6469 763e 11
    │ │ │ │ -00001700: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00001730: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 19 │ │ │ │ -00001770: 2370 7261 676d 6120 6f6e 6365 3c2f 7370 #pragma once
    .
    20 < │ │ │ │ -000017e0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00001830: 2032 313c 2f73 7061 6e3e 3c73 7061 6e20 21#include &l │ │ │ │ -00001860: 743b 3c61 2063 6c61 7373 3d22 636f 6465 t;gtsam/discr │ │ │ │ -00001890: 6574 652f 4469 7363 7265 7465 4261 7965 ete/DiscreteBaye │ │ │ │ -000018a0: 734e 6574 2e68 3c2f 613e 2667 743b 3c2f sNet.h>
    .
    < │ │ │ │ -000018f0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -00001900: 6e6f 223e 2020 2032 323c 2f73 7061 6e3e no"> 22 │ │ │ │ -00001910: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ -00001930: 7564 6520 266c 743b 3c61 2063 6c61 7373 ude <gtsam │ │ │ │ -00001960: 2f64 6973 6372 6574 652f 4469 7363 7265 /discrete/Discre │ │ │ │ -00001970: 7465 4661 6374 6f72 4772 6170 682e 683c teFactorGraph.h< │ │ │ │ -00001980: 2f61 3e26 6774 3b3c 2f73 7061 6e3e 3c2f /a>>.
    │ │ │ │ -000019e0: 3233 3c2f 7370 616e 3e3c 7370 616e 2063 23#include < │ │ │ │ -00001a10: 3b3c 6120 636c 6173 733d 2263 6f64 6522 ;gtsam/infere │ │ │ │ -00001a40: 6e63 652f 456c 696d 696e 6174 696f 6e54 nce/EliminationT │ │ │ │ -00001a50: 7265 652e 683c 2f61 3e26 6774 3b3c 2f73 ree.h>
    .
    24 │ │ │ │ -00001ac0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00001b10: 2020 3235 3c2f 7370 616e 3e3c 7370 616e 25namespace gtsam {.
    2 │ │ │ │ -00001bd0: 363c 2f73 7061 6e3e 203c 2f64 6976 3e0a 6
    . │ │ │ │ -00001be0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ -00001c70: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ -00001c80: 7265 663d 2261 3032 3834 382e 6874 6d6c ref="a02848.html │ │ │ │ -00001c90: 223e 2020 2033 313c 2f61 3e3c 2f73 7061 "> 31 class │ │ │ │ -00001cc0: 3c2f 7370 616e 3e47 5453 414d 5f45 5850 GTSAM_EXP │ │ │ │ -00001cd0: 4f52 5420 3c61 2063 6c61 7373 3d22 636f ORT │ │ │ │ -00001d00: 4469 7363 7265 7465 456c 696d 696e 6174 DiscreteEliminat │ │ │ │ -00001d10: 696f 6e54 7265 653c 2f61 3e20 3a3c 2f64 ionTree :.
    3 │ │ │ │ -00001d70: 323c 2f73 7061 6e3e 2020 2020 3c73 7061 2 public │ │ │ │ -00001da0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Elim │ │ │ │ -00001dd0: 696e 6174 696f 6e54 7265 653c 2f61 3e26 inationTree& │ │ │ │ -00001de0: 6c74 3b44 6973 6372 6574 6542 6179 6573 lt;DiscreteBayes │ │ │ │ -00001df0: 4e65 742c 2044 6973 6372 6574 6546 6163 Net, DiscreteFac │ │ │ │ -00001e00: 746f 7247 7261 7068 2667 743b 3c2f 6469 torGraph>.
    33 │ │ │ │ -00001e60: 3c2f 7370 616e 3e20 207b 3c2f 6469 763e {
    │ │ │ │ -00001e70: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ -00001ea0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 34 pub │ │ │ │ -00001ee0: 6c69 633c 2f73 7061 6e3e 3a3c 2f64 6976 lic:
    .
    │ │ │ │ -00001f80: 2020 3335 3c2f 613e 3c2f 7370 616e 3e20 35 │ │ │ │ -00001f90: 2020 203c 7370 616e 2063 6c61 7373 3d22 typedef │ │ │ │ -00001fb0: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 EliminationT │ │ │ │ -00001ff0: 7265 6526 6c74 3b44 6973 6372 6574 6542 ree<DiscreteB │ │ │ │ -00002000: 6179 6573 4e65 742c 2044 6973 6372 6574 ayesNet, Discret │ │ │ │ -00002010: 6546 6163 746f 7247 7261 7068 2667 743b eFactorGraph> │ │ │ │ -00002020: 3c2f 613e 203c 6120 636c 6173 733d 2263 Base; < │ │ │ │ -00002080: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    36 typ │ │ │ │ -00002140: 6564 6566 3c2f 7370 616e 3e20 3c61 2063 edef Discrete │ │ │ │ -00002180: 456c 696d 696e 6174 696f 6e54 7265 653c EliminationTree< │ │ │ │ -00002190: 2f61 3e20 3c61 2063 6c61 7373 3d22 636f /a> This; .
    37 type │ │ │ │ -000022b0: 6465 663c 2f73 7061 6e3e 2062 6f6f 7374 def boost │ │ │ │ -000022c0: 3a3a 7368 6172 6564 5f70 7472 266c 743b ::shared_ptr< │ │ │ │ -000022d0: 5468 6973 2667 743b 203c 6120 636c 6173 This> shared_ │ │ │ │ -00002330: 7074 723c 2f61 3e3b 203c 2f64 6976 3e0a ptr;
    . │ │ │ │ -00002340: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ -000023d0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ -000023e0: 6e6f 223e 2020 2034 373c 2f73 7061 6e3e no"> 47 │ │ │ │ -000023f0: 2020 2020 3c61 2063 6c61 7373 3d22 636f │ │ │ │ -00002420: 4469 7363 7265 7465 456c 696d 696e 6174 DiscreteEliminat │ │ │ │ -00002430: 696f 6e54 7265 653c 2f61 3e28 3c73 7061 ionTree(const < │ │ │ │ -00002460: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ -00002470: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ -00002480: 3238 3634 2e68 746d 6c22 3e44 6973 6372 2864.html">Discr │ │ │ │ -00002490: 6574 6546 6163 746f 7247 7261 7068 3c2f eteFactorGraph& factorGr │ │ │ │ -000024b0: 6170 682c 3c2f 6469 763e 0a3c 6469 7620 aph,
    .
    48 │ │ │ │ -00002510: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ -00002530: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 VariableInde │ │ │ │ -00002570: 783c 2f61 3e26 616d 703b 2073 7472 7563 x& struc │ │ │ │ -00002580: 7475 7265 2c20 3c73 7061 6e20 636c 6173 ture, cons │ │ │ │ -000025a0: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t Ordering& order);.
    │ │ │ │ -00002640: 3439 3c2f 7370 616e 3e20 3c2f 6469 763e 49
    │ │ │ │ -00002650: 0a3c 6469 7620 636c 6173 733d 226c 696e .. │ │ │ │ -00002770: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    ..
    61 < │ │ │ │ -00002ac0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00002ad0: 6f72 6422 3e70 7269 7661 7465 3c2f 7370 ord">private:
    .
    62 │ │ │ │ -00002b40: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ -00002b90: 2020 3633 3c2f 7370 616e 3e20 2020 203c 63 < │ │ │ │ -00002ba0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ -00002bb0: 6f72 6422 3e66 7269 656e 643c 2f73 7061 ord">friend class ::Elimi │ │ │ │ -00002bd0: 6e61 7469 6f6e 5472 6565 5465 7374 6572 nationTreeTester │ │ │ │ -00002be0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ -00002c30: 2020 2036 343c 2f73 7061 6e3e 203c 2f64 64 .
    6 │ │ │ │ -00002c90: 353c 2f73 7061 6e3e 2020 7d3b 3c2f 6469 5 };.
    .
    66 < │ │ │ │ -00002d00: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ -00002d50: 2036 373c 2f73 7061 6e3e 7d3c 2f64 6976 67}
    .
    .
    .
    .
    │ │ │ │ -00002f10: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    Global functi │ │ │ │ -00002f30: 6f6e 7320 696e 2061 2073 6570 6172 6174 ons in a separat │ │ │ │ -00002f40: 6520 7465 7374 696e 6720 6e61 6d65 7370 e testing namesp │ │ │ │ -00002f50: 6163 652e 3c2f 6469 763e 3c64 6976 2063 ace.
    │ │ │ │ -00002f70: 4465 6669 6e69 7469 6f6e 3c2f 623e 2063 Definition c │ │ │ │ -00002f80: 6861 7274 5465 7374 696e 672e 683a 3238 hartTesting.h:28 │ │ │ │ -00002f90: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    . │ │ │ │ -00002fc0: 3c64 6976 2063 6c61 7373 3d22 7474 6e61
    Template to │ │ │ │ -00003020: 6372 6561 7465 2061 2062 696e 6172 7920 create a binary │ │ │ │ -00003030: 7072 6564 6963 6174 652e 3c2f 6469 763e predicate.
    │ │ │ │ -00003040: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    Definition │ │ │ │ -00003060: 3c2f 623e 2054 6573 7461 626c 652e 683a Testable.h: │ │ │ │ -00003070: 3131 313c 2f64 6976 3e3c 2f64 6976 3e0a 111
    . │ │ │ │ -00003080: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    gts │ │ │ │ -000030d0: 616d 3a3a 4469 7363 7265 7465 456c 696d am::DiscreteElim │ │ │ │ -000030e0: 696e 6174 696f 6e54 7265 653c 2f61 3e3c inationTree< │ │ │ │ -000030f0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Eliminat │ │ │ │ -00003110: 696f 6e20 7472 6565 2066 6f72 2064 6973 ion tree for dis │ │ │ │ -00003120: 6372 6574 6520 6661 6374 6f72 732e 3c2f crete factors.
    Defini │ │ │ │ -00003150: 7469 6f6e 3c2f 623e 2044 6973 6372 6574 tion Discret │ │ │ │ -00003160: 6545 6c69 6d69 6e61 7469 6f6e 5472 6565 eEliminationTree │ │ │ │ -00003170: 2e68 3a33 333c 2f64 6976 3e3c 2f64 6976 .h:33
    .
    DiscreteEli │ │ │ │ -00003260: 6d69 6e61 7469 6f6e 5472 6565 2054 6869 minationTree Thi │ │ │ │ -00003270: 733c 2f64 6976 3e3c 6469 7620 636c 6173 s
    This c │ │ │ │ -00003290: 6c61 7373 2e3c 2f64 6976 3e3c 6469 7620 lass.
    Definition │ │ │ │ -000032c0: 4469 7363 7265 7465 456c 696d 696e 6174 DiscreteEliminat │ │ │ │ -000032d0: 696f 6e54 7265 652e 683a 3336 3c2f 6469 ionTree.h:36
    .boost::shared_ │ │ │ │ -000033d0: 7074 7226 6c74 3b20 5468 6973 2026 6774 ptr< This > │ │ │ │ -000033e0: 3b20 7368 6172 6564 5f70 7472 3c2f 6469 ; shared_ptr
    Shared poin │ │ │ │ -00003410: 7465 7220 746f 2074 6869 7320 636c 6173 ter to this clas │ │ │ │ -00003420: 732e 3c2f 6469 763e 3c64 6976 2063 6c61 s.
    De │ │ │ │ -00003440: 6669 6e69 7469 6f6e 3c2f 623e 2044 6973 finition Dis │ │ │ │ -00003450: 6372 6574 6545 6c69 6d69 6e61 7469 6f6e creteElimination │ │ │ │ -00003460: 5472 6565 2e68 3a33 373c 2f64 6976 3e3c Tree.h:37
    < │ │ │ │ -00003470: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    < │ │ │ │ -000034d0: 6120 6872 6566 3d22 6130 3238 3438 2e68 a href="a02848.h │ │ │ │ -000034e0: 746d 6c23 6165 3465 3661 6663 3335 3335 tml#ae4e6afc3535 │ │ │ │ -000034f0: 3832 3138 3366 3363 3838 3161 3361 6361 82183f3c881a3aca │ │ │ │ -00003500: 6435 3763 6122 3e67 7473 616d 3a3a 4469 d57ca">gtsam::Di │ │ │ │ -00003510: 7363 7265 7465 456c 696d 696e 6174 696f screteEliminatio │ │ │ │ -00003520: 6e54 7265 653a 3a42 6173 653c 2f61 3e3c nTree::Base< │ │ │ │ -00003530: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Elimina │ │ │ │ -00003550: 7469 6f6e 5472 6565 266c 743b 2044 6973 tionTree< Dis │ │ │ │ -00003560: 6372 6574 6542 6179 6573 4e65 742c 2044 creteBayesNet, D │ │ │ │ -00003570: 6973 6372 6574 6546 6163 746f 7247 7261 iscreteFactorGra │ │ │ │ -00003580: 7068 2026 6774 3b20 4261 7365 3c2f 6469 ph > Base
    Base class. │ │ │ │ -000035b0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ -000035d0: 6e69 7469 6f6e 3c2f 623e 2044 6973 6372 nition Discr │ │ │ │ -000035e0: 6574 6545 6c69 6d69 6e61 7469 6f6e 5472 eteEliminationTr │ │ │ │ -000035f0: 6565 2e68 3a33 353c 2f64 6976 3e3c 2f64 ee.h:35
    .
    gtsam::Discrete │ │ │ │ -00003660: 4661 6374 6f72 4772 6170 683c 2f61 3e3c FactorGraph< │ │ │ │ -00003670: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    A Discre │ │ │ │ -00003690: 7465 2046 6163 746f 7220 4772 6170 6820 te Factor Graph │ │ │ │ -000036a0: 6973 2061 2066 6163 746f 7220 6772 6170 is a factor grap │ │ │ │ -000036b0: 6820 7768 6572 6520 616c 6c20 6661 6374 h where all fact │ │ │ │ -000036c0: 6f72 7320 6172 6520 4469 7363 7265 7465 ors are Discrete │ │ │ │ -000036d0: 2c20 692e 652e 3c2f 6469 763e 3c64 6976 , i.e.
    < │ │ │ │ -000036f0: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -00003700: 2044 6973 6372 6574 6546 6163 746f 7247 DiscreteFactorG │ │ │ │ -00003710: 7261 7068 2e68 3a31 3031 3c2f 6469 763e raph.h:101
    │ │ │ │ -00003720: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    gtsam::Elimi │ │ │ │ -00003780: 6e61 7469 6f6e 5472 6565 3c2f 613e 3c2f nationTree
    An elimin │ │ │ │ -000037b0: 6174 696f 6e20 7472 6565 2069 7320 6120 ation tree is a │ │ │ │ -000037c0: 6461 7461 2073 7472 7563 7475 7265 2075 data structure u │ │ │ │ -000037d0: 7365 6420 696e 7465 726d 6564 6961 7465 sed intermediate │ │ │ │ -000037e0: 6c79 2064 7572 696e 6720 656c 696d 696e ly during elimin │ │ │ │ -000037f0: 6174 696f 6e2e 3c2f 6469 763e 3c64 6976 ation.
    < │ │ │ │ -00003810: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ -00003820: 2045 6c69 6d69 6e61 7469 6f6e 5472 6565 EliminationTree │ │ │ │ -00003830: 2e68 3a35 323c 2f64 6976 3e3c 2f64 6976 .h:52
    ..
    gtsam::Varia │ │ │ │ -00003940: 626c 6549 6e64 6578 3c2f 613e 3c2f 6469 bleIndex
    The Variabl │ │ │ │ -00003970: 6549 6e64 6578 2063 6c61 7373 2063 6f6d eIndex class com │ │ │ │ -00003980: 7075 7465 7320 616e 6420 7374 6f72 6573 putes and stores │ │ │ │ -00003990: 2074 6865 2062 6c6f 636b 2063 6f6c 756d the block colum │ │ │ │ -000039a0: 6e20 7374 7275 6374 7572 6520 6f66 2061 n structure of a │ │ │ │ -000039b0: 2066 6163 746f 7220 6772 6170 682e 3c2f factor graph.
    Defini │ │ │ │ -000039e0: 7469 6f6e 3c2f 623e 2056 6172 6961 626c tion Variabl │ │ │ │ -000039f0: 6549 6e64 6578 2e68 3a34 333c 2f64 6976 eIndex.h:43
    .
    < │ │ │ │ -00003a20: 2f64 6976 3e3c 212d 2d20 636f 6e74 656e /div>.
    .. │ │ │ │ -00003a90: 3c21 2d2d 2069 6420 6973 206e 6565 6465 . < │ │ │ │ -00003ac0: 756c 3e0a 2020 2020 3c6c 6920 636c 6173 ul>.
  • gtsam │ │ │ │ -00003b20: 3c2f 613e 3c2f 6c69 3e3c 6c69 2063 6c61
  • disc │ │ │ │ -00003b80: 7265 7465 3c2f 613e 3c2f 6c69 3e3c 6c69 rete
  • Gene │ │ │ │ -00003c00: 7261 7465 6420 6f6e 2054 7565 204a 616e rated on Tue Jan │ │ │ │ -00003c10: 2039 2032 3032 3420 3035 3a35 313a 3335 9 2024 05:51:35 │ │ │ │ -00003c20: 2066 6f72 2067 7473 616d 2062 7920 3c61 for gtsam by do
│ │ │ │ -00003ca0: 7879 6765 6e22 2f3e 3c2f 613e 2031 2e39  xygen 1.9 │ │ │ │ -00003cb0: 2e38 203c 2f6c 693e 0a20 203c 2f75 6c3e .8
  • . │ │ │ │ -00003cc0: 0a3c 2f64 6976 3e0a 3c2f 626f 6479 3e0a .
    .. │ │ │ │ -00003cd0: 3c2f 6874 6d6c 3e0a . │ │ │ │ +00000f80: 0a0a 3c64 6976 2063 6c61 7373 3d22 6865 ..
    .
    DecisionTre │ │ │ │ +00000fd0: 652d 696e 6c2e 683c 2f64 6976 3e3c 2f64 e-inl.h
    .
    .
    .< │ │ │ │ +00001010: 6469 7620 636c 6173 733d 2266 7261 676d div class="fragm │ │ │ │ +00001020: 656e 7422 3e3c 6469 7620 636c 6173 733d ent">
    │ │ │ │ +00001070: 2031 3c2f 7370 616e 3e3c 7370 616e 2063 1/ │ │ │ │ +00001090: 2a20 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d * -------------- │ │ │ │ +000010a0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +000010b0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +000010c0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +000010d0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 3c2f --------------
    .
    < │ │ │ │ +00001120: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001130: 6e6f 223e 2020 2020 323c 2f73 7061 6e3e no"> 2 │ │ │ │ +00001140: 3c73 7061 6e20 636c 6173 733d 2263 6f6d .
    │ │ │ │ +000011b0: 2033 3c2f 7370 616e 3e3c 7370 616e 2063 3 │ │ │ │ +000011d0: 2a20 4754 5341 4d20 436f 7079 7269 6768 * GTSAM Copyrigh │ │ │ │ +000011e0: 7420 3230 3130 2c20 4765 6f72 6769 6120 t 2010, Georgia │ │ │ │ +000011f0: 5465 6368 2052 6573 6561 7263 6820 436f Tech Research Co │ │ │ │ +00001200: 7270 6f72 6174 696f 6e2c 3c2f 7370 616e rporation,
    .
    │ │ │ │ +00001260: 2020 2020 343c 2f73 7061 6e3e 3c73 7061 4 * Atlanta, Ge │ │ │ │ +00001290: 6f72 6769 6120 3330 3333 322d 3034 3135 orgia 30332-0415 │ │ │ │ +000012a0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +000012c0: 3c61 2069 643d 226c 3030 3030 3522 206e 5 * All R │ │ │ │ +00001320: 6967 6874 7320 5265 7365 7276 6564 3c2f ights Reserved
    .
    < │ │ │ │ +00001370: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001380: 6e6f 223e 2020 2020 363c 2f73 7061 6e3e no"> 6 │ │ │ │ +00001390: 3c73 7061 6e20 636c 6173 733d 2263 6f6d * Authors │ │ │ │ +000013b0: 3a20 4672 616e 6b20 4465 6c6c 6165 7274 : Frank Dellaert │ │ │ │ +000013c0: 2c20 6574 2061 6c2e 2028 7365 6520 5448 , et al. (see TH │ │ │ │ +000013d0: 414e 4b53 2066 6f72 2074 6865 2066 756c ANKS for the ful │ │ │ │ +000013e0: 6c20 6175 7468 6f72 206c 6973 7429 3c2f l author list)
    .
    < │ │ │ │ +00001430: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001440: 6e6f 223e 2020 2020 373c 2f73 7061 6e3e no"> 7 │ │ │ │ +00001450: 3c73 7061 6e20 636c 6173 733d 2263 6f6d .
    │ │ │ │ +000014c0: 2038 3c2f 7370 616e 3e3c 7370 616e 2063 8 │ │ │ │ +000014e0: 2a20 5365 6520 4c49 4345 4e53 4520 666f * See LICENSE fo │ │ │ │ +000014f0: 7220 7468 6520 6c69 6365 6e73 6520 696e r the license in │ │ │ │ +00001500: 666f 726d 6174 696f 6e3c 2f73 7061 6e3e formation │ │ │ │ +00001510: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00001560: 2020 2039 3c2f 7370 616e 3e3c 7370 616e 9
    . │ │ │ │ +00001590: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    │ │ │ │ +000016a0: 2020 2031 313c 2f73 7061 6e3e 203c 2f64 11 .
    2 │ │ │ │ +00001700: 303c 2f73 7061 6e3e 3c73 7061 6e20 636c 0#pragma once< │ │ │ │ +00001730: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00001750: 6120 6964 3d22 6c30 3030 3231 2220 6e61 a id="l00021" na │ │ │ │ +00001760: 6d65 3d22 6c30 3030 3231 223e 3c2f 613e me="l00021"> │ │ │ │ +00001770: 3c73 7061 6e20 636c 6173 733d 226c 696e 21
    .
    .
    23 │ │ │ │ +000018c0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00001910: 2020 3234 3c2f 7370 616e 3e3c 7370 616e 24#include & │ │ │ │ +00001940: 6c74 3b61 6c67 6f72 6974 686d 2667 743b lt;algorithm> │ │ │ │ +00001950: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00001970: 3c61 2069 643d 226c 3030 3032 3522 206e 25#in │ │ │ │ +000019d0: 636c 7564 6520 266c 743b 626f 6f73 742f clude <boost/ │ │ │ │ +000019e0: 666f 726d 6174 2e68 7070 2667 743b 3c2f format.hpp>
    .
    < │ │ │ │ +00001a30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001a40: 6e6f 223e 2020 2032 363c 2f73 7061 6e3e no"> 26 │ │ │ │ +00001a50: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ +00001a70: 7564 6520 266c 743b 626f 6f73 742f 6d61 ude <boost/ma │ │ │ │ +00001a80: 6b65 5f73 6861 7265 642e 6870 7026 6774 ke_shared.hpp> │ │ │ │ +00001a90: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
    .< │ │ │ │ +00001aa0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00001ab0: 3e3c 6120 6964 3d22 6c30 3030 3237 2220 > 27#i │ │ │ │ +00001b10: 6e63 6c75 6465 2026 6c74 3b62 6f6f 7374 nclude <boost │ │ │ │ +00001b20: 2f6f 7074 696f 6e61 6c2e 6870 7026 6774 /optional.hpp> │ │ │ │ +00001b30: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
    .< │ │ │ │ +00001b40: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00001b50: 3e3c 6120 6964 3d22 6c30 3030 3238 2220 > 28
    .
    29< │ │ │ │ +00001bf0: 7370 616e 2063 6c61 7373 3d22 7072 6570 span class="prep │ │ │ │ +00001c00: 726f 6365 7373 6f72 223e 2369 6e63 6c75 rocessor">#inclu │ │ │ │ +00001c10: 6465 2026 6c74 3b63 6d61 7468 2667 743b de <cmath> │ │ │ │ +00001c20: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00001c40: 3c61 2069 643d 226c 3030 3033 3022 206e 30#in │ │ │ │ +00001ca0: 636c 7564 6520 266c 743b 6673 7472 6561 clude <fstrea │ │ │ │ +00001cb0: 6d26 6774 3b3c 2f73 7061 6e3e 3c2f 6469 m>.
    31 │ │ │ │ +00001d10: 3c2f 7370 616e 3e3c 7370 616e 2063 6c61 #include <l │ │ │ │ +00001d40: 6973 7426 6774 3b3c 2f73 7061 6e3e 3c2f ist>.
    │ │ │ │ +00001da0: 3332 3c2f 7370 616e 3e3c 7370 616e 2063 32#include < │ │ │ │ +00001dd0: 3b6d 6170 2667 743b 3c2f 7370 616e 3e3c ;map>< │ │ │ │ +00001de0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00001e30: 2033 333c 2f73 7061 6e3e 3c73 7061 6e20 33#include &l │ │ │ │ +00001e60: 743b 7365 7426 6774 3b3c 2f73 7061 6e3e t;set> │ │ │ │ +00001e70: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00001ec0: 2020 3334 3c2f 7370 616e 3e3c 7370 616e 34#include & │ │ │ │ +00001ef0: 6c74 3b73 7374 7265 616d 2667 743b 3c2f lt;sstream>
    .
    < │ │ │ │ +00001f40: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00001f50: 6e6f 223e 2020 2033 353c 2f73 7061 6e3e no"> 35 │ │ │ │ +00001f60: 3c73 7061 6e20 636c 6173 733d 2270 7265 #incl │ │ │ │ +00001f80: 7564 6520 266c 743b 7374 7269 6e67 2667 ude <string&g │ │ │ │ +00001f90: 743b 3c2f 7370 616e 3e3c 2f64 6976 3e0a t;
    . │ │ │ │ +00001fa0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00001fd0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 36# │ │ │ │ +00002010: 696e 636c 7564 6520 266c 743b 7665 6374 include <vect │ │ │ │ +00002020: 6f72 2667 743b 3c2f 7370 616e 3e3c 2f64 or>.
    3 │ │ │ │ +00002080: 373c 2f73 7061 6e3e 203c 2f64 6976 3e0a 7
    . │ │ │ │ +00002090: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .< │ │ │ │ +00002160: 6120 6964 3d22 6c30 3030 3339 2220 6e61 a id="l00039" na │ │ │ │ +00002170: 6d65 3d22 6c30 3030 3339 223e 3c2f 613e me="l00039"> │ │ │ │ +00002180: 3c73 7061 6e20 636c 6173 733d 226c 696e 39
    .
    40 /********** │ │ │ │ +00002220: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002230: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002240: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002260: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
    │ │ │ │ +00002270: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000022a0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 41 // │ │ │ │ +000022e0: 4e6f 6465 3c2f 7370 616e 3e3c 2f64 6976 Node
    .
    42< │ │ │ │ +00002340: 2f73 7061 6e3e 3c73 7061 6e20 636c 6173 /span> /* │ │ │ │ +00002360: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002370: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002380: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002390: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000023a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f 7370 ***********/
    .
    43#ifdef │ │ │ │ +00002430: 4454 5f44 4542 5547 5f4d 454d 4f52 593c DT_DEBUG_MEMORY< │ │ │ │ +00002440: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00002460: 6120 6964 3d22 6c30 3030 3434 2220 6e61 a id="l00044" na │ │ │ │ +00002470: 6d65 3d22 6c30 3030 3434 223e 3c2f 613e me="l00044"> │ │ │ │ +00002480: 3c73 7061 6e20 636c 6173 733d 226c 696e 44 templat │ │ │ │ +000024c0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ +00002510: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ +00002520: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00002570: 2020 3435 3c2f 7370 616e 3e20 203c 7370 45 int DecisionTree&l │ │ │ │ +000025b0: 743b 4c2c 2059 2667 743b 3a3a 4e6f 6465 t;L, Y>::Node │ │ │ │ +000025c0: 3a3a 6e72 4e6f 6465 7320 3d20 303b 3c2f ::nrNodes = 0;.
    │ │ │ │ +00002620: 3436 3c2f 7370 616e 3e3c 7370 616e 2063 46#endif
    .
    │ │ │ │ +000026a0: 2020 2034 373c 2f73 7061 6e3e 203c 2f64 47 .
    4 │ │ │ │ +00002700: 383c 2f73 7061 6e3e 3c73 7061 6e20 636c 8 │ │ │ │ +00002720: 2f2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /*************** │ │ │ │ +00002730: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002740: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002750: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002760: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f *************/
    .
    < │ │ │ │ +000027b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000027c0: 6e6f 223e 2020 2034 393c 2f73 7061 6e3e no"> 49 │ │ │ │ +000027d0: 2020 3c73 7061 6e20 636c 6173 733d 2263 // Leaf< │ │ │ │ +000027f0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00002810: 6120 6964 3d22 6c30 3030 3530 2220 6e61 a id="l00050" na │ │ │ │ +00002820: 6d65 3d22 6c30 3030 3530 223e 3c2f 613e me="l00050"> │ │ │ │ +00002830: 3c73 7061 6e20 636c 6173 733d 226c 696e 50 /****** │ │ │ │ +00002870: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002880: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00002890: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000028a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000028b0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    │ │ │ │ +00002910: 3531 3c2f 7370 616e 3e20 203c 7370 616e 51 template │ │ │ │ +00002940: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ +00002960: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ +000029a0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ +00002a30: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00002a40: 7265 663d 2261 3032 3737 362e 6874 6d6c ref="a02776.html │ │ │ │ +00002a50: 223e 2020 2035 323c 2f61 3e3c 2f73 7061 "> 52 struct │ │ │ │ +00002a80: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 DecisionTree │ │ │ │ +00002ac0: 3c2f 613e 266c 743b 4c2c 2059 2667 743b <L, Y> │ │ │ │ +00002ad0: 3a3a 3c61 2063 6c61 7373 3d22 636f 6465 ::L │ │ │ │ +00002b00: 6561 663c 2f61 3e20 3a20 3c73 7061 6e20 eaf : │ │ │ │ +00002b20: 7075 626c 6963 3c2f 7370 616e 3e20 3c61 public Decisi │ │ │ │ +00002b60: 6f6e 5472 6565 3c2f 613e 266c 743b 4c2c onTree<L, │ │ │ │ +00002b70: 2059 2667 743b 3a3a 4e6f 6465 207b 3c2f Y>::Node {.. │ │ │ │ +00002ca0: 3c61 2069 643d 226c 3030 3035 3522 206e 55
    .
    59 │ │ │ │ +00002d80: 3c2f 7370 616e 3e20 2020 203c 7370 616e size_t nrAssignment │ │ │ │ +00002e10: 735f 3c2f 613e 3b3c 2f64 6976 3e0a 3c64 s_;
    . │ │ │ │ +00002e30: 3c61 2069 643d 226c 3030 3036 3022 206e 60
    .
    62 │ │ │ │ +00002f10: 3c2f 7370 616e 3e20 2020 203c 6120 636c Leaf │ │ │ │ +00002f70: 3c2f 613e 2829 207b 7d3c 2f64 6976 3e0a () {}
    . │ │ │ │ +00002f80: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    .
    │ │ │ │ +000030b0: 2020 3635 3c2f 613e 3c2f 7370 616e 3e20 65 │ │ │ │ +000030c0: 2020 203c 6120 636c 6173 733d 2263 6f64 Leaf(const │ │ │ │ +00003140: 5926 616d 703b 2063 6f6e 7374 616e 742c Y& constant, │ │ │ │ +00003150: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ +00003170: 743c 2f73 7061 6e3e 206e 7241 7373 6967 t nrAssig │ │ │ │ +00003180: 6e6d 656e 7473 203d 2031 293c 2f64 6976 nments = 1)
    .
    66< │ │ │ │ +000031e0: 2f73 7061 6e3e 2020 2020 2020 2020 3a20 /span> : │ │ │ │ +000031f0: 636f 6e73 7461 6e74 5f28 636f 6e73 7461 constant_(consta │ │ │ │ +00003200: 6e74 292c 206e 7241 7373 6967 6e6d 656e nt), nrAssignmen │ │ │ │ +00003210: 7473 5f28 6e72 4173 7369 676e 6d65 6e74 ts_(nrAssignment │ │ │ │ +00003220: 7329 207b 7d3c 2f64 6976 3e0a 3c2f 6469 s) {}
    ..
    67 │ │ │ │ +00003280: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +00003290: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ +000032a0: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ +000032b0: 6e30 3030 3639 2220 6461 7461 2d73 7461 n00069" data-sta │ │ │ │ +000032c0: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ +000032d0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    69 cons │ │ │ │ +00003390: 743c 2f73 7061 6e3e 2059 2661 6d70 3b20 t Y& │ │ │ │ +000033a0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 constant()< │ │ │ │ +00003400: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00003410: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    70 │ │ │ │ +00003480: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +000034a0: 6574 7572 6e3c 2f73 7061 6e3e 2063 6f6e eturn con │ │ │ │ +000034b0: 7374 616e 745f 3b3c 2f64 6976 3e0a 3c64 stant_;
    . │ │ │ │ +000034d0: 3c61 2069 643d 226c 3030 3037 3122 206e 71 }
    ..
    │ │ │ │ +00003570: 3732 3c2f 7370 616e 3e20 3c2f 6469 763e 72
    │ │ │ │ +00003580: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000035b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ +00003610: 2037 343c 2f61 3e3c 2f73 7061 6e3e 2020 74 │ │ │ │ +00003620: 2020 3c73 7061 6e20 636c 6173 733d 226b size │ │ │ │ +00003640: 5f74 3c2f 7370 616e 3e20 3c61 2063 6c61 _t nrAss │ │ │ │ +000036a0: 6967 6e6d 656e 7473 3c2f 613e 2829 3c73 ignments() const { re │ │ │ │ +000036f0: 7475 726e 3c2f 7370 616e 3e20 6e72 4173 turn nrAs │ │ │ │ +00003700: 7369 676e 6d65 6e74 735f 3b20 7d3c 2f64 signments_; }.
    7 │ │ │ │ +00003760: 353c 2f73 7061 6e3e 203c 2f64 6976 3e0a 5
    . │ │ │ │ +00003770: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    . │ │ │ │ +00003ae0: 3c61 2069 643d 226c 3030 3038 3022 206e 80
    .
    . │ │ │ │ +00003b70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00003ba0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> │ │ │ │ +00003c00: 3832 3c2f 613e 3c2f 7370 616e 3e20 2020 82 │ │ │ │ +00003c10: 203c 7370 616e 2063 6c61 7373 3d22 6b65 bool< │ │ │ │ +00003c30: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> sameLeaf │ │ │ │ +00003c90: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ +00003cb0: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Node&am │ │ │ │ +00003cf0: 703b 2071 293c 7370 616e 2063 6c61 7373 p; q) cons │ │ │ │ +00003d10: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
    .
    83 │ │ │ │ +00003d80: 2020 2020 3c73 7061 6e20 636c 6173 733d re │ │ │ │ +00003da0: 7475 726e 3c2f 7370 616e 3e20 2871 2e69 turn (q.i │ │ │ │ +00003db0: 734c 6561 6628 2920 2661 6d70 3b26 616d sLeaf() &&am │ │ │ │ +00003dc0: 703b 2071 2e73 616d 654c 6561 6628 2a3c p; q.sameLeaf(*< │ │ │ │ +00003dd0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00003de0: 6f72 6422 3e74 6869 733c 2f73 7061 6e3e ord">this │ │ │ │ +00003df0: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
    .
    84 │ │ │ │ +00003e50: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +00003e60: 0a3c 6469 7620 636c 6173 733d 226c 696e ...
    │ │ │ │ +00003f90: 2020 2038 373c 2f61 3e3c 2f73 7061 6e3e 87 │ │ │ │ +00003fa0: 2020 2020 3c73 7061 6e20 636c 6173 733d bo │ │ │ │ +00003fc0: 6f6c 3c2f 7370 616e 3e20 3c61 2063 6c61 ol equal │ │ │ │ +00004020: 733c 2f61 3e28 3c73 7061 6e20 636c 6173 s(cons │ │ │ │ +00004040: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t Node&a │ │ │ │ +00004080: 6d70 3b20 712c 203c 7370 616e 2063 6c61 mp; q, con │ │ │ │ +000040a0: 7374 3c2f 7370 616e 3e20 436f 6d70 6172 st Compar │ │ │ │ +000040b0: 6546 756e 6326 616d 703b 2063 6f6d 7061 eFunc& compa │ │ │ │ +000040c0: 7265 293c 7370 616e 2063 6c61 7373 3d22 re) const │ │ │ │ +000040e0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +000040f0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +00004140: 2020 2038 383c 2f73 7061 6e3e 2020 2020 88 │ │ │ │ +00004150: 2020 3c73 7061 6e20 636c 6173 733d 226b const Leaf* othe │ │ │ │ +000041b0: 7220 3d20 3c73 7061 6e20 636c 6173 733d r = dynami │ │ │ │ +000041d0: 635f 6361 7374 266c 743b 3c2f 7370 616e c_cast<const Leaf* │ │ │ │ +00004250: 2667 743b 3c2f 7370 616e 3e28 2661 6d70 >(& │ │ │ │ +00004260: 3b71 293b 3c2f 6469 763e 0a3c 6469 7620 ;q);
    .
    89 │ │ │ │ +000042c0: 2020 2020 203c 7370 616e 2063 6c61 7373 i │ │ │ │ +000042e0: 663c 2f73 7061 6e3e 2028 216f 7468 6572 f (!other │ │ │ │ +000042f0: 2920 3c73 7061 6e20 636c 6173 733d 226b ) retu │ │ │ │ +00004310: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ +00004330: 6661 6c73 653c 2f73 7061 6e3e 3b3c 2f64 false;.
    9 │ │ │ │ +00004390: 303c 2f73 7061 6e3e 2020 2020 2020 3c73 0 return compare(th │ │ │ │ +000043d0: 6973 2d26 6774 3b63 6f6e 7374 616e 745f is->constant_ │ │ │ │ +000043e0: 2c20 6f74 6865 722d 2667 743b 3c61 2063 , other->con │ │ │ │ +00004440: 7374 616e 745f 3c2f 613e 293b 3c2f 6469 stant_);.
    91 │ │ │ │ +000044a0: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
    .
    92 < │ │ │ │ +00004510: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ +00004590: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000045a0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 94 void print(const │ │ │ │ +000046a0: 2073 7464 3a3a 7374 7269 6e67 2661 6d70 std::string& │ │ │ │ +000046b0: 3b20 732c 203c 7370 616e 2063 6c61 7373 ; s, const │ │ │ │ +000046d0: 3c2f 7370 616e 3e20 4c61 6265 6c46 6f72 LabelFor │ │ │ │ +000046e0: 6d61 7474 6572 2661 6d70 3b20 6c61 6265 matter& labe │ │ │ │ +000046f0: 6c46 6f72 6d61 7474 6572 2c3c 2f64 6976 lFormatter,
    .
    95< │ │ │ │ +00004750: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +00004760: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ +00004780: 3c2f 7370 616e 3e20 5661 6c75 6546 6f72 ValueFor │ │ │ │ +00004790: 6d61 7474 6572 2661 6d70 3b20 7661 6c75 matter& valu │ │ │ │ +000047a0: 6546 6f72 6d61 7474 6572 293c 7370 616e eFormatter) const override │ │ │ │ +000047d0: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ +000047e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    97 │ │ │ │ +00004920: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
    .
    .
    98< │ │ │ │ +00004980: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    ...
    │ │ │ │ +00004ba0: 2031 3031 3c2f 7370 616e 3e20 2020 2020 101 │ │ │ │ +00004bb0: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ +00004bd0: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ +00004be0: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ +00004bf0: 616c 7565 466f 726d 6174 7465 722c 3c2f alueFormatter,.
    1 │ │ │ │ +00004c50: 3032 3c2f 7370 616e 3e20 2020 2020 2020 02 │ │ │ │ +00004c60: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ +00004c80: 626f 6f6c 3c2f 7370 616e 3e20 7368 6f77 bool show │ │ │ │ +00004c90: 5a65 726f 293c 7370 616e 2063 6c61 7373 Zero) cons │ │ │ │ +00004cb0: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
    .
    103 │ │ │ │ +00004d20: 2020 2020 7374 643a 3a73 7472 696e 6720 std::string │ │ │ │ +00004d30: 7661 6c75 6520 3d20 7661 6c75 6546 6f72 value = valueFor │ │ │ │ +00004d40: 6d61 7474 6572 2863 6f6e 7374 616e 745f matter(constant_ │ │ │ │ +00004d50: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    104 │ │ │ │ +00004db0: 2020 203c 7370 616e 2063 6c61 7373 3d22 if< │ │ │ │ +00004dd0: 2f73 7061 6e3e 2028 7368 6f77 5a65 726f /span> (showZero │ │ │ │ +00004de0: 207c 7c20 7661 6c75 652e 636f 6d70 6172 || value.compar │ │ │ │ +00004df0: 6528 3c73 7061 6e20 636c 6173 733d 2273 e(&q │ │ │ │ +00004e10: 756f 743b 3026 7175 6f74 3b3c 2f73 7061 uot;0"))
    .
    105 │ │ │ │ +00004e80: 2020 2020 2020 206f 7320 266c 743b 266c os <&l │ │ │ │ +00004e90: 743b 203c 7370 616e 2063 6c61 7373 3d22 t; & │ │ │ │ +00004eb0: 7175 6f74 3b5c 2671 756f 743b 2671 756f quot;\"&quo │ │ │ │ +00004ec0: 743b 3c2f 7370 616e 3e20 266c 743b 266c t; <&l │ │ │ │ +00004ed0: 743b 2074 6869 732d 2667 743b 6964 2829 t; this->id() │ │ │ │ +00004ee0: 2026 6c74 3b26 6c74 3b20 3c73 7061 6e20 << "\&qu │ │ │ │ +00004f10: 6f74 3b20 5b6c 6162 656c 3d5c 2671 756f ot; [label=\&quo │ │ │ │ +00004f20: 743b 2671 756f 743b 3c2f 7370 616e 3e20 t;" │ │ │ │ +00004f30: 266c 743b 266c 743b 2076 616c 7565 3c2f << value.
    1 │ │ │ │ +00004f90: 3036 3c2f 7370 616e 3e20 2020 2020 2020 06 │ │ │ │ +00004fa0: 2020 2020 266c 743b 266c 743b 203c 7370 << "\ │ │ │ │ +00004fd0: 2671 756f 743b 2c20 7368 6170 653d 626f ", shape=bo │ │ │ │ +00004fe0: 782c 2072 616e 6b3d 7369 6e6b 2c20 6865 x, rank=sink, he │ │ │ │ +00004ff0: 6967 6874 3d30 2e33 352c 2066 6978 6564 ight=0.35, fixed │ │ │ │ +00005000: 7369 7a65 3d74 7275 655d 5c6e 2671 756f size=true]\n&quo │ │ │ │ +00005010: 743b 3c2f 7370 616e 3e3b 3c2f 6469 763e t;;
    │ │ │ │ +00005020: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ +00005080: 0a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 .
    .
    │ │ │ │ +000050d0: 2020 3130 383c 2f73 7061 6e3e 203c 2f64 108 .
    .
    110 │ │ │ │ +000051c0: 3c2f 7370 616e 3e20 2020 203c 7370 616e const Y& │ │ │ │ +000051f0: 616d 703b 203c 6120 636c 6173 733d 2263 amp; operator() │ │ │ │ +00005250: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ +00005270: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Assignment&l │ │ │ │ +000052b0: 743b 4c26 6774 3b3c 2f61 3e26 616d 703b t;L>& │ │ │ │ +000052c0: 2078 293c 7370 616e 2063 6c61 7373 3d22 x) const │ │ │ │ +000052e0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +000052f0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +00005340: 2020 3131 313c 2f73 7061 6e3e 2020 2020 111 │ │ │ │ +00005350: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ +00005370: 726e 3c2f 7370 616e 3e20 636f 6e73 7461 rn consta │ │ │ │ +00005380: 6e74 5f3b 3c2f 6469 763e 0a3c 6469 7620 nt_;
    .
    112 │ │ │ │ +000053e0: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
    .
    .
    113< │ │ │ │ +00005440: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    ..
    115 NodePtr │ │ │ │ +00005590: 203c 6120 636c 6173 733d 2263 6f64 6520 apply(const < │ │ │ │ +00005610: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00005620: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ +00005630: 6130 3237 3936 2e68 746d 6c23 6136 3065 a02796.html#a60e │ │ │ │ +00005640: 6436 3136 3235 3934 3564 3039 3636 3130 d61625945d096610 │ │ │ │ +00005650: 3361 3539 6439 6334 3763 3861 3622 3e55 3a59d9c47c8a6">U │ │ │ │ +00005660: 6e61 7279 3c2f 613e 2661 6d70 3b20 6f70 nary& op │ │ │ │ +00005670: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const ov │ │ │ │ +00005690: 6572 7269 6465 203c 2f73 7061 6e3e 7b3c erride {< │ │ │ │ +000056a0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000056f0: 3131 363c 2f73 7061 6e3e 2020 2020 2020 116 │ │ │ │ +00005700: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ +00005750: 4e6f 6465 5074 723c 2f61 3e20 6628 3c73 NodePtr f(new < │ │ │ │ +00005780: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00005790: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +000057a0: 3032 3737 362e 6874 6d6c 223e 4c65 6166 02776.html">Leaf │ │ │ │ +000057b0: 3c2f 613e 286f 7028 636f 6e73 7461 6e74 (op(constant │ │ │ │ +000057c0: 5f29 2c20 6e72 4173 7369 676e 6d65 6e74 _), nrAssignment │ │ │ │ +000057d0: 735f 2929 3b3c 2f64 6976 3e0a 3c64 6976 s_));
    .
    < │ │ │ │ +00005810: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00005820: 6e6f 223e 2020 3131 373c 2f73 7061 6e3e no"> 117 │ │ │ │ +00005830: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ +00005850: 7265 7475 726e 3c2f 7370 616e 3e20 663b return f; │ │ │ │ +00005860: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000058b0: 2031 3138 3c2f 7370 616e 3e20 2020 207d 118 } │ │ │ │ +000058c0: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c64
    .
    . │ │ │ │ +000058e0: 3c61 2069 643d 226c 3030 3131 3922 206e 119
    .
    . │ │ │ │ +00005970: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000059a0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 1 │ │ │ │ +00005a00: 3231 3c2f 613e 3c2f 7370 616e 3e20 2020 21 │ │ │ │ +00005a10: 203c 6120 636c 6173 733d 2263 6f64 6520 NodePtr ap │ │ │ │ +00005ac0: 706c 793c 2f61 3e28 3c73 7061 6e20 636c ply(co │ │ │ │ +00005ae0: 6e73 743c 2f73 7061 6e3e 2055 6e61 7279 nst Unary │ │ │ │ +00005af0: 4173 7369 676e 6d65 6e74 2661 6d70 3b20 Assignment& │ │ │ │ +00005b00: 6f70 2c3c 2f64 6976 3e0a 3c64 6976 2063 op,
    .
    122 │ │ │ │ +00005b60: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00005b70: 3c73 7061 6e20 636c 6173 733d 226b 6579 const A │ │ │ │ +00005bc0: 7373 6967 6e6d 656e 7426 6c74 3b4c 2667 ssignment<L&g │ │ │ │ +00005bd0: 743b 3c2f 613e 2661 6d70 3b20 6173 7369 t;& assi │ │ │ │ +00005be0: 676e 6d65 6e74 293c 7370 616e 2063 6c61 gnment) co │ │ │ │ +00005c00: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override {
    .
    < │ │ │ │ +00005c50: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00005c60: 6e6f 223e 2020 3132 333c 2f73 7061 6e3e no"> 123 │ │ │ │ +00005c70: 2020 2020 2020 3c61 2063 6c61 7373 3d22 NodePtr f(new Leaf(op(as │ │ │ │ +00005d30: 7369 676e 6d65 6e74 2c20 636f 6e73 7461 signment, consta │ │ │ │ +00005d40: 6e74 5f29 2c20 6e72 4173 7369 676e 6d65 nt_), nrAssignme │ │ │ │ +00005d50: 6e74 735f 2929 3b3c 2f64 6976 3e0a 3c64 nts_));
    . │ │ │ │ +00005d70: 3c61 2069 643d 226c 3030 3132 3422 206e 124 return │ │ │ │ +00005de0: 663b 3c2f 6469 763e 0a3c 6469 7620 636c f;
    .
    125 │ │ │ │ +00005e40: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ +00005e50: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ +00005ee0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00005ef0: 6e6f 223e 2020 3132 373c 2f73 7061 6e3e no"> 127 │ │ │ │ +00005f00: 2020 2020 3c73 7061 6e20 636c 6173 733d // App │ │ │ │ +00005f20: 6c79 2062 696e 6172 7920 6f70 6572 6174 ly binary operat │ │ │ │ +00005f30: 6f72 2026 7175 6f74 3b68 203d 2066 206f or "h = f o │ │ │ │ +00005f40: 7020 6726 7175 6f74 3b20 6f6e 204c 6561 p g" on Lea │ │ │ │ +00005f50: 6620 6e6f 6465 3c2f 7370 616e 3e3c 2f64 f node.
    12 │ │ │ │ +00005fb0: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 // Note op is │ │ │ │ +00005fe0: 6e6f 7420 6173 7375 6d65 6420 636f 6d6d not assumed comm │ │ │ │ +00005ff0: 7574 6174 6976 6520 736f 2077 6520 6e65 utative so we ne │ │ │ │ +00006000: 6564 2074 6f20 6b65 6570 2074 7261 636b ed to keep track │ │ │ │ +00006010: 206f 6620 6f72 6465 723c 2f73 7061 6e3e of order │ │ │ │ +00006020: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00006070: 2031 3239 3c2f 7370 616e 3e20 2020 203c 129 < │ │ │ │ +00006080: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +00006090: 656e 7422 3e2f 2f20 5369 6d70 6c79 2063 ent">// Simply c │ │ │ │ +000060a0: 616c 6c73 2061 7070 6c79 206f 6e20 6172 alls apply on ar │ │ │ │ +000060b0: 6775 6d65 6e74 2074 6f20 6361 6c6c 2063 gument to call c │ │ │ │ +000060c0: 6f72 7265 6374 2076 6972 7475 616c 206d orrect virtual m │ │ │ │ +000060d0: 6574 686f 643a 3c2f 7370 616e 3e3c 2f64 ethod:.
    13 │ │ │ │ +00006130: 303c 2f73 7061 6e3e 2020 2020 3c73 7061 0 // fL.apply_f_ │ │ │ │ +00006160: 6f70 5f67 2867 4c29 202d 2667 743b 2067 op_g(gL) -> g │ │ │ │ +00006170: 4c2e 6170 706c 795f 675f 6f70 5f66 4c28 L.apply_g_op_fL( │ │ │ │ +00006180: 664c 2920 2862 656c 6f77 293c 2f73 7061 fL) (below)
    .
    131 │ │ │ │ +000061f0: 203c 7370 616e 2063 6c61 7373 3d22 636f // fL.app │ │ │ │ +00006210: 6c79 5f66 5f6f 705f 6728 6743 2920 2d26 ly_f_op_g(gC) -& │ │ │ │ +00006220: 6774 3b20 6743 2e61 7070 6c79 5f67 5f6f gt; gC.apply_g_o │ │ │ │ +00006230: 705f 664c 2866 4c29 2028 4368 6f69 6365 p_fL(fL) (Choice │ │ │ │ +00006240: 293c 2f73 7061 6e3e 3c2f 6469 763e 0a3c )
    .< │ │ │ │ +00006250: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00006260: 3e3c 6120 6964 3d22 6c30 3031 3332 2220 > 132 NodePtr apply_f_op_g( │ │ │ │ +00006310: 3c73 7061 6e20 636c 6173 733d 226b 6579 const │ │ │ │ +00006360: 4e6f 6465 3c2f 613e 2661 6d70 3b20 672c Node& g, │ │ │ │ +00006370: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const Binary& │ │ │ │ +000063a0: 6f70 293c 7370 616e 2063 6c61 7373 3d22 op) const │ │ │ │ +000063c0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +000063d0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +00006420: 2020 3133 333c 2f73 7061 6e3e 2020 2020 133 │ │ │ │ +00006430: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ +00006450: 726e 3c2f 7370 616e 3e20 672e 6170 706c rn g.appl │ │ │ │ +00006460: 795f 675f 6f70 5f66 4c28 2a3c 7370 616e y_g_op_fL(*this, op │ │ │ │ +00006490: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    134 │ │ │ │ +000064f0: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    135 .
    1 │ │ │ │ +000065a0: 3336 3c2f 7370 616e 3e20 2020 203c 7370 36 // Applying b │ │ │ │ +000065d0: 696e 6172 7920 6f70 6572 6174 6f72 2074 inary operator t │ │ │ │ +000065e0: 6f20 7477 6f20 6c65 6176 6573 2072 6573 o two leaves res │ │ │ │ +000065f0: 756c 7473 2069 6e20 6120 6c65 6166 3c2f ults in a leaf
    .
    < │ │ │ │ +00006640: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00006650: 6e6f 223e 2020 3133 373c 2f73 7061 6e3e no"> 137 │ │ │ │ +00006660: 2020 2020 3c61 2063 6c61 7373 3d22 636f NodePtr │ │ │ │ +000066c0: 6170 706c 795f 675f 6f70 5f66 4c28 3c73 apply_g_op_fL(const │ │ │ │ +000066f0: 204c 6561 6626 616d 703b 2066 4c2c 203c Leaf& fL, < │ │ │ │ +00006700: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00006710: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const Binary& op │ │ │ │ +00006730: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const ov │ │ │ │ +00006750: 6572 7269 6465 203c 2f73 7061 6e3e 7b3c erride {< │ │ │ │ +00006760: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000067b0: 3133 383c 2f73 7061 6e3e 2020 2020 2020 138 │ │ │ │ +000067c0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // fL op g │ │ │ │ +000067e0: 4c3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c L
    .< │ │ │ │ +000067f0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00006800: 3e3c 6120 6964 3d22 6c30 3031 3339 2220 > 139 NodePtr │ │ │ │ +000068a0: 3c2f 613e 2068 283c 7370 616e 2063 6c61 h(new │ │ │ │ +000068c0: 3c2f 7370 616e 3e20 4c65 6166 286f 7028 Leaf(op( │ │ │ │ +000068d0: 664c 2e63 6f6e 7374 616e 745f 2c20 636f fL.constant_, co │ │ │ │ +000068e0: 6e73 7461 6e74 5f29 2c20 6e72 4173 7369 nstant_), nrAssi │ │ │ │ +000068f0: 676e 6d65 6e74 735f 2929 3b3c 2f64 6976 gnments_));
    .
    140< │ │ │ │ +00006950: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return h;
    .< │ │ │ │ +000069a0: 6120 6964 3d22 6c30 3031 3431 2220 6e61 a id="l00141" na │ │ │ │ +000069b0: 6d65 3d22 6c30 3031 3431 223e 3c2f 613e me="l00141"> │ │ │ │ +000069c0: 3c73 7061 6e20 636c 6173 733d 226c 696e 141 }
    .< │ │ │ │ +00006a00: 6120 6964 3d22 6c30 3031 3432 2220 6e61 a id="l00142" na │ │ │ │ +00006a10: 6d65 3d22 6c30 3031 3432 223e 3c2f 613e me="l00142"> │ │ │ │ +00006a20: 3c73 7061 6e20 636c 6173 733d 226c 696e 142
    .
    143 │ │ │ │ +00006aa0: 203c 7370 616e 2063 6c61 7373 3d22 636f // If sec │ │ │ │ +00006ac0: 6f6e 6420 6172 6775 6d65 6e74 2069 7320 ond argument is │ │ │ │ +00006ad0: 6120 4368 6f69 6365 206e 6f64 652c 2063 a Choice node, c │ │ │ │ +00006ae0: 616c 6c20 6974 2623 3339 3b73 2061 7070 all it's app │ │ │ │ +00006af0: 6c79 2077 6974 6820 6c65 6166 2061 7320 ly with leaf as │ │ │ │ +00006b00: 7365 636f 6e64 3c2f 7370 616e 3e3c 2f64 second.
    14 │ │ │ │ +00006b60: 343c 2f73 7061 6e3e 2020 2020 3c61 2063 4 Node │ │ │ │ +00006bc0: 5074 723c 2f61 3e20 6170 706c 795f 675f Ptr apply_g_ │ │ │ │ +00006bd0: 6f70 5f66 4328 3c73 7061 6e20 636c 6173 op_fC(cons │ │ │ │ +00006bf0: 743c 2f73 7061 6e3e 2043 686f 6963 6526 t Choice& │ │ │ │ +00006c00: 616d 703b 2066 432c 203c 7370 616e 2063 amp; fC, c │ │ │ │ +00006c20: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ +00006c30: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const override │ │ │ │ +00006c60: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ +00006c70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    │ │ │ │ +00006d70: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ +00006dd0: 0a3c 6469 7620 636c 6173 733d 226c 696e ...
    │ │ │ │ +00006f00: 2020 3134 393c 2f61 3e3c 2f73 7061 6e3e 149 │ │ │ │ +00006f10: 2020 2020 3c61 2063 6c61 7373 3d22 636f NodePtr │ │ │ │ +00006f70: 3c61 2063 6c61 7373 3d22 636f 6465 2068 choose(const L │ │ │ │ +00006ff0: 2661 6d70 3b20 6c61 6265 6c2c 203c 7370 & label, size_t index) const override │ │ │ │ +00007050: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ +00007060: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00007090: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 150 return NodePtr(new < │ │ │ │ +00007160: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00007170: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +00007180: 3032 3737 362e 6874 6d6c 223e 4c65 6166 02776.html">Leaf │ │ │ │ +00007190: 3c2f 613e 2863 6f6e 7374 616e 7428 292c (constant(), │ │ │ │ +000071a0: 206e 7241 7373 6967 6e6d 656e 7473 2829 nrAssignments() │ │ │ │ +000071b0: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
    .
    151 │ │ │ │ +00007210: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +00007220: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00007290: 6120 6964 3d22 6c30 3031 3533 2220 6e61 a id="l00153" na │ │ │ │ +000072a0: 6d65 3d22 6c30 3031 3533 223e 3c2f 613e me="l00153"> │ │ │ │ +000072b0: 3c73 7061 6e20 636c 6173 733d 226c 696e 153 b │ │ │ │ +000072f0: 6f6f 6c3c 2f73 7061 6e3e 2069 734c 6561 ool isLea │ │ │ │ +00007300: 6628 293c 7370 616e 2063 6c61 7373 3d22 f() const │ │ │ │ +00007320: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +00007330: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ +00007350: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ +00007370: 7472 7565 3c2f 7370 616e 3e3b 207d 3c2f true; }.
    .. │ │ │ │ +00007470: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000074a0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 155 pri │ │ │ │ +000074e0: 7661 7465 3c2f 7370 616e 3e3a 3c2f 6469 vate:.
    156 │ │ │ │ +00007540: 3c2f 7370 616e 3e20 2020 203c 7370 616e using Ba │ │ │ │ +00007570: 7365 203d 203c 6120 636c 6173 733d 2263 se = DecisionTree&l │ │ │ │ +000075b0: 743b 4c2c 2059 2667 743b 3a3a 4e6f 6465 t;L, Y>::Node │ │ │ │ +000075c0: 3c2f 613e 3b3c 2f64 6976 3e0a 3c64 6976 ;
    .
    < │ │ │ │ +00007600: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00007610: 6e6f 223e 2020 3135 373c 2f73 7061 6e3e no"> 157 │ │ │ │ +00007620: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    │ │ │ │ +00007670: 2020 3135 393c 2f73 7061 6e3e 2020 2020 159 │ │ │ │ +00007680: 3c73 7061 6e20 636c 6173 733d 226b 6579 friend class │ │ │ │ +000076c0: 3c2f 7370 616e 3e62 6f6f 7374 3a3a 7365 boost::se │ │ │ │ +000076d0: 7269 616c 697a 6174 696f 6e3a 3a61 6363 rialization::acc │ │ │ │ +000076e0: 6573 733b 3c2f 6469 763e 0a3c 6469 7620 ess;
    .
    160 │ │ │ │ +00007740: 2020 203c 7370 616e 2063 6c61 7373 3d22 templat │ │ │ │ +00007760: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <class │ │ │ │ +00007790: 4152 4348 4956 4526 6774 3b3c 2f64 6976 ARCHIVE>
    .
    161< │ │ │ │ +000077f0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> void │ │ │ │ +00007820: 7365 7269 616c 697a 6528 4152 4348 4956 serialize(ARCHIV │ │ │ │ +00007830: 4526 616d 703b 2061 722c 203c 7370 616e E& ar, const unsigned │ │ │ │ +00007880: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c int /*version*/) {
    .< │ │ │ │ +000078e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000078f0: 3e3c 6120 6964 3d22 6c30 3031 3632 2220 > 162 ar & │ │ │ │ +00007940: 3b20 424f 4f53 545f 5345 5249 414c 495a ; BOOST_SERIALIZ │ │ │ │ +00007950: 4154 494f 4e5f 4241 5345 5f4f 424a 4543 ATION_BASE_OBJEC │ │ │ │ +00007960: 545f 4e56 5028 4261 7365 293b 3c2f 6469 T_NVP(Base);.
    163 │ │ │ │ +000079c0: 3c2f 7370 616e 3e20 2020 2020 2061 7226 ar& │ │ │ │ +000079d0: 616d 703b 2042 4f4f 5354 5f53 4552 4941 amp; BOOST_SERIA │ │ │ │ +000079e0: 4c49 5a41 5449 4f4e 5f4e 5650 2863 6f6e LIZATION_NVP(con │ │ │ │ +000079f0: 7374 616e 745f 293b 3c2f 6469 763e 0a3c stant_);
    .< │ │ │ │ +00007a00: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00007a10: 3e3c 6120 6964 3d22 6c30 3031 3634 2220 > 164 ar& │ │ │ │ +00007a60: 2042 4f4f 5354 5f53 4552 4941 4c49 5a41 BOOST_SERIALIZA │ │ │ │ +00007a70: 5449 4f4e 5f4e 5650 286e 7241 7373 6967 TION_NVP(nrAssig │ │ │ │ +00007a80: 6e6d 656e 7473 5f29 3b3c 2f64 6976 3e0a nments_);
    . │ │ │ │ +00007a90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 . │ │ │ │ +00007af0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00007b20: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 166 }; │ │ │ │ +00007b60: 2f2f 204c 6561 663c 2f73 7061 6e3e 3c2f // Leaf.
    .. │ │ │ │ +00007c60: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    │ │ │ │ +00007d70: 2020 3136 393c 2f73 7061 6e3e 2020 3c73 169 // Choice
    .
    170< │ │ │ │ +00007e00: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +00007e10: 656e 7422 3e20 202f 2a2a 2a2a 2a2a 2a2a ent"> /******** │ │ │ │ +00007e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00007e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00007e40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00007e50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00007e60: 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 ****/.
    171 │ │ │ │ +00007ec0: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +00007ee0: 656d 706c 6174 653c 2f73 7061 6e3e 266c emplate&l │ │ │ │ +00007ef0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ +00007f10: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ +00007f40: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ +00008000: 2031 3732 3c2f 613e 3c2f 7370 616e 3e20 172 │ │ │ │ +00008010: 203c 7370 616e 2063 6c61 7373 3d22 6b65 struct DecisionTree<L, Y>::< │ │ │ │ +00008080: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00008090: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +000080a0: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ +000080b0: 6365 3c2f 613e 3a20 3c73 7061 6e20 636c ce: pu │ │ │ │ +000080d0: 626c 6963 3c2f 7370 616e 3e20 3c61 2063 blic Decision │ │ │ │ +00008110: 5472 6565 3c2f 613e 266c 743b 4c2c 2059 Tree<L, Y │ │ │ │ +00008120: 2667 743b 3a3a 4e6f 6465 207b 3c2f 6469 >::Node {.
    │ │ │ │ +000081c0: 2020 3137 343c 2f61 3e3c 2f73 7061 6e3e 174 │ │ │ │ +000081d0: 2020 2020 4c20 3c61 2063 6c61 7373 3d22 L label_;
    .
    175 .
    177 std::vecto │ │ │ │ +00008340: 7226 6c74 3b4e 6f64 6550 7472 2667 743b r<NodePtr> │ │ │ │ +00008350: 203c 6120 636c 6173 733d 2263 6f64 6520 branches_; │ │ │ │ +000083b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00008400: 2031 3738 3c2f 7370 616e 3e20 3c2f 6469 178 .
    .
    179< │ │ │ │ +000084f0: 2f73 7061 6e3e 2020 203c 7370 616e 2063 /span> p │ │ │ │ +00008510: 7269 7661 7465 3c2f 7370 616e 3e3a 3c2f rivate:.
    1 │ │ │ │ +00008570: 3834 3c2f 7370 616e 3e20 2020 203c 7370 84 size_t allSame_;.
    1 │ │ │ │ +00008600: 3835 3c2f 7370 616e 3e20 3c2f 6469 763e 85
    │ │ │ │ +00008610: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00008640: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 186 u │ │ │ │ +00008680: 7369 6e67 203c 2f73 7061 6e3e 4368 6f69 sing Choi │ │ │ │ +00008690: 6365 5074 7220 3d20 626f 6f73 743a 3a73 cePtr = boost::s │ │ │ │ +000086a0: 6861 7265 645f 7074 7226 6c74 3b63 6f6e hared_ptr<con │ │ │ │ +000086b0: 7374 2043 686f 6963 6526 6774 3b3b 3c2f st Choice>;.
    1 │ │ │ │ +00008710: 3837 3c2f 7370 616e 3e20 3c2f 6469 763e 87
    │ │ │ │ +00008720: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +000087b0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +000087c0: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +000087d0: 2361 6530 6238 3434 3437 3338 3233 6161 #ae0b844473823aa │ │ │ │ +000087e0: 3039 6337 3165 3636 3834 3439 3530 3963 09c71e668449509c │ │ │ │ +000087f0: 6432 223e 2020 3138 383c 2f61 3e3c 2f73 d2"> 188 pub │ │ │ │ +00008820: 6c69 633c 2f73 7061 6e3e 3a3c 2f64 6976 lic:
    .
    │ │ │ │ +000088c0: 2031 3930 3c2f 613e 3c2f 7370 616e 3e20 190 │ │ │ │ +000088d0: 2020 203c 6120 636c 6173 733d 2263 6f64 Choice() │ │ │ │ +00008930: 207b 7d3c 2f64 6976 3e0a 3c64 6976 2063 {}
    .
    . │ │ │ │ +00008980: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000089b0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 1 │ │ │ │ +00008a10: 3931 3c2f 613e 3c2f 7370 616e 3e20 3c2f 91 .
    1 │ │ │ │ +00008a70: 3932 3c2f 7370 616e 3e20 2020 207e 4368 92 ~Ch │ │ │ │ +00008a80: 6f69 6365 2829 3c73 7061 6e20 636c 6173 oice() ove │ │ │ │ +00008aa0: 7272 6964 6520 3c2f 7370 616e 3e7b 3c2f rride {.
    1 │ │ │ │ +00008b00: 3933 3c2f 7370 616e 3e3c 7370 616e 2063 93#ifdef DT_DE │ │ │ │ +00008b30: 4255 475f 4d45 4d4f 5259 3c2f 7370 616e BUG_MEMORY
    .
    . │ │ │ │ +00008ba0: 3c61 2069 643d 226c 3030 3139 3422 206e 194 │ │ │ │ +00008c20: 3c2f 613e 3c2f 7370 616e 3e20 2020 2020 │ │ │ │ +00008c30: 2073 7464 3a3a 7374 643a 3a63 6f75 7420 std::std::cout │ │ │ │ +00008c40: 266c 743b 266c 743b 204e 6f64 653a 3a6e << Node::n │ │ │ │ +00008c50: 724e 6f64 6573 2026 6c74 3b26 6c74 3b20 rNodes << │ │ │ │ +00008c60: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ +00008c80: 743b 2064 6573 7472 7563 7469 6e67 2028 t; destructing ( │ │ │ │ +00008c90: 4368 6f69 6365 2920 2671 756f 743b 3c2f Choice) " << t │ │ │ │ +00008cb0: 6869 732d 2667 743b 6964 2829 3c2f 6469 his->id().
    195 │ │ │ │ +00008d10: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ +00008d20: 2020 2020 2020 2020 2020 2020 266c 743b < │ │ │ │ +00008d30: 266c 743b 2073 7464 3a3a 7374 643a 3a65 < std::std::e │ │ │ │ +00008d40: 6e64 6c3b 3c2f 6469 763e 0a3c 6469 7620 ndl;
    .
    196< │ │ │ │ +00008da0: 7370 616e 2063 6c61 7373 3d22 7072 6570 span class="prep │ │ │ │ +00008db0: 726f 6365 7373 6f72 223e 2365 6e64 6966 rocessor">#endif │ │ │ │ +00008dc0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    ...< │ │ │ │ +00008ed0: 6120 6964 3d22 6c30 3031 3938 2220 6e61 a id="l00198" na │ │ │ │ +00008ee0: 6d65 3d22 6c30 3031 3938 223e 3c2f 613e me="l00198"> │ │ │ │ +00008ef0: 3c73 7061 6e20 636c 6173 733d 226c 696e 198
    .
    .< │ │ │ │ +00008f60: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00008f70: 3e3c 6120 6964 3d22 6c30 3032 3030 2220 > 20 │ │ │ │ +00008ff0: 303c 2f61 3e3c 2f73 7061 6e3e 2020 2020 0 │ │ │ │ +00009000: 3c73 7061 6e20 636c 6173 733d 226b 6579 static NodePtr │ │ │ │ +00009080: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Unique(const C │ │ │ │ +00009100: 686f 6963 6550 7472 2661 6d70 3b20 6629 hoicePtr& f) │ │ │ │ +00009110: 207b 3c2f 6469 763e 0a3c 6469 7620 636c {
    .
    .< │ │ │ │ +00009160: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00009170: 3e3c 6120 6964 3d22 6c30 3032 3031 2220 > 20 │ │ │ │ +000091f0: 313c 2f61 3e3c 2f73 7061 6e3e 3c73 7061 1#ifndef G │ │ │ │ +00009220: 5453 414d 5f44 545f 4e4f 5f50 5255 4e49 TSAM_DT_NO_PRUNI │ │ │ │ +00009230: 4e47 3c2f 7370 616e 3e3c 2f64 6976 3e0a NG
    . │ │ │ │ +00009240: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +00009320: 2032 3033 3c2f 7370 616e 3e20 2020 2020 203 │ │ │ │ +00009330: 2020 2061 7373 6572 7428 662d 2667 743b assert(f-> │ │ │ │ +00009340: 6272 616e 6368 6573 2829 2e73 697a 6528 branches().size( │ │ │ │ +00009350: 2920 2667 743b 2030 293b 3c2f 6469 763e ) > 0);
    │ │ │ │ +00009360: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +000093f0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00009400: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +00009410: 2361 6662 3635 6266 6535 6433 6463 3262 #afb65bfe5d3dc2b │ │ │ │ +00009420: 3866 3239 3465 6138 3934 6161 6263 3235 8f294ea894aabc25 │ │ │ │ +00009430: 6133 223e 2020 3230 343c 2f61 3e3c 2f73 a3"> 204 Node │ │ │ │ +000094a0: 5074 723c 2f61 3e20 6630 203d 2066 2d26 Ptr f0 = f-& │ │ │ │ +000094b0: 6774 3b62 7261 6e63 6865 735f 5b30 5d3b gt;branches_[0]; │ │ │ │ +000094c0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00009510: 2032 3035 3c2f 7370 616e 3e20 3c2f 6469 205 .
    206 │ │ │ │ +00009570: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ +00009580: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00009590: 6f72 6474 7970 6522 3e73 697a 655f 743c ordtype">size_t< │ │ │ │ +000095a0: 2f73 7061 6e3e 206e 7241 7373 6967 6e6d /span> nrAssignm │ │ │ │ +000095b0: 656e 7473 203d 2030 3b3c 2f64 6976 3e0a ents = 0;
    . │ │ │ │ +000095c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000095f0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 207 for │ │ │ │ +00009640: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (auto branch: f-> │ │ │ │ +00009670: 3b62 7261 6e63 6865 7328 2929 207b 3c2f ;branches()) {.
    2 │ │ │ │ +000096d0: 3038 3c2f 7370 616e 3e20 2020 2020 2020 08 │ │ │ │ +000096e0: 2020 2061 7373 6572 7428 6272 616e 6368 assert(branch │ │ │ │ +000096f0: 2d26 6774 3b69 734c 6561 6628 2929 3b3c ->isLeaf());< │ │ │ │ +00009700: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00009750: 3230 393c 2f73 7061 6e3e 2020 2020 2020 209 │ │ │ │ +00009760: 2020 2020 6e72 4173 7369 676e 6d65 6e74 nrAssignment │ │ │ │ +00009770: 7320 2b3d 3c2f 6469 763e 0a3c 6469 7620 s +=
    .
    210 │ │ │ │ +000097d0: 2020 2020 2020 2020 2020 2020 2062 6f6f boo │ │ │ │ +000097e0: 7374 3a3a 6479 6e61 6d69 635f 706f 696e st::dynamic_poin │ │ │ │ +000097f0: 7465 725f 6361 7374 266c 743b 636f 6e73 ter_cast<cons │ │ │ │ +00009800: 7420 4c65 6166 2667 743b 2862 7261 6e63 t Leaf>(branc │ │ │ │ +00009810: 6829 2d26 6774 3b6e 7241 7373 6967 6e6d h)->nrAssignm │ │ │ │ +00009820: 656e 7473 2829 3b3c 2f64 6976 3e0a 3c64 ents();
    . │ │ │ │ +00009840: 3c61 2069 643d 226c 3030 3231 3122 206e 211 }
    .
    212< │ │ │ │ +000098e0: 2f73 7061 6e3e 2020 2020 2020 2020 3c61 /span> No │ │ │ │ +00009940: 6465 5074 723c 2f61 3e20 6e65 774c 6561 dePtr newLea │ │ │ │ +00009950: 6628 3c2f 6469 763e 0a3c 6469 7620 636c f(
    .
    213 │ │ │ │ +000099b0: 2020 2020 2020 2020 203c 7370 616e 2063 n │ │ │ │ +000099d0: 6577 3c2f 7370 616e 3e20 4c65 6166 2862 ew Leaf(b │ │ │ │ +000099e0: 6f6f 7374 3a3a 6479 6e61 6d69 635f 706f oost::dynamic_po │ │ │ │ +000099f0: 696e 7465 725f 6361 7374 266c 743b 636f inter_cast<co │ │ │ │ +00009a00: 6e73 7420 4c65 6166 2667 743b 2866 3029 nst Leaf>(f0) │ │ │ │ +00009a10: 2d26 6774 3b63 6f6e 7374 616e 7428 292c ->constant(), │ │ │ │ +00009a20: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00009a70: 2032 3134 3c2f 7370 616e 3e20 2020 2020 214 │ │ │ │ +00009a80: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00009a90: 6e72 4173 7369 676e 6d65 6e74 7329 293b nrAssignments)); │ │ │ │ +00009aa0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ +00009b00: 6120 6964 3d22 6c30 3032 3135 2220 6e61 a id="l00215" na │ │ │ │ +00009b10: 6d65 3d22 6c30 3032 3135 223e 3c2f 613e me="l00215"> │ │ │ │ +00009b20: 3c73 7061 6e20 636c 6173 733d 226c 696e 215< │ │ │ │ +00009b80: 2f61 3e3c 2f73 7061 6e3e 2020 2020 2020 /a> │ │ │ │ +00009b90: 2020 3c73 7061 6e20 636c 6173 733d 226b retu │ │ │ │ +00009bb0: 726e 3c2f 7370 616e 3e20 6e65 774c 6561 rn newLea │ │ │ │ +00009bc0: 663b 3c2f 6469 763e 0a3c 6469 7620 636c f;
    .
    216 │ │ │ │ +00009c20: 2020 207d 203c 7370 616e 2063 6c61 7373 } e │ │ │ │ +00009c40: 6c73 653c 2f73 7061 6e3e 3c2f 6469 763e lse
    │ │ │ │ +00009c50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00009c80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 217 │ │ │ │ +00009cc0: 2365 6e64 6966 3c2f 7370 616e 3e3c 2f64 #endif.
    21 │ │ │ │ +00009d20: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ +00009d30: 3c73 7061 6e20 636c 6173 733d 226b 6579 return │ │ │ │ +00009d50: 3c2f 7370 616e 3e20 663b 3c2f 6469 763e f;
    │ │ │ │ +00009d60: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ +00009dc0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00009e30: 6120 6964 3d22 6c30 3032 3231 2220 6e61 a id="l00221" na │ │ │ │ +00009e40: 6d65 3d22 6c30 3032 3231 223e 3c2f 613e me="l00221"> │ │ │ │ +00009e50: 3c73 7061 6e20 636c 6173 733d 226c 696e 221 b │ │ │ │ +00009e90: 6f6f 6c3c 2f73 7061 6e3e 2069 734c 6561 ool isLea │ │ │ │ +00009ea0: 6628 293c 7370 616e 2063 6c61 7373 3d22 f() const │ │ │ │ +00009ec0: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +00009ed0: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ +00009ef0: 726e 3c2f 7370 616e 3e20 3c73 7061 6e20 rn │ │ │ │ +00009f10: 6661 6c73 653c 2f73 7061 6e3e 3b20 7d3c false; }< │ │ │ │ +00009f20: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00009f70: 3232 323c 2f73 7061 6e3e 203c 2f64 6976 222
    .
    .
    │ │ │ │ +0000a010: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 224 Choice │ │ │ │ +0000a0c0: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ +0000a0e0: 3c2f 7370 616e 3e20 4c26 616d 703b 206c L& l │ │ │ │ +0000a0f0: 6162 656c 2c20 3c73 7061 6e20 636c 6173 abel, │ │ │ │ +0000a110: 7369 7a65 5f74 3c2f 7370 616e 3e20 636f size_t co │ │ │ │ +0000a120: 756e 7429 203a 3c2f 6469 763e 0a3c 6469 unt) :
    .< │ │ │ │ +0000a140: 6120 6964 3d22 6c30 3032 3235 2220 6e61 a id="l00225" na │ │ │ │ +0000a150: 6d65 3d22 6c30 3032 3235 223e 3c2f 613e me="l00225"> │ │ │ │ +0000a160: 3c73 7061 6e20 636c 6173 733d 226c 696e 225 label_(la │ │ │ │ +0000a190: 6265 6c29 2c20 616c 6c53 616d 655f 2874 bel), allSame_(t │ │ │ │ +0000a1a0: 7275 6529 207b 3c2f 6469 763e 0a3c 6469 rue) {
    .< │ │ │ │ +0000a1c0: 6120 6964 3d22 6c30 3032 3236 2220 6e61 a id="l00226" na │ │ │ │ +0000a1d0: 6d65 3d22 6c30 3032 3236 223e 3c2f 613e me="l00226"> │ │ │ │ +0000a1e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 226 branches_ │ │ │ │ +0000a210: 2e72 6573 6572 7665 2863 6f75 6e74 293b .reserve(count); │ │ │ │ +0000a220: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000a270: 2032 3237 3c2f 7370 616e 3e20 2020 207d 227 } │ │ │ │ +0000a280: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c64
    .
    ...
    .< │ │ │ │ +0000a3c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000a3d0: 3e3c 6120 6964 3d22 6c30 3032 3330 2220 > 23 │ │ │ │ +0000a450: 303c 2f61 3e3c 2f73 7061 6e3e 2020 2020 0 │ │ │ │ +0000a460: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Choice(const < │ │ │ │ +0000a4e0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0000a4f0: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +0000a500: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ +0000a510: 6365 3c2f 613e 2661 6d70 3b20 662c 203c ce& f, < │ │ │ │ +0000a520: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000a530: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const C │ │ │ │ +0000a570: 686f 6963 653c 2f61 3e26 616d 703b 2067 hoice& g │ │ │ │ +0000a580: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const Binary& │ │ │ │ +0000a5b0: 206f 7029 203a 3c2f 6469 763e 0a3c 6469 op) :
    .< │ │ │ │ +0000a5d0: 6120 6964 3d22 6c30 3032 3331 2220 6e61 a id="l00231" na │ │ │ │ +0000a5e0: 6d65 3d22 6c30 3032 3331 223e 3c2f 613e me="l00231"> │ │ │ │ +0000a5f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 231 allSame_( │ │ │ │ +0000a620: 7472 7565 2920 7b3c 2f64 6976 3e0a 3c64 true) {
    . │ │ │ │ +0000a640: 3c61 2069 643d 226c 3030 3233 3222 206e 232 // │ │ │ │ +0000a6a0: 2043 686f 6f73 6520 7768 6174 2074 6f20 Choose what to │ │ │ │ +0000a6b0: 646f 2062 6173 6564 206f 6e20 6c61 6265 do based on labe │ │ │ │ +0000a6c0: 6c3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c l
    .< │ │ │ │ +0000a6d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000a6e0: 3e3c 6120 6964 3d22 6c30 3032 3333 2220 > 233 if (f. │ │ │ │ +0000a750: 3c61 2063 6c61 7373 3d22 636f 6465 2068 label() > │ │ │ │ +0000a7b0: 3b20 672e 3c61 2063 6c61 7373 3d22 636f ; g.label() │ │ │ │ +0000a810: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    234 │ │ │ │ +0000a870: 2020 2020 2020 3c73 7061 6e20 636c 6173 // f │ │ │ │ +0000a890: 2068 6967 6865 7220 7468 616e 2067 3c2f higher than g
    .
    < │ │ │ │ +0000a8e0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000a8f0: 6e6f 223e 2020 3233 353c 2f73 7061 6e3e no"> 235 │ │ │ │ +0000a900: 2020 2020 2020 2020 6c61 6265 6c5f 203d label_ = │ │ │ │ +0000a910: 2066 2e3c 6120 636c 6173 733d 2263 6f64 f.label(); │ │ │ │ +0000a970: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000a9c0: 2032 3336 3c2f 7370 616e 3e20 2020 2020 236 │ │ │ │ +0000a9d0: 2020 203c 7370 616e 2063 6c61 7373 3d22 siz │ │ │ │ +0000a9f0: 655f 743c 2f73 7061 6e3e 2063 6f75 6e74 e_t count │ │ │ │ +0000aa00: 203d 2066 2e6e 7243 686f 6963 6573 2829 = f.nrChoices() │ │ │ │ +0000aa10: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0000aa60: 2020 3233 373c 2f73 7061 6e3e 2020 2020 237 │ │ │ │ +0000aa70: 2020 2020 6272 616e 6368 6573 5f2e 7265 branches_.re │ │ │ │ +0000aa80: 7365 7276 6528 636f 756e 7429 3b3c 2f64 serve(count);.
    23 │ │ │ │ +0000aae0: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ +0000aaf0: 3c73 7061 6e20 636c 6173 733d 226b 6579 for ( │ │ │ │ +0000ab30: 7369 7a65 5f74 3c2f 7370 616e 3e20 6920 size_t i │ │ │ │ +0000ab40: 3d20 303b 2069 2026 6c74 3b20 636f 756e = 0; i < coun │ │ │ │ +0000ab50: 743b 2069 2b2b 293c 2f64 6976 3e0a 3c64 t; i++)
    . │ │ │ │ +0000ab70: 3c61 2069 643d 226c 3030 3233 3922 206e 239 push │ │ │ │ +0000abc0: 5f62 6163 6b28 662e 3c61 2063 6c61 7373 _back(f.branche │ │ │ │ +0000ac20: 735f 3c2f 613e 5b69 5d2d 2667 743b 6170 s_[i]->ap │ │ │ │ +0000ac30: 706c 795f 665f 6f70 5f67 2867 2c20 6f70 ply_f_op_g(g, op │ │ │ │ +0000ac40: 2929 3b3c 2f64 6976 3e0a 3c64 6976 2063 ));
    .
    240 │ │ │ │ +0000aca0: 2020 2020 7d20 3c73 7061 6e20 636c 6173 } │ │ │ │ +0000acc0: 656c 7365 3c2f 7370 616e 3e20 3c73 7061 else if │ │ │ │ +0000acf0: 2867 2e3c 6120 636c 6173 733d 2263 6f64 (g.label() │ │ │ │ +0000ad50: 2667 743b 2066 2e3c 6120 636c 6173 733d > f.label()) {
    .< │ │ │ │ +0000add0: 6120 6964 3d22 6c30 3032 3431 2220 6e61 a id="l00241" na │ │ │ │ +0000ade0: 6d65 3d22 6c30 3032 3431 223e 3c2f 613e me="l00241"> │ │ │ │ +0000adf0: 3c73 7061 6e20 636c 6173 733d 226c 696e 241 / │ │ │ │ +0000ae30: 2f20 6620 6c6f 7765 7220 7468 616e 2067 / f lower than g │ │ │ │ +0000ae40: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    ..
    242 label_ │ │ │ │ +0000af40: 3d20 672e 3c61 2063 6c61 7373 3d22 636f = g.label() │ │ │ │ +0000afa0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0000aff0: 2020 3234 333c 2f73 7061 6e3e 2020 2020 243 │ │ │ │ +0000b000: 2020 2020 3c73 7061 6e20 636c 6173 733d si │ │ │ │ +0000b020: 7a65 5f74 3c2f 7370 616e 3e20 636f 756e ze_t coun │ │ │ │ +0000b030: 7420 3d20 672e 6e72 4368 6f69 6365 7328 t = g.nrChoices( │ │ │ │ +0000b040: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    244 │ │ │ │ +0000b0a0: 2020 2020 2062 7261 6e63 6865 735f 2e72 branches_.r │ │ │ │ +0000b0b0: 6573 6572 7665 2863 6f75 6e74 293b 3c2f eserve(count);.
    2 │ │ │ │ +0000b110: 3435 3c2f 7370 616e 3e20 2020 2020 2020 45 │ │ │ │ +0000b120: 203c 7370 616e 2063 6c61 7373 3d22 6b65 for (size_t i │ │ │ │ +0000b170: 203d 2030 3b20 6920 266c 743b 2063 6f75 = 0; i < cou │ │ │ │ +0000b180: 6e74 3b20 692b 2b29 3c2f 6469 763e 0a3c nt; i++)
    .< │ │ │ │ +0000b190: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000b1a0: 3e3c 6120 6964 3d22 6c30 3032 3436 2220 > 246 pus │ │ │ │ +0000b1f0: 685f 6261 636b 2867 2e3c 6120 636c 6173 h_back(g.branch │ │ │ │ +0000b250: 6573 5f3c 2f61 3e5b 695d 2d26 6774 3b61 es_[i]->a │ │ │ │ +0000b260: 7070 6c79 5f67 5f6f 705f 6643 2866 2c20 pply_g_op_fC(f, │ │ │ │ +0000b270: 6f70 2929 3b3c 2f64 6976 3e0a 3c64 6976 op));
    .
    < │ │ │ │ +0000b2b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000b2c0: 6e6f 223e 2020 3234 373c 2f73 7061 6e3e no"> 247 │ │ │ │ +0000b2d0: 2020 2020 2020 7d20 3c73 7061 6e20 636c } else {< │ │ │ │ +0000b300: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0000b350: 3234 383c 2f73 7061 6e3e 2020 2020 2020 248 │ │ │ │ +0000b360: 2020 3c73 7061 6e20 636c 6173 733d 2263 // f sam │ │ │ │ +0000b380: 6520 6c65 7665 6c20 6173 2067 3c2f 7370 e level as g
    .
    249 │ │ │ │ +0000b3f0: 2020 2020 2020 6c61 6265 6c5f 203d 2066 label_ = f │ │ │ │ +0000b400: 2e3c 6120 636c 6173 733d 2263 6f64 6520 .label();.
    2 │ │ │ │ +0000b4b0: 3530 3c2f 7370 616e 3e20 2020 2020 2020 50 │ │ │ │ +0000b4c0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ +0000b4e0: 743c 2f73 7061 6e3e 2063 6f75 6e74 203d t count = │ │ │ │ +0000b4f0: 2066 2e6e 7243 686f 6963 6573 2829 3b3c f.nrChoices();< │ │ │ │ +0000b500: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0000b550: 3235 313c 2f73 7061 6e3e 2020 2020 2020 251 │ │ │ │ +0000b560: 2020 6272 616e 6368 6573 5f2e 7265 7365 branches_.rese │ │ │ │ +0000b570: 7276 6528 636f 756e 7429 3b3c 2f64 6976 rve(count);
    .
    252< │ │ │ │ +0000b5d0: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> for (si │ │ │ │ +0000b620: 7a65 5f74 3c2f 7370 616e 3e20 6920 3d20 ze_t i = │ │ │ │ +0000b630: 303b 2069 2026 6c74 3b20 636f 756e 743b 0; i < count; │ │ │ │ +0000b640: 2069 2b2b 293c 2f64 6976 3e0a 3c64 6976 i++)
    .
    < │ │ │ │ +0000b680: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000b690: 6e6f 223e 2020 3235 333c 2f73 7061 6e3e no"> 253 │ │ │ │ +0000b6a0: 2020 2020 2020 2020 2020 7075 7368 5f62 push_b │ │ │ │ +0000b6b0: 6163 6b28 662e 3c61 2063 6c61 7373 3d22 ack(f.branches_ │ │ │ │ +0000b710: 3c2f 613e 5b69 5d2d 2667 743b 6170 706c [i]->appl │ │ │ │ +0000b720: 795f 665f 6f70 5f67 282a 672e 3c61 2063 y_f_op_g(*g.bra │ │ │ │ +0000b780: 6e63 6865 735f 3c2f 613e 5b69 5d2c 206f nches_[i], o │ │ │ │ +0000b790: 7029 293b 3c2f 6469 763e 0a3c 6469 7620 p));
    .
    254 │ │ │ │ +0000b7f0: 2020 2020 207d 3c2f 6469 763e 0a3c 6469 }
    .< │ │ │ │ +0000b810: 6120 6964 3d22 6c30 3032 3535 2220 6e61 a id="l00255" na │ │ │ │ +0000b820: 6d65 3d22 6c30 3032 3535 223e 3c2f 613e me="l00255"> │ │ │ │ +0000b830: 3c73 7061 6e20 636c 6173 733d 226c 696e 255 }
    .< │ │ │ │ +0000b870: 6120 6964 3d22 6c30 3032 3536 2220 6e61 a id="l00256" na │ │ │ │ +0000b880: 6d65 3d22 6c30 3032 3536 223e 3c2f 613e me="l00256"> │ │ │ │ +0000b890: 3c73 7061 6e20 636c 6173 733d 226c 696e 256
    .
    .< │ │ │ │ +0000b900: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000b910: 3e3c 6120 6964 3d22 6c30 3032 3538 2220 > 25 │ │ │ │ +0000b990: 383c 2f61 3e3c 2f73 7061 6e3e 2020 2020 8 │ │ │ │ +0000b9a0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const L& label │ │ │ │ +0000ba20: 3c2f 613e 2829 3c73 7061 6e20 636c 6173 () con │ │ │ │ +0000ba40: 7374 203c 2f73 7061 6e3e 7b3c 2f64 6976 st {
    .
    259< │ │ │ │ +0000baa0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return label_;
    .
    │ │ │ │ +0000bb70: 2032 3630 3c2f 613e 3c2f 7370 616e 3e20 260 │ │ │ │ +0000bb80: 2020 207d 3c2f 6469 763e 0a3c 2f64 6976 }
    .
    .
    261< │ │ │ │ +0000bbe0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +0000bc00: 3c61 2069 643d 226c 3030 3236 3222 206e 262 │ │ │ │ +0000bc60: 7369 7a65 5f74 3c2f 7370 616e 3e20 6e72 size_t nr │ │ │ │ +0000bc70: 4368 6f69 6365 7328 293c 7370 616e 2063 Choices() │ │ │ │ +0000bc90: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    .
    263 return branches_. │ │ │ │ +0000bdc0: 7369 7a65 2829 3b3c 2f64 6976 3e0a 3c64 size();
    . │ │ │ │ +0000bde0: 3c61 2069 643d 226c 3030 3236 3422 206e 264 }
    . │ │ │ │ +0000be40: 3c61 2069 643d 226c 3030 3236 3522 206e 265
    .
    266 │ │ │ │ +0000bee0: 2020 3c73 7061 6e20 636c 6173 733d 226b const std::vector │ │ │ │ +0000bf10: 266c 743b 4e6f 6465 5074 7226 6774 3b26 <NodePtr>& │ │ │ │ +0000bf20: 616d 703b 2062 7261 6e63 6865 7328 293c amp; branches()< │ │ │ │ +0000bf30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000bf40: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    267 │ │ │ │ +0000bfb0: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +0000bfd0: 6574 7572 6e3c 2f73 7061 6e3e 2062 7261 eturn bra │ │ │ │ +0000bfe0: 6e63 6865 735f 3b3c 2f64 6976 3e0a 3c64 nches_;
    . │ │ │ │ +0000c000: 3c61 2069 643d 226c 3030 3236 3822 206e 268 }
    . │ │ │ │ +0000c060: 3c61 2069 643d 226c 3030 3236 3922 206e 269
    .
    . │ │ │ │ +0000c0f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +0000c120: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 2 │ │ │ │ +0000c180: 3731 3c2f 613e 3c2f 7370 616e 3e20 2020 71 │ │ │ │ +0000c190: 203c 7370 616e 2063 6c61 7373 3d22 6b65 void< │ │ │ │ +0000c1b0: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> push_bac │ │ │ │ +0000c210: 6b3c 2f61 3e28 3c73 7061 6e20 636c 6173 k(cons │ │ │ │ +0000c230: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t NodePtr │ │ │ │ +0000c290: 3c2f 613e 2661 6d70 3b20 6e6f 6465 2920 & node) │ │ │ │ +0000c2a0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +0000c2f0: 2020 3237 323c 2f73 7061 6e3e 2020 2020 272 │ │ │ │ +0000c300: 2020 3c73 7061 6e20 636c 6173 733d 2263 // allSa │ │ │ │ +0000c320: 6d65 5f20 6973 2072 6573 7472 6963 7465 me_ is restricte │ │ │ │ +0000c330: 6420 746f 206c 6561 6620 6e6f 6465 7320 d to leaf nodes │ │ │ │ +0000c340: 696e 2061 2064 6563 6973 696f 6e20 7472 in a decision tr │ │ │ │ +0000c350: 6565 3c2f 7370 616e 3e3c 2f64 6976 3e0a ee
    . │ │ │ │ +0000c360: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .< │ │ │ │ +0000c410: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000c420: 3e3c 6120 6964 3d22 6c30 3032 3734 2220 > 274 allSa │ │ │ │ +0000c470: 6d65 5f20 3d20 6e6f 6465 2d26 6774 3b73 me_ = node->s │ │ │ │ +0000c480: 616d 654c 6561 6628 2a62 7261 6e63 6865 ameLeaf(*branche │ │ │ │ +0000c490: 735f 2e62 6163 6b28 2929 3b3c 2f64 6976 s_.back());
    .
    275< │ │ │ │ +0000c4f0: 2f73 7061 6e3e 2020 2020 2020 7d3c 2f64 /span> }.
    27 │ │ │ │ +0000c550: 363c 2f73 7061 6e3e 2020 2020 2020 6272 6 br │ │ │ │ +0000c560: 616e 6368 6573 5f2e 7075 7368 5f62 6163 anches_.push_bac │ │ │ │ +0000c570: 6b28 6e6f 6465 293b 3c2f 6469 763e 0a3c k(node);
    .< │ │ │ │ +0000c580: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000c590: 3e3c 6120 6964 3d22 6c30 3032 3737 2220 > 277 }
    .< │ │ │ │ +0000c5e0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0000c630: 3237 383c 2f73 7061 6e3e 203c 2f64 6976 278
    .
    .
    │ │ │ │ +0000c6d0: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 280 void < │ │ │ │ +0000c750: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0000c760: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ +0000c770: 2261 3032 3738 302e 6874 6d6c 2361 3437 "a02780.html#a47 │ │ │ │ +0000c780: 6366 3766 3361 6563 6538 6335 3365 6165 cf7f3aece8c53eae │ │ │ │ +0000c790: 6531 3039 3664 3665 3565 3062 3034 223e e1096d6e5e0b04"> │ │ │ │ +0000c7a0: 7072 696e 743c 2f61 3e28 3c73 7061 6e20 print( │ │ │ │ +0000c7c0: 636f 6e73 743c 2f73 7061 6e3e 2073 7464 const std │ │ │ │ +0000c7d0: 3a3a 7374 7269 6e67 2661 6d70 3b20 732c ::string& s, │ │ │ │ +0000c7e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const LabelFormatt │ │ │ │ +0000c810: 6572 2661 6d70 3b20 6c61 6265 6c46 6f72 er& labelFor │ │ │ │ +0000c820: 6d61 7474 6572 2c3c 2f64 6976 3e0a 3c64 matter,
    . │ │ │ │ +0000c840: 3c61 2069 643d 226c 3030 3238 3122 206e 281 │ │ │ │ +0000c890: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const ValueFormatt │ │ │ │ +0000c8c0: 6572 2661 6d70 3b20 7661 6c75 6546 6f72 er& valueFor │ │ │ │ +0000c8d0: 6d61 7474 6572 293c 7370 616e 2063 6c61 matter) co │ │ │ │ +0000c8f0: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override
    {
    .
    < │ │ │ │ +0000c940: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000c950: 6e6f 223e 2020 3238 323c 2f73 7061 6e3e no"> 282 │ │ │ │ +0000c960: 2020 2020 2020 7374 643a 3a63 6f75 7420 std::cout │ │ │ │ +0000c970: 266c 743b 266c 743b 2073 2026 6c74 3b26 << s <& │ │ │ │ +0000c980: 6c74 3b20 3c73 7061 6e20 636c 6173 733d lt; │ │ │ │ +0000c9a0: 2671 756f 743b 2043 686f 6963 6528 2671 " Choice(&q │ │ │ │ +0000c9b0: 756f 743b 3c2f 7370 616e 3e3b 3c2f 6469 uot;;.
    283 │ │ │ │ +0000ca10: 3c2f 7370 616e 3e20 2020 2020 2073 7464 std │ │ │ │ +0000ca20: 3a3a 636f 7574 2026 6c74 3b26 6c74 3b20 ::cout << │ │ │ │ +0000ca30: 6c61 6265 6c46 6f72 6d61 7474 6572 286c labelFormatter(l │ │ │ │ +0000ca40: 6162 656c 5f29 2026 6c74 3b26 6c74 3b20 abel_) << │ │ │ │ +0000ca50: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ +0000ca70: 743b 2920 2671 756f 743b 3c2f 7370 616e t;) " << std:: │ │ │ │ +0000ca90: 656e 646c 3b3c 2f64 6976 3e0a 3c64 6976 endl;
    .
    < │ │ │ │ +0000cad0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000cae0: 6e6f 223e 2020 3238 343c 2f73 7061 6e3e no"> 284 │ │ │ │ +0000caf0: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ +0000cb10: 666f 723c 2f73 7061 6e3e 2028 3c73 7061 for (size_t i = 0; i < │ │ │ │ +0000cb50: 3b20 6272 616e 6368 6573 5f2e 7369 7a65 ; branches_.size │ │ │ │ +0000cb60: 2829 3b20 692b 2b29 3c2f 6469 763e 0a3c (); i++)
    .< │ │ │ │ +0000cb70: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000cb80: 3e3c 6120 6964 3d22 6c30 3032 3835 2220 > 285 branc │ │ │ │ +0000cbd0: 6865 735f 5b69 5d2d 2667 743b 3c61 2063 hes_[i]->pri │ │ │ │ +0000cc30: 6e74 3c2f 613e 2828 626f 6f73 743a 3a66 nt((boost::f │ │ │ │ +0000cc40: 6f72 6d61 7428 3c73 7061 6e20 636c 6173 ormat("%s %d&qu │ │ │ │ +0000cc70: 6f74 3b3c 2f73 7061 6e3e 2920 2520 7320 ot;) % s │ │ │ │ +0000cc80: 2520 6929 2e73 7472 2829 2c3c 2f64 6976 % i).str(),
    .
    286< │ │ │ │ +0000cce0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +0000ccf0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0000cd00: 2020 6c61 6265 6c46 6f72 6d61 7474 6572 labelFormatter │ │ │ │ +0000cd10: 2c20 7661 6c75 6546 6f72 6d61 7474 6572 , valueFormatter │ │ │ │ +0000cd20: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    287 │ │ │ │ +0000cd80: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ +0000cd90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    .
    │ │ │ │ +0000cec0: 2032 3930 3c2f 613e 3c2f 7370 616e 3e20 290 │ │ │ │ +0000ced0: 2020 203c 7370 616e 2063 6c61 7373 3d22 voi │ │ │ │ +0000cef0: 643c 2f73 7061 6e3e 203c 6120 636c 6173 d dot(std::ostream&a │ │ │ │ +0000cf60: 6d70 3b20 6f73 2c20 3c73 7061 6e20 636c mp; os, co │ │ │ │ +0000cf80: 6e73 743c 2f73 7061 6e3e 204c 6162 656c nst Label │ │ │ │ +0000cf90: 466f 726d 6174 7465 7226 616d 703b 206c Formatter& l │ │ │ │ +0000cfa0: 6162 656c 466f 726d 6174 7465 722c 3c2f abelFormatter,.
    2 │ │ │ │ +0000d000: 3931 3c2f 7370 616e 3e20 2020 2020 2020 91 │ │ │ │ +0000d010: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ +0000d030: 743c 2f73 7061 6e3e 2056 616c 7565 466f t ValueFo │ │ │ │ +0000d040: 726d 6174 7465 7226 616d 703b 2076 616c rmatter& val │ │ │ │ +0000d050: 7565 466f 726d 6174 7465 722c 3c2f 6469 ueFormatter,.
    292 │ │ │ │ +0000d0b0: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ +0000d0c0: 2020 2020 3c73 7061 6e20 636c 6173 733d bo │ │ │ │ +0000d0e0: 6f6c 3c2f 7370 616e 3e20 7368 6f77 5a65 ol showZe │ │ │ │ +0000d0f0: 726f 293c 7370 616e 2063 6c61 7373 3d22 ro) const │ │ │ │ +0000d110: 6f76 6572 7269 6465 203c 2f73 7061 6e3e override │ │ │ │ +0000d120: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +0000d170: 2020 3239 333c 2f73 7061 6e3e 2020 2020 293 │ │ │ │ +0000d180: 2020 6f73 2026 6c74 3b26 6c74 3b20 3c73 os << " │ │ │ │ +0000d1b0: 5c26 7175 6f74 3b26 7175 6f74 3b3c 2f73 \"" << th │ │ │ │ +0000d1d0: 6973 2d26 6774 3b69 6428 2920 266c 743b is->id() < │ │ │ │ +0000d1e0: 266c 743b 203c 7370 616e 2063 6c61 7373 < "\" [ │ │ │ │ +0000d210: 7368 6170 653d 6369 7263 6c65 2c20 6c61 shape=circle, la │ │ │ │ +0000d220: 6265 6c3d 5c26 7175 6f74 3b26 7175 6f74 bel=\"" │ │ │ │ +0000d230: 3b3c 2f73 7061 6e3e 2026 6c74 3b26 6c74 ; << │ │ │ │ +0000d240: 3b20 6c61 6265 6c5f 3c2f 6469 763e 0a3c ; label_
    .< │ │ │ │ +0000d250: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000d260: 3e3c 6120 6964 3d22 6c30 3032 3934 2220 > 294 < │ │ │ │ +0000d2b0: 3b26 6c74 3b20 3c73 7061 6e20 636c 6173 ;< "\"] │ │ │ │ +0000d2e0: 5c6e 2671 756f 743b 3c2f 7370 616e 3e3b \n"; │ │ │ │ +0000d2f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000d340: 2032 3935 3c2f 7370 616e 3e20 2020 2020 295 │ │ │ │ +0000d350: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ +0000d370: 743c 2f73 7061 6e3e 2042 203d 2062 7261 t B = bra │ │ │ │ +0000d380: 6e63 6865 735f 2e73 697a 6528 293b 3c2f nches_.size();.
    2 │ │ │ │ +0000d3e0: 3936 3c2f 7370 616e 3e20 2020 2020 203c 96 < │ │ │ │ +0000d3f0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000d400: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (s │ │ │ │ +0000d430: 697a 655f 743c 2f73 7061 6e3e 2069 203d ize_t i = │ │ │ │ +0000d440: 2030 3b20 6920 266c 743b 2042 3b20 692b 0; i < B; i+ │ │ │ │ +0000d450: 2b29 207b 3c2f 6469 763e 0a3c 6469 7620 +) {
    .
    │ │ │ │ +0000d4a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0000d4d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ +0000d530: 3239 373c 2f61 3e3c 2f73 7061 6e3e 2020 297 │ │ │ │ +0000d540: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ +0000d560: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t NodePtr │ │ │ │ +0000d5c0: 3c2f 613e 2661 6d70 3b20 6272 616e 6368 & branch │ │ │ │ +0000d5d0: 203d 2062 7261 6e63 6865 735f 5b69 5d3b = branches_[i]; │ │ │ │ +0000d5e0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000d630: 2032 3938 3c2f 7370 616e 3e20 3c2f 6469 298 .
    299 │ │ │ │ +0000d690: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ +0000d6a0: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +0000d6b0: 656e 7422 3e2f 2f20 4368 6563 6b20 6966 ent">// Check if │ │ │ │ +0000d6c0: 207a 6572 6f3c 2f73 7061 6e3e 3c2f 6469 zero.
    300 │ │ │ │ +0000d720: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ +0000d730: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000d740: 6f72 6466 6c6f 7722 3e69 663c 2f73 7061 ordflow">if (!showZero) { │ │ │ │ +0000d760: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000d7b0: 2033 3031 3c2f 7370 616e 3e20 2020 2020 301 │ │ │ │ +0000d7c0: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ +0000d7e0: 3c2f 7370 616e 3e20 4c65 6166 2a20 6c65 Leaf* le │ │ │ │ +0000d7f0: 6166 203d 203c 7370 616e 2063 6c61 7373 af = dynam │ │ │ │ +0000d810: 6963 5f63 6173 7426 6c74 3b3c 2f73 7061 ic_cast<const Leaf* │ │ │ │ +0000d860: 2667 743b 3c2f 7370 616e 3e28 6272 616e >(bran │ │ │ │ +0000d870: 6368 2e67 6574 2829 293b 3c2f 6469 763e ch.get());
    │ │ │ │ +0000d880: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0000d8b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 302 < │ │ │ │ +0000d8e0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000d8f0: 6f72 6466 6c6f 7722 3e69 663c 2f73 7061 ordflow">if (leaf &&a │ │ │ │ +0000d910: 6d70 3b20 7661 6c75 6546 6f72 6d61 7474 mp; valueFormatt │ │ │ │ +0000d920: 6572 286c 6561 662d 2667 743b 636f 6e73 er(leaf->cons │ │ │ │ +0000d930: 7461 6e74 2829 292e 636f 6d70 6172 6528 tant()).compare( │ │ │ │ +0000d940: 3c73 7061 6e20 636c 6173 733d 2273 7472 &quo │ │ │ │ +0000d960: 743b 3026 7175 6f74 3b3c 2f73 7061 6e3e t;0" │ │ │ │ +0000d970: 2929 203c 7370 616e 2063 6c61 7373 3d22 )) con │ │ │ │ +0000d990: 7469 6e75 653c 2f73 7061 6e3e 3b3c 2f64 tinue;.
    30 │ │ │ │ +0000d9f0: 333c 2f73 7061 6e3e 2020 2020 2020 2020 3 │ │ │ │ +0000da00: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ +0000da50: 2020 3330 343c 2f73 7061 6e3e 203c 2f64 304 .
    30 │ │ │ │ +0000dab0: 353c 2f73 7061 6e3e 2020 2020 2020 2020 5 │ │ │ │ +0000dac0: 6f73 2026 6c74 3b26 6c74 3b20 3c73 7061 os << "\& │ │ │ │ +0000daf0: 7175 6f74 3b26 7175 6f74 3b3c 2f73 7061 quot;" << this │ │ │ │ +0000db10: 2d26 6774 3b69 6428 2920 266c 743b 266c ->id() <&l │ │ │ │ +0000db20: 743b 203c 7370 616e 2063 6c61 7373 3d22 t; & │ │ │ │ +0000db40: 7175 6f74 3b5c 2671 756f 743b 202d 2667 quot;\" -&g │ │ │ │ +0000db50: 743b 205c 2671 756f 743b 2671 756f 743b t; \"" │ │ │ │ +0000db60: 3c2f 7370 616e 3e20 266c 743b 266c 743b << │ │ │ │ +0000db70: 2062 7261 6e63 682d 2667 743b 6964 2829 branch->id() │ │ │ │ +0000db80: 2026 6c74 3b26 6c74 3b20 3c73 7061 6e20 << "\&qu │ │ │ │ +0000dbb0: 6f74 3b26 7175 6f74 3b3c 2f73 7061 6e3e ot;" │ │ │ │ +0000dbc0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0000dc10: 2020 3330 363c 2f73 7061 6e3e 2020 2020 306 │ │ │ │ +0000dc20: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ +0000dc40: 3c2f 7370 616e 3e20 2842 203d 3d20 3220 (B == 2 │ │ │ │ +0000dc50: 2661 6d70 3b26 616d 703b 2069 203d 3d20 && i == │ │ │ │ +0000dc60: 3029 206f 7320 266c 743b 266c 743b 203c 0) os << < │ │ │ │ +0000dc70: 7370 616e 2063 6c61 7373 3d22 7374 7269 span class="stri │ │ │ │ +0000dc80: 6e67 6c69 7465 7261 6c22 3e26 7175 6f74 ngliteral">" │ │ │ │ +0000dc90: 3b20 5b73 7479 6c65 3d64 6173 6865 645d ; [style=dashed] │ │ │ │ +0000dca0: 2671 756f 743b 3c2f 7370 616e 3e3b 3c2f ";.
    3 │ │ │ │ +0000dd00: 3037 3c2f 7370 616e 3e20 2020 2020 2020 07 │ │ │ │ +0000dd10: 206f 7320 266c 743b 266c 743b 2073 7464 os << std │ │ │ │ +0000dd20: 3a3a 656e 646c 3b3c 2f64 6976 3e0a 3c64 ::endl;
    . │ │ │ │ +0000dd40: 3c61 2069 643d 226c 3030 3330 3822 206e 308 branch │ │ │ │ +0000dd90: 2d26 6774 3b64 6f74 286f 732c 206c 6162 ->dot(os, lab │ │ │ │ +0000dda0: 656c 466f 726d 6174 7465 722c 2076 616c elFormatter, val │ │ │ │ +0000ddb0: 7565 466f 726d 6174 7465 722c 2073 686f ueFormatter, sho │ │ │ │ +0000ddc0: 775a 6572 6f29 3b3c 2f64 6976 3e0a 3c64 wZero);
    . │ │ │ │ +0000dde0: 3c61 2069 643d 226c 3030 3330 3922 206e 309 }
    . │ │ │ │ +0000de30: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ +0000de90: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    .
    │ │ │ │ +0000dfc0: 2033 3133 3c2f 613e 3c2f 7370 616e 3e20 313 │ │ │ │ +0000dfd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 boo │ │ │ │ +0000dff0: 6c3c 2f73 7061 6e3e 203c 6120 636c 6173 l sameLe │ │ │ │ +0000e050: 6166 3c2f 613e 283c 7370 616e 2063 6c61 af(con │ │ │ │ +0000e070: 7374 3c2f 7370 616e 3e20 3c61 2063 6c61 st Leaf& │ │ │ │ +0000e0b0: 616d 703b 2071 293c 7370 616e 2063 6c61 amp; q) co │ │ │ │ +0000e0d0: 6e73 7420 6f76 6572 7269 6465 203c 2f73 nst override {
    .
    .
    │ │ │ │ +0000e1c0: 2033 3134 3c2f 613e 3c2f 7370 616e 3e20 314 │ │ │ │ +0000e1d0: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +0000e1f0: 6574 7572 6e3c 2f73 7061 6e3e 203c 7370 eturn false; │ │ │ │ +0000e220: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000e270: 2033 3135 3c2f 7370 616e 3e20 2020 207d 315 } │ │ │ │ +0000e280: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0000e2d0: 2033 3136 3c2f 7370 616e 3e20 3c2f 6469 316 .
    .
    318< │ │ │ │ +0000e3c0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> bool │ │ │ │ +0000e3f0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 sameLeaf(const │ │ │ │ +0000e470: 203c 6120 636c 6173 733d 2263 6f64 6520 No │ │ │ │ +0000e4a0: 6465 3c2f 613e 2661 6d70 3b20 7129 3c73 de& q) const overr │ │ │ │ +0000e4d0: 6964 6520 3c2f 7370 616e 3e7b 3c2f 6469 ide {.
    319 │ │ │ │ +0000e530: 3c2f 7370 616e 3e20 2020 2020 203c 7370 return (q.isLeaf() │ │ │ │ +0000e570: 2026 616d 703b 2661 6d70 3b20 712e 7361 && q.sa │ │ │ │ +0000e580: 6d65 4c65 6166 282a 3c73 7061 6e20 636c meLeaf(*th │ │ │ │ +0000e5a0: 6973 3c2f 7370 616e 3e29 293b 3c2f 6469 is));.
    320 │ │ │ │ +0000e600: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
    .
    321 < │ │ │ │ +0000e670: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ +0000e6f0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000e700: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 323 bool equals(< │ │ │ │ +0000e7e0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000e7f0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const N │ │ │ │ +0000e830: 6f64 653c 2f61 3e26 616d 703b 2071 2c20 ode& q, │ │ │ │ +0000e840: 3c73 7061 6e20 636c 6173 733d 226b 6579 const CompareFunc&a │ │ │ │ +0000e870: 6d70 3b20 636f 6d70 6172 6529 3c73 7061 mp; compare) const overrid │ │ │ │ +0000e8a0: 6520 3c2f 7370 616e 3e7b 3c2f 6469 763e e {
    │ │ │ │ +0000e8b0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0000e8e0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 324 const Choic │ │ │ │ +0000e960: 653c 2f61 3e2a 206f 7468 6572 203d 203c e* other = < │ │ │ │ +0000e970: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000e980: 6f72 6422 3e64 796e 616d 6963 5f63 6173 ord">dynamic_cas │ │ │ │ +0000e990: 7426 6c74 3b3c 2f73 7061 6e3e 3c73 7061 t<const < │ │ │ │ +0000e9c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0000e9d0: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +0000e9e0: 3032 3738 302e 6874 6d6c 223e 4368 6f69 02780.html">Choi │ │ │ │ +0000e9f0: 6365 3c2f 613e 2a3c 7370 616e 2063 6c61 ce*> │ │ │ │ +0000ea10: 3b3c 2f73 7061 6e3e 2826 616d 703b 7129 ;(&q) │ │ │ │ +0000ea20: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0000ea70: 2020 3332 353c 2f73 7061 6e3e 2020 2020 325 │ │ │ │ +0000ea80: 2020 3c73 7061 6e20 636c 6173 733d 226b if (!other) < │ │ │ │ +0000eab0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0000eac0: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ +0000ead0: 2f73 7061 6e3e 203c 7370 616e 2063 6c61 /span> fal │ │ │ │ +0000eaf0: 7365 3c2f 7370 616e 3e3b 3c2f 6469 763e se;
    │ │ │ │ +0000eb00: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0000eb30: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 326 if ( │ │ │ │ +0000eb80: 7468 6973 2d26 6774 3b6c 6162 656c 5f20 this->label_ │ │ │ │ +0000eb90: 213d 206f 7468 6572 2d26 6774 3b3c 6120 != other->la │ │ │ │ +0000ebf0: 6265 6c5f 3c2f 613e 2920 3c73 7061 6e20 bel_) return false;
    .
    < │ │ │ │ +0000ec80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000ec90: 6e6f 223e 2020 3332 373c 2f73 7061 6e3e no"> 327 │ │ │ │ +0000eca0: 2020 2020 2020 3c73 7061 6e20 636c 6173 │ │ │ │ +0000ecc0: 6966 3c2f 7370 616e 3e20 2862 7261 6e63 if (branc │ │ │ │ +0000ecd0: 6865 735f 2e73 697a 6528 2920 213d 206f hes_.size() != o │ │ │ │ +0000ece0: 7468 6572 2d26 6774 3b3c 6120 636c 6173 ther->branch │ │ │ │ +0000ed40: 6573 5f3c 2f61 3e2e 7369 7a65 2829 2920 es_.size()) │ │ │ │ +0000ed50: 3c73 7061 6e20 636c 6173 733d 226b 6579 return │ │ │ │ +0000ed70: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c fa │ │ │ │ +0000ed90: 6c73 653c 2f73 7061 6e3e 3b3c 2f64 6976 lse;
    .
    328< │ │ │ │ +0000edf0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> // we don' │ │ │ │ +0000ee20: 7420 6361 7265 2061 626f 7574 2073 6861 t care about sha │ │ │ │ +0000ee30: 7265 6420 706f 696e 7465 7273 2062 6569 red pointers bei │ │ │ │ +0000ee40: 6e67 2065 7175 616c 2068 6572 653c 2f73 ng equal here
    .
    329 │ │ │ │ +0000eeb0: 2020 2020 203c 7370 616e 2063 6c61 7373 f │ │ │ │ +0000eed0: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (size_t i = 0; i < │ │ │ │ +0000ef10: 2062 7261 6e63 6865 735f 2e73 697a 6528 branches_.size( │ │ │ │ +0000ef20: 293b 2069 2b2b 293c 2f64 6976 3e0a 3c64 ); i++)
    . │ │ │ │ +0000ef40: 3c61 2069 643d 226c 3030 3333 3022 206e 330 if (! │ │ │ │ +0000efb0: 2862 7261 6e63 6865 735f 5b69 5d2d 2667 (branches_[i]-&g │ │ │ │ +0000efc0: 743b 3c61 2063 6c61 7373 3d22 636f 6465 t;e │ │ │ │ +0000eff0: 7175 616c 733c 2f61 3e28 2a28 6f74 6865 quals(*(othe │ │ │ │ +0000f000: 722d 2667 743b 3c61 2063 6c61 7373 3d22 r->branches_ │ │ │ │ +0000f060: 3c2f 613e 5b69 5d29 2c20 636f 6d70 6172 [i]), compar │ │ │ │ +0000f070: 6529 2929 3c2f 6469 763e 0a3c 6469 7620 e)))
    .
    331 │ │ │ │ +0000f0d0: 2020 2020 2020 2020 203c 7370 616e 2063 return │ │ │ │ +0000f100: 203c 7370 616e 2063 6c61 7373 3d22 6b65 false;
    .
    332 │ │ │ │ +0000f180: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +0000f1a0: 6574 7572 6e3c 2f73 7061 6e3e 203c 7370 eturn true;< │ │ │ │ +0000f1d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0000f220: 3333 333c 2f73 7061 6e3e 2020 2020 7d3c 333 }< │ │ │ │ +0000f230: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ +0000f250: 6120 6964 3d22 6c30 3033 3334 2220 6e61 a id="l00334" na │ │ │ │ +0000f260: 6d65 3d22 6c30 3033 3334 223e 3c2f 613e me="l00334"> │ │ │ │ +0000f270: 3c73 7061 6e20 636c 6173 733d 226c 696e 334
    .
    .< │ │ │ │ +0000f2e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000f2f0: 3e3c 6120 6964 3d22 6c30 3033 3336 2220 > 33 │ │ │ │ +0000f370: 363c 2f61 3e3c 2f73 7061 6e3e 2020 2020 6 │ │ │ │ +0000f380: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Y& opera │ │ │ │ +0000f400: 746f 7228 293c 2f61 3e28 3c73 7061 6e20 tor()( │ │ │ │ +0000f420: 636f 6e73 743c 2f73 7061 6e3e 203c 6120 const Assignm │ │ │ │ +0000f460: 656e 7426 6c74 3b4c 2667 743b 3c2f 613e ent<L> │ │ │ │ +0000f470: 2661 6d70 3b20 7829 3c73 7061 6e20 636c & x) c │ │ │ │ +0000f490: 6f6e 7374 206f 7665 7272 6964 6520 3c2f onst override {
    .< │ │ │ │ +0000f4c0: 6120 6964 3d22 6c30 3033 3337 2220 6e61 a id="l00337" na │ │ │ │ +0000f4d0: 6d65 3d22 6c30 3033 3337 223e 3c2f 613e me="l00337"> │ │ │ │ +0000f4e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 337#ifn │ │ │ │ +0000f520: 6465 6620 4e44 4542 5547 3c2f 7370 616e def NDEBUG
    .
    . │ │ │ │ +0000f590: 3c61 2069 643d 226c 3030 3333 3822 206e 338 │ │ │ │ +0000f610: 3c2f 613e 3c2f 7370 616e 3e20 2020 2020 │ │ │ │ +0000f620: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ +0000f640: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> Assignment< │ │ │ │ +0000f680: 3b4c 2667 743b 3a3a 636f 6e73 745f 6974 ;L>::const_it │ │ │ │ +0000f690: 6572 6174 6f72 3c2f 613e 2069 7420 3d20 erator it = │ │ │ │ +0000f6a0: 782e 6669 6e64 286c 6162 656c 5f29 3b3c x.find(label_);< │ │ │ │ +0000f6b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0000f700: 3333 393c 2f73 7061 6e3e 2020 2020 2020 339 │ │ │ │ +0000f710: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (it == x.end │ │ │ │ +0000f740: 2829 2920 7b3c 2f64 6976 3e0a 3c64 6976 ()) {
    .
    < │ │ │ │ +0000f780: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000f790: 6e6f 223e 2020 3334 303c 2f73 7061 6e3e no"> 340 │ │ │ │ +0000f7a0: 2020 2020 2020 2020 7374 643a 3a63 6f75 std::cou │ │ │ │ +0000f7b0: 7420 266c 743b 266c 743b 203c 7370 616e t << "Try │ │ │ │ +0000f7e0: 696e 6720 746f 2066 696e 6420 7661 6c75 ing to find valu │ │ │ │ +0000f7f0: 6520 666f 7220 2671 756f 743b 3c2f 7370 e for " << lab │ │ │ │ +0000f810: 656c 5f20 266c 743b 266c 743b 2073 7464 el_ << std │ │ │ │ +0000f820: 3a3a 656e 646c 3b3c 2f64 6976 3e0a 3c64 ::endl;
    ..
    341 throw │ │ │ │ +0000f940: 7374 643a 3a69 6e76 616c 6964 5f61 7267 std::invalid_arg │ │ │ │ +0000f950: 756d 656e 7428 3c2f 6469 763e 0a3c 6469 ument(
    .< │ │ │ │ +0000f970: 6120 6964 3d22 6c30 3033 3432 2220 6e61 a id="l00342" na │ │ │ │ +0000f980: 6d65 3d22 6c30 3033 3432 223e 3c2f 613e me="l00342"> │ │ │ │ +0000f990: 3c73 7061 6e20 636c 6173 733d 226c 696e 342 "D │ │ │ │ +0000f9e0: 6563 6973 696f 6e54 7265 653a 3a6f 7065 ecisionTree::ope │ │ │ │ +0000f9f0: 7261 746f 7228 293a 2076 616c 7565 2075 rator(): value u │ │ │ │ +0000fa00: 6e64 6566 696e 6564 2066 6f72 2061 206c ndefined for a l │ │ │ │ +0000fa10: 6162 656c 2671 756f 743b 3c2f 7370 616e abel"
    );
    .
    343 │ │ │ │ +0000fa80: 2020 2020 7d3c 2f64 6976 3e0a 3c64 6976 }
    .
    < │ │ │ │ +0000fac0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0000fad0: 6e6f 223e 2020 3334 343c 2f73 7061 6e3e no"> 344 │ │ │ │ +0000fae0: 3c73 7061 6e20 636c 6173 733d 2270 7265 #endi │ │ │ │ +0000fb00: 663c 2f73 7061 6e3e 3c2f 6469 763e 0a3c f
    .< │ │ │ │ +0000fb10: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0000fb20: 3e3c 6120 6964 3d22 6c30 3033 3435 2220 > 345 size_t │ │ │ │ +0000fb90: 2069 6e64 6578 203d 2078 2e61 7428 6c61 index = x.at(la │ │ │ │ +0000fba0: 6265 6c5f 293b 3c2f 6469 763e 0a3c 6469 bel_);
    .< │ │ │ │ +0000fbc0: 6120 6964 3d22 6c30 3033 3436 2220 6e61 a id="l00346" na │ │ │ │ +0000fbd0: 6d65 3d22 6c30 3033 3436 223e 3c2f 613e me="l00346"> │ │ │ │ +0000fbe0: 3c73 7061 6e20 636c 6173 733d 226c 696e 346 NodePtr child = branc │ │ │ │ +0000fc70: 6865 735f 5b69 6e64 6578 5d3b 3c2f 6469 hes_[index];.
    347 │ │ │ │ +0000fcd0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 return (*child)(x) │ │ │ │ +0000fd10: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0000fd60: 2020 3334 383c 2f73 7061 6e3e 2020 2020 348 │ │ │ │ +0000fd70: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ +0000fdc0: 2020 3334 393c 2f73 7061 6e3e 203c 2f64 349 .
    .
    351 │ │ │ │ +0000feb0: 3c2f 7370 616e 3e20 2020 2043 686f 6963 Choic │ │ │ │ +0000fec0: 6528 3c73 7061 6e20 636c 6173 733d 226b e(const L& labe │ │ │ │ +0000fef0: 6c2c 203c 7370 616e 2063 6c61 7373 3d22 l, const Choice& │ │ │ │ +0000ff20: 3b20 662c 203c 7370 616e 2063 6c61 7373 ; f, const │ │ │ │ +0000ff40: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Unary& op) :.
    352 │ │ │ │ +00010000: 3c2f 7370 616e 3e20 2020 2020 206c 6162 lab │ │ │ │ +00010010: 656c 5f28 6c61 6265 6c29 2c20 616c 6c53 el_(label), allS │ │ │ │ +00010020: 616d 655f 2874 7275 6529 207b 3c2f 6469 ame_(true) {.
    353 │ │ │ │ +00010080: 3c2f 7370 616e 3e20 2020 2020 2062 7261 bra │ │ │ │ +00010090: 6e63 6865 735f 2e72 6573 6572 7665 2866 nches_.reserve(f │ │ │ │ +000100a0: 2e62 7261 6e63 6865 735f 2e73 697a 6528 .branches_.size( │ │ │ │ +000100b0: 2929 3b20 203c 7370 616e 2063 6c61 7373 )); // re │ │ │ │ +000100d0: 7365 7276 6520 7370 6163 653c 2f73 7061 serve space
    .
    .< │ │ │ │ +00010130: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00010140: 3e3c 6120 6964 3d22 6c30 3033 3534 2220 > 35 │ │ │ │ +000101c0: 343c 2f61 3e3c 2f73 7061 6e3e 2020 2020 4 │ │ │ │ +000101d0: 2020 3c73 7061 6e20 636c 6173 733d 226b for< │ │ │ │ +000101f0: 2f73 7061 6e3e 2028 3c73 7061 6e20 636c /span> (co │ │ │ │ +00010210: 6e73 743c 2f73 7061 6e3e 203c 6120 636c nst NodeP │ │ │ │ +00010270: 7472 3c2f 613e 2661 6d70 3b20 6272 616e tr& bran │ │ │ │ +00010280: 6368 203a 2066 2e62 7261 6e63 6865 735f ch : f.branches_ │ │ │ │ +00010290: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    355 │ │ │ │ +000102f0: 2020 2020 2020 7075 7368 5f62 6163 6b28 push_back( │ │ │ │ +00010300: 6272 616e 6368 2d26 6774 3b61 7070 6c79 branch->apply │ │ │ │ +00010310: 286f 7029 293b 3c2f 6469 763e 0a3c 6469 (op));
    .< │ │ │ │ +00010330: 6120 6964 3d22 6c30 3033 3536 2220 6e61 a id="l00356" na │ │ │ │ +00010340: 6d65 3d22 6c30 3033 3536 223e 3c2f 613e me="l00356"> │ │ │ │ +00010350: 3c73 7061 6e20 636c 6173 733d 226c 696e 356 }
    .< │ │ │ │ +00010380: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00010390: 3e3c 6120 6964 3d22 6c30 3033 3537 2220 > 357 }
    .< │ │ │ │ +000103e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000103f0: 3e3c 6120 6964 3d22 6c30 3033 3538 2220 > 358
    .
    │ │ │ │ +00010480: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000104b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ +00010510: 3336 393c 2f61 3e3c 2f73 7061 6e3e 2020 369 │ │ │ │ +00010520: 2020 3c61 2063 6c61 7373 3d22 636f 6465 Choice(const │ │ │ │ +000105a0: 204c 2661 6d70 3b20 6c61 6265 6c2c 203c L& label, < │ │ │ │ +000105b0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000105c0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const C │ │ │ │ +00010600: 686f 6963 653c 2f61 3e26 616d 703b 2066 hoice& f │ │ │ │ +00010610: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const UnaryAssign │ │ │ │ +00010640: 6d65 6e74 2661 6d70 3b20 6f70 2c3c 2f64 ment& op,.
    37 │ │ │ │ +000106a0: 303c 2f73 7061 6e3e 2020 2020 2020 2020 0 │ │ │ │ +000106b0: 2020 203c 7370 616e 2063 6c61 7373 3d22 const Assignment< │ │ │ │ +00010710: 4c26 6774 3b3c 2f61 3e26 616d 703b 2061 L>& a │ │ │ │ +00010720: 7373 6967 6e6d 656e 7429 3c2f 6469 763e ssignment)
    │ │ │ │ +00010730: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00010760: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 371 : l │ │ │ │ +00010790: 6162 656c 5f28 6c61 6265 6c29 2c20 616c abel_(label), al │ │ │ │ +000107a0: 6c53 616d 655f 2874 7275 6529 207b 3c2f lSame_(true) {.
    3 │ │ │ │ +00010800: 3732 3c2f 7370 616e 3e20 2020 2020 2062 72 b │ │ │ │ +00010810: 7261 6e63 6865 735f 2e72 6573 6572 7665 ranches_.reserve │ │ │ │ +00010820: 2866 2e3c 6120 636c 6173 733d 2263 6f64 (f.branches_.size()); // reserve spa │ │ │ │ +000108b0: 6365 3c2f 7370 616e 3e3c 2f64 6976 3e0a ce
    . │ │ │ │ +000108c0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    ..
    374 │ │ │ │ +00010a00: 2020 2020 203c 6120 636c 6173 733d 2263 Assignment<L │ │ │ │ +00010a40: 2667 743b 3c2f 613e 2061 7373 6967 6e6d > assignm │ │ │ │ +00010a50: 656e 745f 203d 2061 7373 6967 6e6d 656e ent_ = assignmen │ │ │ │ +00010a60: 743b 3c2f 6469 763e 0a3c 6469 7620 636c t;
    .
    375 .
    3 │ │ │ │ +00010b10: 3736 3c2f 7370 616e 3e20 2020 2020 203c 76 < │ │ │ │ +00010b20: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00010b30: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (s │ │ │ │ +00010b60: 697a 655f 743c 2f73 7061 6e3e 2069 203d ize_t i = │ │ │ │ +00010b70: 2030 3b20 6920 266c 743b 2066 2e3c 6120 0; i < f.br │ │ │ │ +00010bd0: 616e 6368 6573 5f3c 2f61 3e2e 7369 7a65 anches_.size │ │ │ │ +00010be0: 2829 3b20 692b 2b29 207b 3c2f 6469 763e (); i++) {
    │ │ │ │ +00010bf0: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +00010c80: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00010c90: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +00010ca0: 2361 6364 6366 3732 3439 3966 3230 6437 #acdcf72499f20d7 │ │ │ │ +00010cb0: 3935 3835 6134 3332 3239 3834 3862 3138 9585a43229848b18 │ │ │ │ +00010cc0: 3433 223e 2020 3337 373c 2f61 3e3c 2f73 43"> 377 assi │ │ │ │ +00010ce0: 676e 6d65 6e74 5f5b 6c61 6265 6c5f 5d20 gnment_[label_] │ │ │ │ +00010cf0: 3d20 693b 2020 3c73 7061 6e20 636c 6173 = i; // S │ │ │ │ +00010d10: 6574 2061 7373 6967 6e6d 656e 7420 666f et assignment fo │ │ │ │ +00010d20: 7220 6c61 6265 6c20 746f 2069 3c2f 7370 r label to i
    .
    378 < │ │ │ │ +00010d90: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00010de0: 3337 393c 2f73 7061 6e3e 2020 2020 2020 379 │ │ │ │ +00010df0: 2020 3c73 7061 6e20 636c 6173 733d 226b const NodePtr │ │ │ │ +00010e70: 2062 7261 6e63 6820 3d20 662e 3c61 2063 branch = f.bra │ │ │ │ +00010ed0: 6e63 6865 735f 3c2f 613e 5b69 5d3b 3c2f nches_[i];.
    3 │ │ │ │ +00010f30: 3830 3c2f 7370 616e 3e20 2020 2020 2020 80 │ │ │ │ +00010f40: 2070 7573 685f 6261 636b 2862 7261 6e63 push_back(branc │ │ │ │ +00010f50: 682d 2667 743b 6170 706c 7928 6f70 2c20 h->apply(op, │ │ │ │ +00010f60: 6173 7369 676e 6d65 6e74 5f29 293b 3c2f assignment_));..< │ │ │ │ +000111d0: 6120 6964 3d22 6c30 3033 3834 2220 6e61 a id="l00384" na │ │ │ │ +000111e0: 6d65 3d22 6c30 3033 3834 223e 3c2f 613e me="l00384"> │ │ │ │ +000111f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 384 assignm │ │ │ │ +00011220: 656e 745f 2e65 7261 7365 2861 7373 6967 ent_.erase(assig │ │ │ │ +00011230: 6e6d 656e 745f 6974 293b 3c2f 6469 763e nment_it);
    │ │ │ │ +00011240: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00011270: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 385 }.
    386 │ │ │ │ +000112f0: 3c2f 7370 616e 3e20 2020 207d 3c2f 6469 }.
    387 │ │ │ │ +00011350: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +00011360: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ +00011370: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ +00011380: 6e30 3033 3839 2220 6461 7461 2d73 7461 n00389" data-sta │ │ │ │ +00011390: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ +000113a0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    389 NodePtr apply(const │ │ │ │ +00011520: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ +00011570: 556e 6172 793c 2f61 3e26 616d 703b 206f Unary& o │ │ │ │ +00011580: 7029 3c73 7061 6e20 636c 6173 733d 226b p) const o │ │ │ │ +000115a0: 7665 7272 6964 6520 3c2f 7370 616e 3e7b verride { │ │ │ │ +000115b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00011600: 2033 3930 3c2f 7370 616e 3e20 2020 2020 390 │ │ │ │ +00011610: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto r = boost::ma │ │ │ │ +00011640: 6b65 5f73 6861 7265 6426 6c74 3b43 686f ke_shared<Cho │ │ │ │ +00011650: 6963 6526 6774 3b28 6c61 6265 6c5f 2c20 ice>(label_, │ │ │ │ +00011660: 2a3c 7370 616e 2063 6c61 7373 3d22 6b65 *this, op);
    .< │ │ │ │ +00011690: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000116a0: 3e3c 6120 6964 3d22 6c30 3033 3931 2220 > 391 return │ │ │ │ +00011710: 2055 6e69 7175 6528 7229 3b3c 2f64 6976 Unique(r);
    .
    392< │ │ │ │ +00011770: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    .
    393 .
    .
    395 Node │ │ │ │ +00011920: 5074 723c 2f61 3e20 3c61 2063 6c61 7373 Ptr apply(const UnaryAssig │ │ │ │ +000119b0: 6e6d 656e 7426 616d 703b 206f 702c 3c2f nment& op,.
    3 │ │ │ │ +00011a10: 3936 3c2f 7370 616e 3e20 2020 2020 2020 96 │ │ │ │ +00011a20: 2020 2020 2020 2020 2020 203c 7370 616e const Assign │ │ │ │ +00011a80: 6d65 6e74 266c 743b 4c26 6774 3b3c 2f61 ment<L>& assignmen │ │ │ │ +00011aa0: 7429 3c73 7061 6e20 636c 6173 733d 226b t) const o │ │ │ │ +00011ac0: 7665 7272 6964 6520 3c2f 7370 616e 3e7b verride { │ │ │ │ +00011ad0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00011b20: 2033 3937 3c2f 7370 616e 3e20 2020 2020 397 │ │ │ │ +00011b30: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto r = boost::ma │ │ │ │ +00011b60: 6b65 5f73 6861 7265 6426 6c74 3b43 686f ke_shared<Cho │ │ │ │ +00011b70: 6963 6526 6774 3b28 6c61 6265 6c5f 2c20 ice>(label_, │ │ │ │ +00011b80: 2a3c 7370 616e 2063 6c61 7373 3d22 6b65 *this, op, assignme │ │ │ │ +00011bb0: 6e74 293b 3c2f 6469 763e 0a3c 6469 7620 nt);
    .
    398 │ │ │ │ +00011c10: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +00011c30: 6574 7572 6e3c 2f73 7061 6e3e 2055 6e69 eturn Uni │ │ │ │ +00011c40: 7175 6528 7229 3b3c 2f64 6976 3e0a 3c64 que(r);
    . │ │ │ │ +00011c60: 3c61 2069 643d 226c 3030 3339 3922 206e 399 }
    ..
    4 │ │ │ │ +00011d00: 3030 3c2f 7370 616e 3e20 3c2f 6469 763e 00
    │ │ │ │ +00011d10: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00011d40: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 401 / │ │ │ │ +00011d80: 2f20 4170 706c 7920 6269 6e61 7279 206f / Apply binary o │ │ │ │ +00011d90: 7065 7261 746f 7220 2671 756f 743b 6820 perator "h │ │ │ │ +00011da0: 3d20 6620 6f70 2067 2671 756f 743b 206f = f op g" o │ │ │ │ +00011db0: 6e20 4368 6f69 6365 206e 6f64 653c 2f73 n Choice node
    .
    402 │ │ │ │ +00011e20: 2020 203c 7370 616e 2063 6c61 7373 3d22 // Note │ │ │ │ +00011e40: 206f 7020 6973 206e 6f74 2061 7373 756d op is not assum │ │ │ │ +00011e50: 6564 2063 6f6d 6d75 7461 7469 7665 2073 ed commutative s │ │ │ │ +00011e60: 6f20 7765 206e 6565 6420 746f 206b 6565 o we need to kee │ │ │ │ +00011e70: 7020 7472 6163 6b20 6f66 206f 7264 6572 p track of order │ │ │ │ +00011e80: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00011ea0: 3c61 2069 643d 226c 3030 3430 3322 206e 403 // S │ │ │ │ +00011f00: 696d 706c 7920 6361 6c6c 7320 6170 706c imply calls appl │ │ │ │ +00011f10: 7920 6f6e 2061 7267 756d 656e 7420 746f y on argument to │ │ │ │ +00011f20: 2063 616c 6c20 636f 7272 6563 7420 7669 call correct vi │ │ │ │ +00011f30: 7274 7561 6c20 6d65 7468 6f64 3a3c 2f73 rtual method:
    .
    404 │ │ │ │ +00011fa0: 2020 203c 7370 616e 2063 6c61 7373 3d22 // fC.a │ │ │ │ +00011fc0: 7070 6c79 5f66 5f6f 705f 6728 674c 2920 pply_f_op_g(gL) │ │ │ │ +00011fd0: 2d26 6774 3b20 674c 2e61 7070 6c79 5f67 -> gL.apply_g │ │ │ │ +00011fe0: 5f6f 705f 6643 2866 4329 202d 2667 743b _op_fC(fC) -> │ │ │ │ +00011ff0: 2028 4c65 6166 293c 2f73 7061 6e3e 3c2f (Leaf).
    4 │ │ │ │ +00012050: 3035 3c2f 7370 616e 3e20 2020 203c 7370 05 // fC.apply_f │ │ │ │ +00012080: 5f6f 705f 6728 6743 2920 2d26 6774 3b20 _op_g(gC) -> │ │ │ │ +00012090: 6743 2e61 7070 6c79 5f67 5f6f 705f 6643 gC.apply_g_op_fC │ │ │ │ +000120a0: 2866 4329 202d 2667 743b 2028 6265 6c6f (fC) -> (belo │ │ │ │ +000120b0: 7729 3c2f 7370 616e 3e3c 2f64 6976 3e0a w)
    . │ │ │ │ +000120c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +000120f0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 406 NodePtr< │ │ │ │ +00012170: 2f61 3e20 6170 706c 795f 665f 6f70 5f67 /a> apply_f_op_g │ │ │ │ +00012180: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const Node& g │ │ │ │ +000121e0: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const Binary& │ │ │ │ +00012210: 206f 7029 3c73 7061 6e20 636c 6173 733d op) const │ │ │ │ +00012230: 206f 7665 7272 6964 6520 3c2f 7370 616e override {
    .
    407 │ │ │ │ +000122a0: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ +000122c0: 7572 6e3c 2f73 7061 6e3e 2067 2e61 7070 urn g.app │ │ │ │ +000122d0: 6c79 5f67 5f6f 705f 6643 282a 3c73 7061 ly_g_op_fC(*this, o │ │ │ │ +00012300: 7029 3b3c 2f64 6976 3e0a 3c64 6976 2063 p);
    .
    408 │ │ │ │ +00012360: 2020 7d3c 2f64 6976 3e0a 3c64 6976 2063 }
    .
    409 < │ │ │ │ +000123c0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00012410: 3431 303c 2f73 7061 6e3e 2020 2020 3c73 410 // If second │ │ │ │ +00012440: 2061 7267 756d 656e 7420 6f66 2062 696e argument of bin │ │ │ │ +00012450: 6172 7920 6f70 2069 7320 4c65 6166 206e ary op is Leaf n │ │ │ │ +00012460: 6f64 652c 2072 6563 7572 7365 206f 6e20 ode, recurse on │ │ │ │ +00012470: 6272 616e 6368 6573 3c2f 7370 616e 3e3c branches< │ │ │ │ +00012480: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000124d0: 3431 313c 2f73 7061 6e3e 2020 2020 3c61 411 No │ │ │ │ +00012530: 6465 5074 723c 2f61 3e20 6170 706c 795f dePtr apply_ │ │ │ │ +00012540: 675f 6f70 5f66 4c28 3c73 7061 6e20 636c g_op_fL(co │ │ │ │ +00012560: 6e73 743c 2f73 7061 6e3e 204c 6561 6626 nst Leaf& │ │ │ │ +00012570: 616d 703b 2066 4c2c 203c 7370 616e 2063 amp; fL, c │ │ │ │ +00012590: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ +000125a0: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const override │ │ │ │ +000125d0: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ +000125e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ +000126b0: 3c61 2069 643d 226c 3030 3431 3322 206e 413 for (auto& │ │ │ │ +00012740: 616d 703b 2661 6d70 3b20 6272 616e 6368 amp;& branch │ │ │ │ +00012750: 203a 2062 7261 6e63 6865 735f 293c 2f64 : branches_).
    41 │ │ │ │ +000127b0: 343c 2f73 7061 6e3e 2020 2020 2020 2020 4 │ │ │ │ +000127c0: 682d 2667 743b 7075 7368 5f62 6163 6b28 h->push_back( │ │ │ │ +000127d0: 664c 2e61 7070 6c79 5f66 5f6f 705f 6728 fL.apply_f_op_g( │ │ │ │ +000127e0: 2a62 7261 6e63 682c 206f 7029 293b 3c2f *branch, op));.
    4 │ │ │ │ +00012840: 3135 3c2f 7370 616e 3e20 2020 2020 203c 15 < │ │ │ │ +00012850: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00012860: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ +00012870: 2f73 7061 6e3e 2055 6e69 7175 6528 6829 /span> Unique(h) │ │ │ │ +00012880: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +000128d0: 2020 3431 363c 2f73 7061 6e3e 2020 2020 416 │ │ │ │ +000128e0: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ +00012930: 2020 3431 373c 2f73 7061 6e3e 203c 2f64 417 .
    41 │ │ │ │ +00012990: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 // If second a │ │ │ │ +000129c0: 7267 756d 656e 7420 6f66 2062 696e 6172 rgument of binar │ │ │ │ +000129d0: 7920 6f70 2069 7320 4368 6f69 6365 2c20 y op is Choice, │ │ │ │ +000129e0: 6361 6c6c 2063 6f6e 7374 7275 6374 6f72 call constructor │ │ │ │ +000129f0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +00012a10: 3c61 2069 643d 226c 3030 3431 3922 206e 419 NodePtr apply_g_op_fC( │ │ │ │ +00012ac0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Choice& f │ │ │ │ +00012af0: 432c 203c 7370 616e 2063 6c61 7373 3d22 C, const Binary& │ │ │ │ +00012b20: 3b20 6f70 293c 7370 616e 2063 6c61 7373 ; op) cons │ │ │ │ +00012b40: 7420 6f76 6572 7269 6465 203c 2f73 7061 t override {
    .
    420 │ │ │ │ +00012bb0: 2020 2020 3c73 7061 6e20 636c 6173 733d auto h = boost: │ │ │ │ +00012be0: 3a6d 616b 655f 7368 6172 6564 266c 743b :make_shared< │ │ │ │ +00012bf0: 4368 6f69 6365 2667 743b 2866 432c 202a Choice>(fC, * │ │ │ │ +00012c00: 3c73 7061 6e20 636c 6173 733d 226b 6579 this, op);
    . │ │ │ │ +00012c40: 3c61 2069 643d 226c 3030 3432 3122 206e 421 return │ │ │ │ +00012cb0: 556e 6971 7565 2868 293b 3c2f 6469 763e Unique(h);
    │ │ │ │ +00012cc0: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ +00012d20: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00012d90: 6120 6964 3d22 6c30 3034 3234 2220 6e61 a id="l00424" na │ │ │ │ +00012da0: 6d65 3d22 6c30 3034 3234 223e 3c2f 613e me="l00424"> │ │ │ │ +00012db0: 3c73 7061 6e20 636c 6173 733d 226c 696e 424 // If │ │ │ │ +00012df0: 2073 6563 6f6e 6420 6172 6775 6d65 6e74 second argument │ │ │ │ +00012e00: 206f 6620 6269 6e61 7279 206f 7020 6973 of binary op is │ │ │ │ +00012e10: 204c 6561 663c 2f73 7061 6e3e 3c2f 6469 Leaf.
    425 │ │ │ │ +00012e70: 3c2f 7370 616e 3e20 2020 203c 7370 616e template │ │ │ │ +00012ea0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ +00012ec0: 6d65 3c2f 7370 616e 3e20 4f50 2667 743b me OP> │ │ │ │ +00012ed0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00012f20: 2034 3236 3c2f 7370 616e 3e20 2020 203c 426 < │ │ │ │ +00012f30: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00012f40: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ +00012f50: 6130 3237 3936 2e68 746d 6c23 6133 3661 a02796.html#a36a │ │ │ │ +00012f60: 3938 6636 3162 3062 3764 6630 3432 3432 98f61b0b7df04242 │ │ │ │ +00012f70: 3762 3133 3262 3331 3364 3766 6422 3e4e 7b132b313d7fd">N │ │ │ │ +00012f80: 6f64 6550 7472 3c2f 613e 2061 7070 6c79 odePtr apply │ │ │ │ +00012f90: 5f66 435f 6f70 5f67 4c28 3c73 7061 6e20 _fC_op_gL( │ │ │ │ +00012fb0: 636f 6e73 743c 2f73 7061 6e3e 204c 6561 const Lea │ │ │ │ +00012fc0: 6626 616d 703b 2067 4c2c 204f 5020 6f70 f& gL, OP op │ │ │ │ +00012fd0: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ +00013010: 6120 6964 3d22 6c30 3034 3237 2220 6e61 a id="l00427" na │ │ │ │ +00013020: 6d65 3d22 6c30 3034 3237 223e 3c2f 613e me="l00427"> │ │ │ │ +00013030: 3c73 7061 6e20 636c 6173 733d 226c 696e 427 aut │ │ │ │ +00013070: 6f3c 2f73 7061 6e3e 2068 203d 2062 6f6f o h = boo │ │ │ │ +00013080: 7374 3a3a 6d61 6b65 5f73 6861 7265 6426 st::make_shared& │ │ │ │ +00013090: 6c74 3b43 686f 6963 6526 6774 3b28 6c61 lt;Choice>(la │ │ │ │ +000130a0: 6265 6c28 292c 206e 7243 686f 6963 6573 bel(), nrChoices │ │ │ │ +000130b0: 2829 293b 3c2f 6469 763e 0a3c 6469 7620 ());
    .
    428 │ │ │ │ +00013110: 2020 2020 203c 7370 616e 2063 6c61 7373 f │ │ │ │ +00013130: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (auto& │ │ │ │ +00013160: 3b26 616d 703b 2062 7261 6e63 6820 3a20 ;& branch : │ │ │ │ +00013170: 6272 616e 6368 6573 5f29 3c2f 6469 763e branches_)
    │ │ │ │ +00013180: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000131b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 429 h-& │ │ │ │ +000131e0: 6774 3b70 7573 685f 6261 636b 2862 7261 gt;push_back(bra │ │ │ │ +000131f0: 6e63 682d 2667 743b 6170 706c 795f 665f nch->apply_f_ │ │ │ │ +00013200: 6f70 5f67 2867 4c2c 206f 7029 293b 3c2f op_g(gL, op));.
    4 │ │ │ │ +00013260: 3330 3c2f 7370 616e 3e20 2020 2020 203c 30 < │ │ │ │ +00013270: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00013280: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ +00013290: 2f73 7061 6e3e 2055 6e69 7175 6528 6829 /span> Unique(h) │ │ │ │ +000132a0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +000132f0: 2020 3433 313c 2f73 7061 6e3e 2020 2020 431 │ │ │ │ +00013300: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ +00013350: 2020 3433 323c 2f73 7061 6e3e 203c 2f64 432 .
    ..
    │ │ │ │ +000135e0: 2034 3335 3c2f 7370 616e 3e20 2020 2020 435 │ │ │ │ +000135f0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 if (label_ == │ │ │ │ +00013620: 6c61 6265 6c29 203c 7370 616e 2063 6c61 label) return b │ │ │ │ +00013650: 7261 6e63 6865 735f 5b69 6e64 6578 5d3b ranches_[index]; │ │ │ │ +00013660: 2020 3c73 7061 6e20 636c 6173 733d 2263 // choos │ │ │ │ +00013680: 6520 6272 616e 6368 3c2f 7370 616e 3e3c e branch< │ │ │ │ +00013690: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000136e0: 3433 363c 2f73 7061 6e3e 203c 2f64 6976 436
    .
    437< │ │ │ │ +00013740: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> // second case │ │ │ │ +00013770: 2c20 6e6f 7420 6c61 6265 6c20 6f66 2069 , not label of i │ │ │ │ +00013780: 6e74 6572 6573 742c 206a 7573 7420 7265 nterest, just re │ │ │ │ +00013790: 6375 7273 653c 2f73 7061 6e3e 3c2f 6469 curse.
    438 │ │ │ │ +000137f0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 auto r │ │ │ │ +00013820: 203d 2062 6f6f 7374 3a3a 6d61 6b65 5f73 = boost::make_s │ │ │ │ +00013830: 6861 7265 6426 6c74 3b43 686f 6963 6526 hared<Choice& │ │ │ │ +00013840: 6774 3b28 6c61 6265 6c5f 2c20 6272 616e gt;(label_, bran │ │ │ │ +00013850: 6368 6573 5f2e 7369 7a65 2829 293b 3c2f ches_.size());.
    4 │ │ │ │ +000138b0: 3339 3c2f 7370 616e 3e20 2020 2020 203c 39 < │ │ │ │ +000138c0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000138d0: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (auto< │ │ │ │ +00013900: 2f73 7061 6e3e 2661 6d70 3b26 616d 703b /span>&& │ │ │ │ +00013910: 2062 7261 6e63 6820 3a20 6272 616e 6368 branch : branch │ │ │ │ +00013920: 6573 5f29 3c2f 6469 763e 0a3c 6469 7620 es_)
    .
    440 │ │ │ │ +00013980: 2020 2020 2020 2072 2d26 6774 3b70 7573 r->pus │ │ │ │ +00013990: 685f 6261 636b 2862 7261 6e63 682d 2667 h_back(branch-&g │ │ │ │ +000139a0: 743b 6368 6f6f 7365 286c 6162 656c 2c20 t;choose(label, │ │ │ │ +000139b0: 696e 6465 7829 293b 3c2f 6469 763e 0a3c index));
    .< │ │ │ │ +000139c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000139d0: 3e3c 6120 6964 3d22 6c30 3034 3431 2220 > 441
    return │ │ │ │ +00013a40: 2055 6e69 7175 6528 7229 3b3c 2f64 6976 Unique(r);
    .
    442< │ │ │ │ +00013aa0: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    .
    443 .
    4 │ │ │ │ +00013b60: 3434 3c2f 7370 616e 3e20 2020 3c73 7061 44 private │ │ │ │ +00013b90: 3a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 :
    .
    │ │ │ │ +00013be0: 2020 3434 353c 2f73 7061 6e3e 2020 2020 445 │ │ │ │ +00013bf0: 3c73 7061 6e20 636c 6173 733d 226b 6579 using Base = DecisionT │ │ │ │ +00013c50: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ +00013c60: 3a4e 6f64 653c 2f61 3e3b 3c2f 6469 763e :Node;
    │ │ │ │ +00013c70: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00013ce0: 6120 6964 3d22 6c30 3034 3438 2220 6e61 a id="l00448" na │ │ │ │ +00013cf0: 6d65 3d22 6c30 3034 3438 223e 3c2f 613e me="l00448"> │ │ │ │ +00013d00: 3c73 7061 6e20 636c 6173 733d 226c 696e 448 frien │ │ │ │ +00013d40: 643c 2f73 7061 6e3e 203c 7370 616e 2063 d c │ │ │ │ +00013d60: 6c61 7373 203c 2f73 7061 6e3e 3c61 2063 lass boost │ │ │ │ +00013dc0: 3a3a 7365 7269 616c 697a 6174 696f 6e3a ::serialization: │ │ │ │ +00013dd0: 3a61 6363 6573 733c 2f61 3e3b 3c2f 6469 :access;.
    449 │ │ │ │ +00013e30: 3c2f 7370 616e 3e20 2020 203c 7370 616e template │ │ │ │ +00013e60: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <class │ │ │ │ +00013e80: 3c2f 7370 616e 3e20 4152 4348 4956 4526 ARCHIVE& │ │ │ │ +00013e90: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    450 │ │ │ │ +00013ef0: 2020 3c73 7061 6e20 636c 6173 733d 226b void │ │ │ │ +00013f10: 3c2f 7370 616e 3e20 7365 7269 616c 697a serializ │ │ │ │ +00013f20: 6528 4152 4348 4956 4526 616d 703b 2061 e(ARCHIVE& a │ │ │ │ +00013f30: 722c 203c 7370 616e 2063 6c61 7373 3d22 r, const │ │ │ │ +00013f70: 756e 7369 676e 6564 3c2f 7370 616e 3e20 unsigned │ │ │ │ +00013f80: 3c73 7061 6e20 636c 6173 733d 226b 6579 int /*ver │ │ │ │ +00013fc0: 7369 6f6e 2a2f 3c2f 7370 616e 3e29 207b sion*/) { │ │ │ │ +00013fd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00014020: 2034 3531 3c2f 7370 616e 3e20 2020 2020 451 │ │ │ │ +00014030: 2061 7220 2661 6d70 3b20 424f 4f53 545f ar & BOOST_ │ │ │ │ +00014040: 5345 5249 414c 495a 4154 494f 4e5f 4241 SERIALIZATION_BA │ │ │ │ +00014050: 5345 5f4f 424a 4543 545f 4e56 5028 4261 SE_OBJECT_NVP(Ba │ │ │ │ +00014060: 7365 293b 3c2f 6469 763e 0a3c 6469 7620 se);
    .
    452 │ │ │ │ +000140c0: 2020 2020 2061 7226 616d 703b 2042 4f4f ar& BOO │ │ │ │ +000140d0: 5354 5f53 4552 4941 4c49 5a41 5449 4f4e ST_SERIALIZATION │ │ │ │ +000140e0: 5f4e 5650 286c 6162 656c 5f29 3b3c 2f64 _NVP(label_);.
    45 │ │ │ │ +00014140: 333c 2f73 7061 6e3e 2020 2020 2020 6172 3 ar │ │ │ │ +00014150: 2661 6d70 3b20 424f 4f53 545f 5345 5249 & BOOST_SERI │ │ │ │ +00014160: 414c 495a 4154 494f 4e5f 4e56 5028 6272 ALIZATION_NVP(br │ │ │ │ +00014170: 616e 6368 6573 5f29 3b3c 2f64 6976 3e0a anches_);
    . │ │ │ │ +00014180: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    455 │ │ │ │ +00014260: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    456 │ │ │ │ +000142c0: 207d 3b20 203c 7370 616e 2063 6c61 7373 }; // Ch │ │ │ │ +000142e0: 6f69 6365 3c2f 7370 616e 3e3c 2f64 6976 oice
    .
    457< │ │ │ │ +00014340: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00014360: 3c61 2069 643d 226c 3030 3435 3822 206e 458 /***** │ │ │ │ +000143c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000143d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000143e0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000143f0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014400: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ +00014410: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00014460: 3435 393c 2f73 7061 6e3e 2020 3c73 7061 459 // DecisionTre │ │ │ │ +00014490: 653c 2f73 7061 6e3e 3c2f 6469 763e 0a3c e
    .< │ │ │ │ +000144a0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000144b0: 3e3c 6120 6964 3d22 6c30 3034 3630 2220 > 460 /**** │ │ │ │ +00014510: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014520: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014530: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014540: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014550: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ +00014560: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000145b0: 2034 3631 3c2f 7370 616e 3e20 203c 7370 461 template<type │ │ │ │ +00014600: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ +00014610: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00014620: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ +00014640: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +000146d0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +000146e0: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +000146f0: 2361 6632 3930 6664 6232 3962 6232 3534 #af290fdb29bb254 │ │ │ │ +00014700: 6535 3162 6661 3239 3532 3038 3635 3562 e51bfa295208655b │ │ │ │ +00014710: 6366 223e 2020 3436 323c 2f61 3e3c 2f73 cf"> 462 DecisionT │ │ │ │ +00014780: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ +00014790: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree() {
    .
    < │ │ │ │ +000147e0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000147f0: 6e6f 223e 2020 3436 333c 2f73 7061 6e3e no"> 463 │ │ │ │ +00014800: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +00014810: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00014880: 6120 6964 3d22 6c30 3034 3635 2220 6e61 a id="l00465" na │ │ │ │ +00014890: 6d65 3d22 6c30 3034 3635 223e 3c2f 613e me="l00465"> │ │ │ │ +000148a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 465 templat │ │ │ │ +000148e0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ +00014930: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ +00014940: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00014990: 2034 3636 3c2f 7370 616e 3e20 203c 6120 466 De │ │ │ │ +000149f0: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00014a00: 2059 2667 743b 3a3a 4465 6369 7369 6f6e Y>::Decision │ │ │ │ +00014a10: 5472 6565 3c2f 613e 283c 7370 616e 2063 Tree(c │ │ │ │ +00014a30: 6f6e 7374 3c2f 7370 616e 3e20 4e6f 6465 onst Node │ │ │ │ +00014a40: 5074 7226 616d 703b 2072 6f6f 7429 203a Ptr& root) : │ │ │ │ +00014a50: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00014aa0: 2034 3637 3c2f 7370 616e 3e20 2020 2072 467 r │ │ │ │ +00014ab0: 6f6f 745f 2872 6f6f 7429 207b 3c2f 6469 oot_(root) {.
    468 │ │ │ │ +00014b10: 3c2f 7370 616e 3e20 207d 3c2f 6469 763e }
    │ │ │ │ +00014b20: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00014b90: 6120 6964 3d22 6c30 3034 3730 2220 6e61 a id="l00470" na │ │ │ │ +00014ba0: 6d65 3d22 6c30 3034 3730 223e 3c2f 613e me="l00470"> │ │ │ │ +00014bb0: 3c73 7061 6e20 636c 6173 733d 226c 696e 470 /****** │ │ │ │ +00014bf0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014c00: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014c10: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014c20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00014c30: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    4 │ │ │ │ +00014c90: 3731 3c2f 7370 616e 3e20 203c 7370 616e 71 template │ │ │ │ +00014cc0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ +00014ce0: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ +00014d20: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ +00014d30: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ +00014d40: 6e30 3034 3732 2220 6461 7461 2d73 7461 n00472" data-sta │ │ │ │ +00014d50: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ +00014d60: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    472 DecisionTre │ │ │ │ +00014e60: 6526 6c74 3b4c 2c20 5926 6774 3b3a 3a44 e<L, Y>::D │ │ │ │ +00014e70: 6563 6973 696f 6e54 7265 653c 2f61 3e28 ecisionTree( │ │ │ │ +00014e80: 3c73 7061 6e20 636c 6173 733d 226b 6579 const Y& y) {< │ │ │ │ +00014eb0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00014f00: 3437 333c 2f73 7061 6e3e 2020 2020 726f 473 ro │ │ │ │ +00014f10: 6f74 5f20 3d20 3c61 2063 6c61 7373 3d22 ot_ = NodePtr(new │ │ │ │ +00014fc0: 4c65 6166 3c2f 613e 2879 2929 3b3c 2f64 Leaf(y));.
    47 │ │ │ │ +00015020: 343c 2f73 7061 6e3e 2020 7d3c 2f64 6976 4 }
    .
    .
    475 .
    4 │ │ │ │ +000150e0: 3736 3c2f 7370 616e 3e3c 7370 616e 2063 76 │ │ │ │ +00015100: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ +00015110: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015120: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015130: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015140: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ +00015150: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00015170: 6120 6964 3d22 6c30 3034 3737 2220 6e61 a id="l00477" na │ │ │ │ +00015180: 6d65 3d22 6c30 3034 3737 223e 3c2f 613e me="l00477"> │ │ │ │ +00015190: 3c73 7061 6e20 636c 6173 733d 226c 696e 477 templat │ │ │ │ +000151d0: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename
    L, type │ │ │ │ +00015220: 6e61 6d65 3c2f 7370 616e 3e20 5926 6774 name Y> │ │ │ │ +00015230: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    . │ │ │ │ +00015290: 3c61 2069 643d 226c 3030 3437 3822 206e 478 │ │ │ │ +00015310: 3c2f 613e 3c2f 7370 616e 3e20 203c 6120 De │ │ │ │ +00015370: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00015380: 2059 2667 743b 3a3a 4465 6369 7369 6f6e Y>::Decision │ │ │ │ +00015390: 5472 6565 3c2f 613e 283c 7370 616e 2063 Tree(c │ │ │ │ +000153b0: 6f6e 7374 3c2f 7370 616e 3e20 4c26 616d onst L&am │ │ │ │ +000153c0: 703b 206c 6162 656c 2c20 3c73 7061 6e20 p; label, │ │ │ │ +000153e0: 636f 6e73 743c 2f73 7061 6e3e 2059 2661 const Y&a │ │ │ │ +000153f0: 6d70 3b20 7931 2c20 3c73 7061 6e20 636c mp; y1, co │ │ │ │ +00015410: 6e73 743c 2f73 7061 6e3e 2059 2661 6d70 nst Y& │ │ │ │ +00015420: 3b20 7932 2920 7b3c 2f64 6976 3e0a 3c64 ; y2) {
    . │ │ │ │ +00015440: 3c61 2069 643d 226c 3030 3437 3922 206e 479 auto │ │ │ │ +000154a0: 3c2f 7370 616e 3e20 6120 3d20 626f 6f73 a = boos │ │ │ │ +000154b0: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ +000154c0: 743b 4368 6f69 6365 2667 743b 286c 6162 t;Choice>(lab │ │ │ │ +000154d0: 656c 2c20 3229 3b3c 2f64 6976 3e0a 3c64 el, 2);
    . │ │ │ │ +000154f0: 3c61 2069 643d 226c 3030 3438 3022 206e 480 NodePtr l1(new Leaf(y1)) │ │ │ │ +000155f0: 2c20 6c32 283c 7370 616e 2063 6c61 7373 , l2(new Leaf(y2)) │ │ │ │ +00015650: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +000156a0: 2020 3438 313c 2f73 7061 6e3e 2020 2020 481 │ │ │ │ +000156b0: 612d 2667 743b 7075 7368 5f62 6163 6b28 a->push_back( │ │ │ │ +000156c0: 6c31 293b 3c2f 6469 763e 0a3c 6469 7620 l1);
    .
    482 │ │ │ │ +00015720: 2020 2061 2d26 6774 3b70 7573 685f 6261 a->push_ba │ │ │ │ +00015730: 636b 286c 3229 3b3c 2f64 6976 3e0a 3c64 ck(l2);
    . │ │ │ │ +00015750: 3c61 2069 643d 226c 3030 3438 3322 206e 483 root_ = Ch │ │ │ │ +000157a0: 6f69 6365 3a3a 556e 6971 7565 2861 293b oice::Unique(a); │ │ │ │ +000157b0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00015800: 2034 3834 3c2f 7370 616e 3e20 207d 3c2f 484 }.
    .
    < │ │ │ │ +00015850: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00015860: 6e6f 223e 2020 3438 353c 2f73 7061 6e3e no"> 485 │ │ │ │ +00015870: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    │ │ │ │ +000158c0: 2020 3438 363c 2f73 7061 6e3e 3c73 7061 486 /*********** │ │ │ │ +000158f0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015900: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015910: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015920: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00015930: 2a2f 3c2f 7370 616e 3e3c 2f64 6976 3e0a */
    . │ │ │ │ +00015940: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +00015a60: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00015a90: 3c2f 613e 3c73 7061 6e20 636c 6173 733d │ │ │ │ +00015af0: 3438 383c 2f61 3e3c 2f73 7061 6e3e 2020 488 │ │ │ │ +00015b00: 3c61 2063 6c61 7373 3d22 636f 6465 2068 DecisionTree< │ │ │ │ +00015b60: 3b4c 2c20 5926 6774 3b3a 3a44 6563 6973 ;L, Y>::Decis │ │ │ │ +00015b70: 696f 6e54 7265 653c 2f61 3e28 3c73 7061 ionTree(const < │ │ │ │ +00015ba0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00015bb0: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ +00015bc0: 6130 3237 3936 2e68 746d 6c23 6164 6364 a02796.html#adcd │ │ │ │ +00015bd0: 6438 3061 6231 6166 3431 3564 3231 3233 d80ab1af415d2123 │ │ │ │ +00015be0: 3937 6132 3636 3633 3230 6533 3522 3e4c 97a2666320e35">L │ │ │ │ +00015bf0: 6162 656c 433c 2f61 3e26 616d 703b 206c abelC& l │ │ │ │ +00015c00: 6162 656c 432c 203c 7370 616e 2063 6c61 abelC, con │ │ │ │ +00015c20: 7374 3c2f 7370 616e 3e20 5926 616d 703b st Y& │ │ │ │ +00015c30: 2079 312c 3c2f 6469 763e 0a3c 6469 7620 y1,
    .
    489 │ │ │ │ +00015c90: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00015ca0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00015cb0: 2020 3c73 7061 6e20 636c 6173 733d 226b const Y& y2) │ │ │ │ +00015ce0: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +00015d30: 2020 3439 303c 2f73 7061 6e3e 2020 2020 490 │ │ │ │ +00015d40: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (labelC.seco │ │ │ │ +00015d70: 6e64 2021 3d20 3229 203c 7370 616e 2063 nd != 2) throw │ │ │ │ +00015da0: 7374 643a 3a69 6e76 616c 6964 5f61 7267 std::invalid_arg │ │ │ │ +00015db0: 756d 656e 7428 3c2f 6469 763e 0a3c 6469 ument(
    .< │ │ │ │ +00015dd0: 6120 6964 3d22 6c30 3034 3931 2220 6e61 a id="l00491" na │ │ │ │ +00015de0: 6d65 3d22 6c30 3034 3931 223e 3c2f 613e me="l00491"> │ │ │ │ +00015df0: 3c73 7061 6e20 636c 6173 733d 226c 696e 491 "Decis │ │ │ │ +00015e40: 696f 6e54 7265 653a 2062 696e 6172 7920 ionTree: binary │ │ │ │ +00015e50: 636f 6e73 7472 7563 746f 7220 6361 6c6c constructor call │ │ │ │ +00015e60: 6564 2077 6974 6820 6e6f 6e2d 6269 6e61 ed with non-bina │ │ │ │ +00015e70: 7279 206c 6162 656c 2671 756f 743b 3c2f ry label");
    . │ │ │ │ +00015ea0: 3c61 2069 643d 226c 3030 3439 3222 206e 492 auto │ │ │ │ +00015f00: 3c2f 7370 616e 3e20 6120 3d20 626f 6f73 a = boos │ │ │ │ +00015f10: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ +00015f20: 743b 4368 6f69 6365 2667 743b 286c 6162 t;Choice>(lab │ │ │ │ +00015f30: 656c 432e 6669 7273 742c 2032 293b 3c2f elC.first, 2);.
    4 │ │ │ │ +00015f90: 3933 3c2f 7370 616e 3e20 2020 203c 6120 93 Nod │ │ │ │ +00015ff0: 6550 7472 3c2f 613e 206c 3128 3c73 7061 ePtr l1(new Leaf(y1)), l2(new Leaf(y2));
    .< │ │ │ │ +000160c0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000160d0: 3e3c 6120 6964 3d22 6c30 3034 3934 2220 > 494 a->pus │ │ │ │ +00016120: 685f 6261 636b 286c 3129 3b3c 2f64 6976 h_back(l1);
    .
    495< │ │ │ │ +00016180: 2f73 7061 6e3e 2020 2020 612d 2667 743b /span> a-> │ │ │ │ +00016190: 7075 7368 5f62 6163 6b28 6c32 293b 3c2f push_back(l2);.
    4 │ │ │ │ +000161f0: 3936 3c2f 7370 616e 3e20 2020 2072 6f6f 96 roo │ │ │ │ +00016200: 745f 203d 2043 686f 6963 653a 3a55 6e69 t_ = Choice::Uni │ │ │ │ +00016210: 7175 6528 6129 3b3c 2f64 6976 3e0a 3c64 que(a);
    . │ │ │ │ +00016230: 3c61 2069 643d 226c 3030 3439 3722 206e 497 }
    ..
    498 │ │ │ │ +000162d0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +000162e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000162f0: 3e3c 6120 6964 3d22 6c30 3034 3939 2220 > 499 /**** │ │ │ │ +00016350: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016360: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016370: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016380: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016390: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ +000163a0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000163f0: 2035 3030 3c2f 7370 616e 3e20 203c 7370 500 template<type │ │ │ │ +00016440: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ +00016450: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00016460: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ +00016480: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +00016510: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00016520: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +00016530: 2361 6437 3534 6438 3639 3963 6232 3035 #ad754d8699cb205 │ │ │ │ +00016540: 3738 3866 3838 3966 3031 3237 3831 6264 788f889f012781bd │ │ │ │ +00016550: 6332 223e 2020 3530 313c 2f61 3e3c 2f73 c2"> 501 DecisionT │ │ │ │ +000165c0: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ +000165d0: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree(const std::vector │ │ │ │ +00016610: 266c 743b 4c61 6265 6c43 2667 743b 2661 <LabelC>&a │ │ │ │ +00016620: 6d70 3b20 6c61 6265 6c43 732c 3c2f 6469 mp; labelCs,.
    502 │ │ │ │ +00016680: 3c2f 7370 616e 3e20 2020 2020 203c 7370 const │ │ │ │ +000166b0: 7374 643a 3a76 6563 746f 7226 6c74 3b59 std::vector<Y │ │ │ │ +000166c0: 2667 743b 2661 6d70 3b20 7973 2920 7b3c >& ys) {< │ │ │ │ +000166d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00016720: 3530 333c 2f73 7061 6e3e 2020 2020 3c73 503 // call recu │ │ │ │ +00016750: 7273 6976 6520 4372 6561 7465 3c2f 7370 rsive Create
    .
    504 │ │ │ │ +000167c0: 2020 726f 6f74 5f20 3d20 6372 6561 7465 root_ = create │ │ │ │ +000167d0: 286c 6162 656c 4373 2e62 6567 696e 2829 (labelCs.begin() │ │ │ │ +000167e0: 2c20 6c61 6265 6c43 732e 656e 6428 292c , labelCs.end(), │ │ │ │ +000167f0: 2079 732e 6265 6769 6e28 292c 2079 732e ys.begin(), ys. │ │ │ │ +00016800: 656e 6428 2929 3b3c 2f64 6976 3e0a 3c64 end());
    . │ │ │ │ +00016820: 3c61 2069 643d 226c 3030 3530 3522 206e 505 }
    ..
    506 │ │ │ │ +000168c0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +000168d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000168e0: 3e3c 6120 6964 3d22 6c30 3035 3037 2220 > 507 /**** │ │ │ │ +00016940: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016950: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016960: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016970: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00016980: 2a2a 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e ********/ │ │ │ │ +00016990: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000169e0: 2035 3038 3c2f 7370 616e 3e20 203c 7370 508 template<type │ │ │ │ +00016a30: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ +00016a40: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00016a50: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ +00016a70: 0a3c 6469 7620 636c 6173 733d 2266 6f6c .
    .
    < │ │ │ │ +00016b00: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00016b10: 7265 663d 2261 3032 3739 362e 6874 6d6c ref="a02796.html │ │ │ │ +00016b20: 2361 3361 3961 3462 3432 3039 3831 3266 #a3a9a4b4209812f │ │ │ │ +00016b30: 3037 3163 3132 3235 6439 3534 3266 6134 071c1225d9542fa4 │ │ │ │ +00016b40: 3433 223e 2020 3530 393c 2f61 3e3c 2f73 43"> 509 DecisionT │ │ │ │ +00016bb0: 7265 6526 6c74 3b4c 2c20 5926 6774 3b3a ree<L, Y>: │ │ │ │ +00016bc0: 3a44 6563 6973 696f 6e54 7265 653c 2f61 :DecisionTree(const std::vector │ │ │ │ +00016c00: 266c 743b 4c61 6265 6c43 2667 743b 2661 <LabelC>&a │ │ │ │ +00016c10: 6d70 3b20 6c61 6265 6c43 732c 3c2f 6469 mp; labelCs,.
    510 │ │ │ │ +00016c70: 3c2f 7370 616e 3e20 2020 2020 203c 7370 const │ │ │ │ +00016ca0: 7374 643a 3a73 7472 696e 6726 616d 703b std::string& │ │ │ │ +00016cb0: 2074 6162 6c65 2920 7b3c 2f64 6976 3e0a table) {
    . │ │ │ │ +00016cc0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00016cf0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 511 // │ │ │ │ +00016d30: 2043 6f6e 7665 7274 2073 7464 3a3a 7374 Convert std::st │ │ │ │ +00016d40: 7269 6e67 2074 6f20 7661 6c75 6573 206f ring to values o │ │ │ │ +00016d50: 6620 7479 7065 2059 3c2f 7370 616e 3e3c f type Y< │ │ │ │ +00016d60: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00016db0: 3531 323c 2f73 7061 6e3e 2020 2020 7374 512 st │ │ │ │ +00016dc0: 643a 3a76 6563 746f 7226 6c74 3b59 2667 d::vector<Y&g │ │ │ │ +00016dd0: 743b 2079 733b 3c2f 6469 763e 0a3c 6469 t; ys;
    .< │ │ │ │ +00016df0: 6120 6964 3d22 6c30 3035 3133 2220 6e61 a id="l00513" na │ │ │ │ +00016e00: 6d65 3d22 6c30 3035 3133 223e 3c2f 613e me="l00513"> │ │ │ │ +00016e10: 3c73 7061 6e20 636c 6173 733d 226c 696e 513 std::istrin │ │ │ │ +00016e40: 6773 7472 6561 6d20 6973 7328 7461 626c gstream iss(tabl │ │ │ │ +00016e50: 6529 3b3c 2f64 6976 3e0a 3c64 6976 2063 e);
    .
    514 │ │ │ │ +00016eb0: 2020 636f 7079 2873 7464 3a3a 6973 7472 copy(std::istr │ │ │ │ +00016ec0: 6561 6d5f 6974 6572 6174 6f72 266c 743b eam_iterator< │ │ │ │ +00016ed0: 5926 6774 3b28 6973 7329 2c20 7374 643a Y>(iss), std: │ │ │ │ +00016ee0: 3a69 7374 7265 616d 5f69 7465 7261 746f :istream_iterato │ │ │ │ +00016ef0: 7226 6c74 3b59 2667 743b 2829 2c3c 2f64 r<Y>(),.
    51 │ │ │ │ +00016f50: 353c 2f73 7061 6e3e 2020 2020 2020 2020 5 │ │ │ │ +00016f60: 2062 6163 6b5f 696e 7365 7274 6572 2879 back_inserter(y │ │ │ │ +00016f70: 7329 293b 3c2f 6469 763e 0a3c 6469 7620 s));
    .
    516 │ │ │ │ +00016fd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00017020: 2035 3137 3c2f 7370 616e 3e20 2020 203c 517 < │ │ │ │ +00017030: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +00017040: 656e 7422 3e2f 2f20 6e6f 7720 6361 6c6c ent">// now call │ │ │ │ +00017050: 2072 6563 7572 7369 7665 2043 7265 6174 recursive Creat │ │ │ │ +00017060: 653c 2f73 7061 6e3e 3c2f 6469 763e 0a3c e
    .< │ │ │ │ +00017070: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00017080: 3e3c 6120 6964 3d22 6c30 3035 3138 2220 > 518
    root_ = c │ │ │ │ +000170d0: 7265 6174 6528 6c61 6265 6c43 732e 6265 reate(labelCs.be │ │ │ │ +000170e0: 6769 6e28 292c 206c 6162 656c 4373 2e65 gin(), labelCs.e │ │ │ │ +000170f0: 6e64 2829 2c20 7973 2e62 6567 696e 2829 nd(), ys.begin() │ │ │ │ +00017100: 2c20 7973 2e65 6e64 2829 293b 3c2f 6469 , ys.end());.
    519 │ │ │ │ +00017160: 3c2f 7370 616e 3e20 207d 3c2f 6469 763e }
    │ │ │ │ +00017170: 0a3c 2f64 6976 3e0a 3c64 6976 2063 6c61 .
    .
    │ │ │ │ +000171c0: 2020 3532 303c 2f73 7061 6e3e 203c 2f64 520 .
    52 │ │ │ │ +00017220: 313c 2f73 7061 6e3e 3c73 7061 6e20 636c 1 │ │ │ │ +00017240: 2f2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /*************** │ │ │ │ +00017250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017260: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2f 3c2f *************/
    .
    < │ │ │ │ +000172d0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000172e0: 6e6f 223e 2020 3532 323c 2f73 7061 6e3e no"> 522 │ │ │ │ +000172f0: 2020 3c73 7061 6e20 636c 6173 733d 226b template │ │ │ │ +00017310: 3c2f 7370 616e 3e26 6c74 3b3c 7370 616e <typename │ │ │ │ +00017340: 204c 2c20 3c73 7061 6e20 636c 6173 733d L, typena │ │ │ │ +00017360: 6d65 3c2f 7370 616e 3e20 5926 6774 3b3c me Y>< │ │ │ │ +00017370: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    ..
    524 │ │ │ │ +00017590: 2020 2020 4974 6572 6174 6f72 2062 6567 Iterator beg │ │ │ │ +000175a0: 696e 2c20 4974 6572 6174 6f72 2065 6e64 in, Iterator end │ │ │ │ +000175b0: 2c20 3c73 7061 6e20 636c 6173 733d 226b , const L& labe │ │ │ │ +000175e0: 6c29 207b 3c2f 6469 763e 0a3c 6469 7620 l) {
    .
    525 │ │ │ │ +00017640: 2020 2072 6f6f 745f 203d 2063 6f6d 706f root_ = compo │ │ │ │ +00017650: 7365 2862 6567 696e 2c20 656e 642c 206c se(begin, end, l │ │ │ │ +00017660: 6162 656c 293b 3c2f 6469 763e 0a3c 6469 abel);
    .< │ │ │ │ +00017680: 6120 6964 3d22 6c30 3035 3236 2220 6e61 a id="l00526" na │ │ │ │ +00017690: 6d65 3d22 6c30 3035 3236 223e 3c2f 613e me="l00526"> │ │ │ │ +000176a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 526 }
    .
    .
    527< │ │ │ │ +00017720: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00017740: 3c61 2069 643d 226c 3030 3532 3822 206e 528 /***** │ │ │ │ +000177a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000177b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000177c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000177d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000177e0: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ +000177f0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00017840: 3532 393c 2f73 7061 6e3e 2020 3c73 7061 529 template<typen │ │ │ │ +00017890: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ +000178d0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    530 DecisionTr │ │ │ │ +00017a10: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ +00017a20: 4465 6369 7369 6f6e 5472 6565 3c2f 613e DecisionTree │ │ │ │ +00017a30: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const L& label │ │ │ │ +00017a60: 2c3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ,
    .
    │ │ │ │ +00017ab0: 2020 3533 313c 2f73 7061 6e3e 2020 2020 531 │ │ │ │ +00017ac0: 2020 3c73 7061 6e20 636c 6173 733d 226b const DecisionTree& f0, const Decisi │ │ │ │ +00017b80: 6f6e 5472 6565 3c2f 613e 2661 6d70 3b20 onTree& │ │ │ │ +00017b90: 6631 2920 207b 3c2f 6469 763e 0a3c 6469 f1) {
    .< │ │ │ │ +00017bb0: 6120 6964 3d22 6c30 3035 3332 2220 6e61 a id="l00532" na │ │ │ │ +00017bc0: 6d65 3d22 6c30 3035 3332 223e 3c2f 613e me="l00532"> │ │ │ │ +00017bd0: 3c73 7061 6e20 636c 6173 733d 226c 696e 532 const │ │ │ │ +00017c10: 3c2f 7370 616e 3e20 7374 643a 3a76 6563 std::vec │ │ │ │ +00017c20: 746f 7226 6c74 3b44 6563 6973 696f 6e54 tor<DecisionT │ │ │ │ +00017c30: 7265 6526 6774 3b20 6675 6e63 7469 6f6e ree> function │ │ │ │ +00017c40: 737b 6630 2c20 6631 7d3b 3c2f 6469 763e s{f0, f1};
    │ │ │ │ +00017c50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00017c80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 533 root_ = │ │ │ │ +00017cb0: 2063 6f6d 706f 7365 2866 756e 6374 696f compose(functio │ │ │ │ +00017cc0: 6e73 2e62 6567 696e 2829 2c20 6675 6e63 ns.begin(), func │ │ │ │ +00017cd0: 7469 6f6e 732e 656e 6428 292c 206c 6162 tions.end(), lab │ │ │ │ +00017ce0: 656c 293b 3c2f 6469 763e 0a3c 6469 7620 el);
    .
    534 │ │ │ │ +00017d40: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ +00017d50: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ +00017de0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00017df0: 6e6f 223e 2020 3533 363c 2f73 7061 6e3e no"> 536 │ │ │ │ +00017e00: 3c73 7061 6e20 636c 6173 733d 2263 6f6d /******* │ │ │ │ +00017e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017e40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017e50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00017e60: 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c 2f64 *****/.
    53 │ │ │ │ +00017ec0: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ +00017ee0: 7465 6d70 6c61 7465 3c2f 7370 616e 3e20 template │ │ │ │ +00017ef0: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ +00017f10: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ +00017f50: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00017f60: 3e3c 6120 6964 3d22 6c30 3035 3338 2220 > 538 templ │ │ │ │ +00017fc0: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ +00017fd0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00017fe0: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    X, ty │ │ │ │ +00018010: 7065 6e61 6d65 3c2f 7370 616e 3e20 4675 pename Fu │ │ │ │ +00018020: 6e63 2667 743b 3c2f 6469 763e 0a3c 6469 nc>
    ...
    < │ │ │ │ +00018240: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00018250: 6e6f 223e 2020 3534 303c 2f73 7061 6e3e no"> 540 │ │ │ │ +00018260: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00018270: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00018280: 2020 2046 756e 6320 595f 6f66 5f58 2920 Func Y_of_X) │ │ │ │ +00018290: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    .
    │ │ │ │ +000182e0: 2020 3534 313c 2f73 7061 6e3e 2020 2020 541 │ │ │ │ +000182f0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // Define │ │ │ │ +00018310: 6675 6e63 746f 7220 666f 7220 6964 656e functor for iden │ │ │ │ +00018320: 7469 7479 206d 6170 7069 6e67 206f 6620 tity mapping of │ │ │ │ +00018330: 6e6f 6465 206c 6162 656c 2e3c 2f73 7061 node label.
    .
    542 │ │ │ │ +000183a0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto L_of_L = [](< │ │ │ │ +000183d0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000183e0: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const L& label) │ │ │ │ +00018400: 7b20 3c73 7061 6e20 636c 6173 733d 226b { retu │ │ │ │ +00018420: 726e 3c2f 7370 616e 3e20 6c61 6265 6c3b rn label; │ │ │ │ +00018430: 207d 3b3c 2f64 6976 3e0a 3c64 6976 2063 };
    .
    543 │ │ │ │ +00018490: 2020 726f 6f74 5f20 3d20 636f 6e76 6572 root_ = conver │ │ │ │ +000184a0: 7446 726f 6d26 6c74 3b4c 2c20 5826 6774 tFrom<L, X> │ │ │ │ +000184b0: 3b28 6f74 6865 722e 3c61 2063 6c61 7373 ;(other.root_, L_of_L, Y_of │ │ │ │ +00018520: 5f58 293b 3c2f 6469 763e 0a3c 6469 7620 _X);
    .
    544 │ │ │ │ +00018580: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ +00018590: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ +00018620: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00018630: 6e6f 223e 2020 3534 363c 2f73 7061 6e3e no"> 546 │ │ │ │ +00018640: 3c73 7061 6e20 636c 6173 733d 2263 6f6d /******* │ │ │ │ +00018660: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018670: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018680: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018690: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000186a0: 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c 2f64 *****/.
    54 │ │ │ │ +00018700: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ +00018720: 7465 6d70 6c61 7465 3c2f 7370 616e 3e20 template │ │ │ │ +00018730: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ +00018750: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ +00018790: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000187a0: 3e3c 6120 6964 3d22 6c30 3035 3438 2220 > 548 templ │ │ │ │ +00018800: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ +00018810: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00018820: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    M, ty │ │ │ │ +00018850: 7065 6e61 6d65 3c2f 7370 616e 3e20 582c pename X, │ │ │ │ +00018860: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ +00018880: 2f73 7061 6e3e 2046 756e 6326 6774 3b3c /span> Func>< │ │ │ │ +00018890: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ +00018910: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00018920: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 549 Deci │ │ │ │ +000189d0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ +000189e0: 2667 743b 3a3a 4465 6369 7369 6f6e 5472 >::DecisionTr │ │ │ │ +000189f0: 6565 3c2f 613e 283c 7370 616e 2063 6c61 ee(con │ │ │ │ +00018a10: 7374 3c2f 7370 616e 3e20 3c61 2063 6c61 st DecisionTr │ │ │ │ +00018a50: 6565 266c 743b 4d2c 2058 2667 743b 3c2f ee<M, X>& other,.
    5 │ │ │ │ +00018ac0: 3530 3c2f 7370 616e 3e20 2020 2020 2020 50 │ │ │ │ +00018ad0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00018ae0: 2020 2020 2020 2020 2020 2020 3c73 7061 const s │ │ │ │ +00018b10: 7464 3a3a 6d61 7026 6c74 3b4d 2c20 4c26 td::map<M, L& │ │ │ │ +00018b20: 6774 3b26 616d 703b 206d 6170 2c20 4675 gt;& map, Fu │ │ │ │ +00018b30: 6e63 2059 5f6f 665f 5829 207b 3c2f 6469 nc Y_of_X) {.
    551 │ │ │ │ +00018b90: 3c2f 7370 616e 3e20 2020 203c 7370 616e auto L_o │ │ │ │ +00018bc0: 665f 4d20 3d20 5b26 616d 703b 6d61 705d f_M = [&map] │ │ │ │ +00018bd0: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const M& label │ │ │ │ +00018c00: 2920 2d26 6774 3b20 4c20 7b20 3c73 7061 ) -> L { return map.at(label │ │ │ │ +00018c40: 293b 207d 3b3c 2f64 6976 3e0a 3c64 6976 ); };
    .
    < │ │ │ │ +00018c80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00018c90: 6e6f 223e 2020 3535 323c 2f73 7061 6e3e no"> 552 │ │ │ │ +00018ca0: 2020 2020 726f 6f74 5f20 3d20 636f 6e76 root_ = conv │ │ │ │ +00018cb0: 6572 7446 726f 6d26 6c74 3b4d 2c20 5826 ertFrom<M, X& │ │ │ │ +00018cc0: 6774 3b28 6f74 6865 722e 3c61 2063 6c61 gt;(other.root_ │ │ │ │ +00018d20: 3c2f 613e 2c20 4c5f 6f66 5f4d 2c20 595f , L_of_M, Y_ │ │ │ │ +00018d30: 6f66 5f58 293b 3c2f 6469 763e 0a3c 6469 of_X);
    .< │ │ │ │ +00018d50: 6120 6964 3d22 6c30 3035 3533 2220 6e61 a id="l00553" na │ │ │ │ +00018d60: 6d65 3d22 6c30 3035 3533 223e 3c2f 613e me="l00553"> │ │ │ │ +00018d70: 3c73 7061 6e20 636c 6173 733d 226c 696e 553 }
    .
    .
    554< │ │ │ │ +00018df0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +00018e10: 3c61 2069 643d 226c 3030 3535 3522 206e 555 /***** │ │ │ │ +00018e70: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018e80: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018e90: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018ea0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00018eb0: 2a2a 2a2a 2a2a 2a2f 3c2f 7370 616e 3e3c *******/< │ │ │ │ +00018ec0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00018f10: 3535 363c 2f73 7061 6e3e 2020 3c73 7061 556 // Called by t │ │ │ │ +00018f40: 776f 2063 6f6e 7374 7275 6374 6f72 7320 wo constructors │ │ │ │ +00018f50: 6162 6f76 652e 3c2f 7370 616e 3e3c 2f64 above..
    55 │ │ │ │ +00018fb0: 373c 2f73 7061 6e3e 2020 3c73 7061 6e20 7 │ │ │ │ +00018fd0: 2f2f 2054 616b 6573 2061 206c 6162 656c // Takes a label │ │ │ │ +00018fe0: 2061 6e64 2061 2063 6f72 7265 7370 6f6e and a correspon │ │ │ │ +00018ff0: 6469 6e67 2072 616e 6765 206f 6620 6465 ding range of de │ │ │ │ +00019000: 6369 7369 6f6e 2074 7265 6573 2c20 616e cision trees, an │ │ │ │ +00019010: 6420 6372 6561 7465 7320 613c 2f73 7061 d creates a
    .
    558 < │ │ │ │ +00019080: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +00019090: 656e 7422 3e2f 2f20 6e65 7720 6465 6369 ent">// new deci │ │ │ │ +000190a0: 7369 6f6e 2074 7265 652e 2048 6f77 6576 sion tree. Howev │ │ │ │ +000190b0: 6572 2c20 7468 6520 6f72 6465 7220 6f66 er, the order of │ │ │ │ +000190c0: 2074 6865 206c 6162 656c 7320 6e65 6564 the labels need │ │ │ │ +000190d0: 7320 746f 2062 6520 7265 7370 6563 7465 s to be respecte │ │ │ │ +000190e0: 642c 3c2f 7370 616e 3e3c 2f64 6976 3e0a d,
    . │ │ │ │ +000190f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00019120: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 559 // s │ │ │ │ +00019160: 6f20 7765 2063 616e 6e6f 7420 6a75 7374 o we cannot just │ │ │ │ +00019170: 2063 7265 6174 6520 6120 726f 6f74 2043 create a root C │ │ │ │ +00019180: 686f 6963 6520 6e6f 6465 206f 6e20 7468 hoice node on th │ │ │ │ +00019190: 6520 6c61 6265 6c3a 2069 6620 7468 6520 e label: if the │ │ │ │ +000191a0: 6c61 6265 6c20 6973 3c2f 7370 616e 3e3c label is< │ │ │ │ +000191b0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00019200: 3536 303c 2f73 7061 6e3e 2020 3c73 7061 560 // not the hig │ │ │ │ +00019230: 6865 7374 206c 6162 656c 2c20 7765 206e hest label, we n │ │ │ │ +00019240: 6565 6420 6120 636f 6d70 6c69 6361 7465 eed a complicate │ │ │ │ +00019250: 642f 2065 7870 656e 7369 7665 2072 6563 d/ expensive rec │ │ │ │ +00019260: 7572 7369 7665 2063 616c 6c2e 3c2f 7370 ursive call.
    .
    561 │ │ │ │ +000192d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 template < │ │ │ │ +00019310: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +00019320: 4c2c 203c 7370 616e 2063 6c61 7373 3d22 L, typenam │ │ │ │ +00019340: 653c 2f73 7061 6e3e 2059 2667 743b 3c2f e Y>.
    5 │ │ │ │ +000193a0: 3632 3c2f 7370 616e 3e20 203c 7370 616e 62 template │ │ │ │ +000193d0: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ +000193f0: 616d 653c 2f73 7061 6e3e 2049 7465 7261 ame Itera │ │ │ │ +00019400: 746f 7226 6774 3b3c 2f64 6976 3e0a 3c64 tor>
    . │ │ │ │ +00019420: 3c61 2069 643d 226c 3030 3536 3322 206e 563 typena │ │ │ │ +00019480: 6d65 3c2f 7370 616e 3e20 3c61 2063 6c61 me Decisi │ │ │ │ +000194e0: 6f6e 5472 6565 266c 743b 4c2c 2059 2667 onTree<L, Y&g │ │ │ │ +000194f0: 743b 3a3a 4e6f 6465 5074 723c 2f61 3e20 t;::NodePtr │ │ │ │ +00019500: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Deci │ │ │ │ +00019530: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ +00019540: 2667 743b 3a3a 636f 6d70 6f73 653c 2f61 >::compose(
    .
    564 │ │ │ │ +000195b0: 2020 2049 7465 7261 746f 7220 6265 6769 Iterator begi │ │ │ │ +000195c0: 6e2c 2049 7465 7261 746f 7220 656e 642c n, Iterator end, │ │ │ │ +000195d0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 const L& label │ │ │ │ +00019600: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ +00019640: 6120 6964 3d22 6c30 3035 3635 2220 6e61 a id="l00565" na │ │ │ │ +00019650: 6d65 3d22 6c30 3035 3635 223e 3c2f 613e me="l00565"> │ │ │ │ +00019660: 3c73 7061 6e20 636c 6173 733d 226c 696e 565 // fi │ │ │ │ +000196a0: 6e64 2068 6967 6865 7374 206c 6162 656c nd highest label │ │ │ │ +000196b0: 2061 6d6f 6e67 2062 7261 6e63 6865 733c among branches< │ │ │ │ +000196c0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +000196e0: 6120 6964 3d22 6c30 3035 3636 2220 6e61 a id="l00566" na │ │ │ │ +000196f0: 6d65 3d22 6c30 3035 3636 223e 3c2f 613e me="l00566"> │ │ │ │ +00019700: 3c73 7061 6e20 636c 6173 733d 226c 696e 566 boost::opti │ │ │ │ +00019730: 6f6e 616c 266c 743b 4c26 6774 3b20 6869 onal<L> hi │ │ │ │ +00019740: 6768 6573 744c 6162 656c 3b3c 2f64 6976 ghestLabel;
    .
    567< │ │ │ │ +000197a0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> size_t nrChoices = 0; │ │ │ │ +000197e0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00019830: 2035 3638 3c2f 7370 616e 3e20 2020 203c 568 < │ │ │ │ +00019840: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00019850: 6f72 6466 6c6f 7722 3e66 6f72 3c2f 7370 ordflow">for (Iterator it │ │ │ │ +00019870: 203d 2062 6567 696e 3b20 6974 2021 3d20 = begin; it != │ │ │ │ +00019880: 656e 643b 2069 742b 2b29 207b 3c2f 6469 end; it++) {.
    569 │ │ │ │ +000198e0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 if │ │ │ │ +00019910: 2028 6974 2d26 6774 3b72 6f6f 745f 2d26 (it->root_-& │ │ │ │ +00019920: 6774 3b69 734c 6561 6628 2929 3c2f 6469 gt;isLeaf()).
    570 │ │ │ │ +00019980: 3c2f 7370 616e 3e20 2020 2020 2020 203c < │ │ │ │ +00019990: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000199a0: 6f72 6466 6c6f 7722 3e63 6f6e 7469 6e75 ordflow">continu │ │ │ │ +000199b0: 653c 2f73 7061 6e3e 3b3c 2f64 6976 3e0a e;
    . │ │ │ │ +000199c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    572 │ │ │ │ +00019aa0: 2020 2020 2020 2020 626f 6f73 743a 3a64 boost::d │ │ │ │ +00019ab0: 796e 616d 6963 5f70 6f69 6e74 6572 5f63 ynamic_pointer_c │ │ │ │ +00019ac0: 6173 7426 6c74 3b63 6f6e 7374 2043 686f ast<const Cho │ │ │ │ +00019ad0: 6963 6526 6774 3b28 6974 2d26 6774 3b72 ice>(it->r │ │ │ │ +00019ae0: 6f6f 745f 293b 3c2f 6469 763e 0a3c 6469 oot_);
    .< │ │ │ │ +00019b00: 6120 6964 3d22 6c30 3035 3733 2220 6e61 a id="l00573" na │ │ │ │ +00019b10: 6d65 3d22 6c30 3035 3733 223e 3c2f 613e me="l00573"> │ │ │ │ +00019b20: 3c73 7061 6e20 636c 6173 733d 226c 696e 573 if (!hig │ │ │ │ +00019b70: 6865 7374 4c61 6265 6c20 7c7c 2063 2d26 hestLabel || c-& │ │ │ │ +00019b80: 6774 3b6c 6162 656c 2829 2026 6774 3b20 gt;label() > │ │ │ │ +00019b90: 2a68 6967 6865 7374 4c61 6265 6c29 207b *highestLabel) { │ │ │ │ +00019ba0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00019bf0: 2035 3734 3c2f 7370 616e 3e20 2020 2020 574 │ │ │ │ +00019c00: 2020 2068 6967 6865 7374 4c61 6265 6c2e highestLabel. │ │ │ │ +00019c10: 7265 7365 7428 632d 2667 743b 6c61 6265 reset(c->labe │ │ │ │ +00019c20: 6c28 2929 3b3c 2f64 6976 3e0a 3c64 6976 l());
    .
    < │ │ │ │ +00019c60: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00019c70: 6e6f 223e 2020 3537 353c 2f73 7061 6e3e no"> 575 │ │ │ │ +00019c80: 2020 2020 2020 2020 6e72 4368 6f69 6365 nrChoice │ │ │ │ +00019c90: 7320 3d20 632d 2667 743b 6e72 4368 6f69 s = c->nrChoi │ │ │ │ +00019ca0: 6365 7328 293b 3c2f 6469 763e 0a3c 6469 ces();
    .< │ │ │ │ +00019cc0: 6120 6964 3d22 6c30 3035 3736 2220 6e61 a id="l00576" na │ │ │ │ +00019cd0: 6d65 3d22 6c30 3035 3736 223e 3c2f 613e me="l00576"> │ │ │ │ +00019ce0: 3c73 7061 6e20 636c 6173 733d 226c 696e 576 }
    .< │ │ │ │ +00019d10: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00019d20: 3e3c 6120 6964 3d22 6c30 3035 3737 2220 > 577 }
    .< │ │ │ │ +00019d70: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00019d80: 3e3c 6120 6964 3d22 6c30 3035 3738 2220 > 578
    .
    579 │ │ │ │ +00019e20: 2020 203c 7370 616e 2063 6c61 7373 3d22 // if l │ │ │ │ +00019e40: 6162 656c 2069 7320 616c 7265 6164 7920 abel is already │ │ │ │ +00019e50: 696e 2063 6f72 7265 6374 206f 7264 6572 in correct order │ │ │ │ +00019e60: 2c20 6a75 7374 2070 7574 2074 6f67 6574 , just put toget │ │ │ │ +00019e70: 6865 7220 6120 6368 6f69 6365 206f 6e20 her a choice on │ │ │ │ +00019e80: 6c61 6265 6c3c 2f73 7061 6e3e 3c2f 6469 label.
    580 │ │ │ │ +00019ee0: 3c2f 7370 616e 3e20 2020 203c 7370 616e if ( │ │ │ │ +00019f10: 216e 7243 686f 6963 6573 207c 7c20 2168 !nrChoices || !h │ │ │ │ +00019f20: 6967 6865 7374 4c61 6265 6c20 7c7c 206c ighestLabel || l │ │ │ │ +00019f30: 6162 656c 2026 6774 3b20 2a68 6967 6865 abel > *highe │ │ │ │ +00019f40: 7374 4c61 6265 6c29 207b 3c2f 6469 763e stLabel) {
    │ │ │ │ +00019f50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00019f80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 581 auto cho │ │ │ │ +00019fd0: 6963 654f 6e4c 6162 656c 203d 2062 6f6f iceOnLabel = boo │ │ │ │ +00019fe0: 7374 3a3a 6d61 6b65 5f73 6861 7265 6426 st::make_shared& │ │ │ │ +00019ff0: 6c74 3b43 686f 6963 6526 6774 3b28 6c61 lt;Choice>(la │ │ │ │ +0001a000: 6265 6c2c 2065 6e64 202d 2062 6567 696e bel, end - begin │ │ │ │ +0001a010: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    582 │ │ │ │ +0001a070: 2020 203c 7370 616e 2063 6c61 7373 3d22 for │ │ │ │ +0001a090: 3c2f 7370 616e 3e20 2849 7465 7261 746f (Iterato │ │ │ │ +0001a0a0: 7220 6974 203d 2062 6567 696e 3b20 6974 r it = begin; it │ │ │ │ +0001a0b0: 2021 3d20 656e 643b 2069 742b 2b29 3c2f != end; it++).
    5 │ │ │ │ +0001a110: 3833 3c2f 7370 616e 3e20 2020 2020 2020 83 │ │ │ │ +0001a120: 2063 686f 6963 654f 6e4c 6162 656c 2d26 choiceOnLabel-& │ │ │ │ +0001a130: 6774 3b70 7573 685f 6261 636b 2869 742d gt;push_back(it- │ │ │ │ +0001a140: 2667 743b 726f 6f74 5f29 3b3c 2f64 6976 >root_);
    .
    584< │ │ │ │ +0001a1a0: 2f73 7061 6e3e 2020 2020 2020 3c73 7061 /span> return Choice::Uniq │ │ │ │ +0001a1e0: 7565 2863 686f 6963 654f 6e4c 6162 656c ue(choiceOnLabel │ │ │ │ +0001a1f0: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    585 │ │ │ │ +0001a250: 207d 203c 7370 616e 2063 6c61 7373 3d22 } els │ │ │ │ +0001a270: 653c 2f73 7061 6e3e 207b 3c2f 6469 763e e {
    │ │ │ │ +0001a280: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001a2b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 586 // Set up a new │ │ │ │ +0001a300: 2063 686f 6963 6520 6f6e 2074 6865 2068 choice on the h │ │ │ │ +0001a310: 6967 6865 7374 206c 6162 656c 3c2f 7370 ighest label
    .
    587 │ │ │ │ +0001a380: 2020 2020 3c73 7061 6e20 636c 6173 733d auto choiceOnHi │ │ │ │ +0001a3b0: 6768 6573 744c 6162 656c 203d 3c2f 6469 ghestLabel =.
    588 │ │ │ │ +0001a410: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ +0001a420: 2062 6f6f 7374 3a3a 6d61 6b65 5f73 6861 boost::make_sha │ │ │ │ +0001a430: 7265 6426 6c74 3b43 686f 6963 6526 6774 red<Choice> │ │ │ │ +0001a440: 3b28 2a68 6967 6865 7374 4c61 6265 6c2c ;(*highestLabel, │ │ │ │ +0001a450: 206e 7243 686f 6963 6573 293b 3c2f 6469 nrChoices);.
    589 │ │ │ │ +0001a4b0: 3c2f 7370 616e 3e20 2020 2020 203c 7370 // now, for a │ │ │ │ +0001a4e0: 6c6c 2070 6f73 7369 626c 6520 7661 6c75 ll possible valu │ │ │ │ +0001a4f0: 6573 206f 6620 6869 6768 6573 744c 6162 es of highestLab │ │ │ │ +0001a500: 656c 3c2f 7370 616e 3e3c 2f64 6976 3e0a el
    . │ │ │ │ +0001a510: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    591 │ │ │ │ +0001a640: 2020 2020 2020 3c73 7061 6e20 636c 6173 // m │ │ │ │ +0001a660: 616b 6520 6120 6e65 7720 7365 7420 6f66 ake a new set of │ │ │ │ +0001a670: 2066 756e 6374 696f 6e73 2066 6f72 2063 functions for c │ │ │ │ +0001a680: 6f6d 706f 7369 6e67 2062 7920 6974 6572 omposing by iter │ │ │ │ +0001a690: 6174 696e 6720 6f76 6572 2074 6865 2067 ating over the g │ │ │ │ +0001a6a0: 6976 656e 3c2f 7370 616e 3e3c 2f64 6976 iven
    .
    592< │ │ │ │ +0001a700: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> // functions │ │ │ │ +0001a730: 2c20 616e 6420 7365 6c65 6374 696e 6720 , and selecting │ │ │ │ +0001a740: 7468 6520 6170 7072 6f70 7269 6174 6520 the appropriate │ │ │ │ +0001a750: 6272 616e 6368 2e3c 2f73 7061 6e3e 3c2f branch..
    5 │ │ │ │ +0001a7b0: 3933 3c2f 7370 616e 3e20 2020 2020 2020 93 │ │ │ │ +0001a7c0: 2073 7464 3a3a 7665 6374 6f72 266c 743b std::vector< │ │ │ │ +0001a7d0: 4465 6369 7369 6f6e 5472 6565 2667 743b DecisionTree> │ │ │ │ +0001a7e0: 2066 756e 6374 696f 6e73 3b3c 2f64 6976 functions;
    .
    594< │ │ │ │ +0001a840: 2f73 7061 6e3e 2020 2020 2020 2020 3c73 /span> for (Iterator it │ │ │ │ +0001a880: 3d20 6265 6769 6e3b 2069 7420 213d 2065 = begin; it != e │ │ │ │ +0001a890: 6e64 3b20 6974 2b2b 2920 7b3c 2f64 6976 nd; it++) {
    .
    595< │ │ │ │ +0001a8f0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +0001a900: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // by rest │ │ │ │ +0001a920: 7269 6374 696e 6720 7468 6520 696e 7075 ricting the inpu │ │ │ │ +0001a930: 7420 6675 6e63 7469 6f6e 7320 746f 2076 t functions to v │ │ │ │ +0001a940: 616c 7565 2069 2066 6f72 206c 6162 656c alue i for label │ │ │ │ +0001a950: 4265 6c6f 773c 2f73 7061 6e3e 3c2f 6469 Below.
    596 │ │ │ │ +0001a9b0: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ +0001a9c0: 2044 6563 6973 696f 6e54 7265 6520 6368 DecisionTree ch │ │ │ │ +0001a9d0: 6f73 656e 203d 2069 742d 2667 743b 6368 osen = it->ch │ │ │ │ +0001a9e0: 6f6f 7365 282a 6869 6768 6573 744c 6162 oose(*highestLab │ │ │ │ +0001a9f0: 656c 2c20 696e 6465 7829 3b3c 2f64 6976 el, index);
    .
    597< │ │ │ │ +0001aa50: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +0001aa60: 6675 6e63 7469 6f6e 732e 7075 7368 5f62 functions.push_b │ │ │ │ +0001aa70: 6163 6b28 6368 6f73 656e 293b 3c2f 6469 ack(chosen);.
    598 │ │ │ │ +0001aad0: 3c2f 7370 616e 3e20 2020 2020 2020 207d } │ │ │ │ +0001aae0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001ab30: 2035 3939 3c2f 7370 616e 3e20 2020 2020 599 │ │ │ │ +0001ab40: 2020 203c 7370 616e 2063 6c61 7373 3d22 // We t │ │ │ │ +0001ab60: 6865 6e20 7265 6375 7273 652c 2066 6f72 hen recurse, for │ │ │ │ +0001ab70: 2061 6c6c 2076 616c 7565 7320 6f66 2074 all values of t │ │ │ │ +0001ab80: 6865 2068 6967 6865 7374 206c 6162 656c he highest label │ │ │ │ +0001ab90: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +0001abb0: 3c61 2069 643d 226c 3030 3630 3022 206e 600 NodePt │ │ │ │ +0001ac00: 7220 6669 203d 2063 6f6d 706f 7365 2866 r fi = compose(f │ │ │ │ +0001ac10: 756e 6374 696f 6e73 2e62 6567 696e 2829 unctions.begin() │ │ │ │ +0001ac20: 2c20 6675 6e63 7469 6f6e 732e 656e 6428 , functions.end( │ │ │ │ +0001ac30: 292c 206c 6162 656c 293b 3c2f 6469 763e ), label);
    │ │ │ │ +0001ac40: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001ac70: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 601 cho │ │ │ │ +0001aca0: 6963 654f 6e48 6967 6865 7374 4c61 6265 iceOnHighestLabe │ │ │ │ +0001acb0: 6c2d 2667 743b 7075 7368 5f62 6163 6b28 l->push_back( │ │ │ │ +0001acc0: 6669 293b 3c2f 6469 763e 0a3c 6469 7620 fi);
    .
    602 │ │ │ │ +0001ad20: 2020 2020 207d 3c2f 6469 763e 0a3c 6469 }
    .< │ │ │ │ +0001ad40: 6120 6964 3d22 6c30 3036 3033 2220 6e61 a id="l00603" na │ │ │ │ +0001ad50: 6d65 3d22 6c30 3036 3033 223e 3c2f 613e me="l00603"> │ │ │ │ +0001ad60: 3c73 7061 6e20 636c 6173 733d 226c 696e 603 return C │ │ │ │ +0001adb0: 686f 6963 653a 3a55 6e69 7175 6528 6368 hoice::Unique(ch │ │ │ │ +0001adc0: 6f69 6365 4f6e 4869 6768 6573 744c 6162 oiceOnHighestLab │ │ │ │ +0001add0: 656c 293b 3c2f 6469 763e 0a3c 6469 7620 el);
    .
    604 │ │ │ │ +0001ae30: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    605 │ │ │ │ +0001ae90: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    606 .
    6 │ │ │ │ +0001af40: 3037 3c2f 7370 616e 3e3c 7370 616e 2063 07 │ │ │ │ +0001af60: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ +0001af70: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001af80: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001af90: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001afa0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ +0001afb0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +0001afd0: 6120 6964 3d22 6c30 3036 3038 2220 6e61 a id="l00608" na │ │ │ │ +0001afe0: 6d65 3d22 6c30 3036 3038 223e 3c2f 613e me="l00608"> │ │ │ │ +0001aff0: 3c73 7061 6e20 636c 6173 733d 226c 696e 608 // &quo │ │ │ │ +0001b030: 743b 6372 6561 7465 2671 756f 743b 2069 t;create" i │ │ │ │ +0001b040: 7320 6120 6269 7420 6f66 2061 2063 6f6d s a bit of a com │ │ │ │ +0001b050: 706c 6963 6174 6564 2074 6869 6e67 2c20 plicated thing, │ │ │ │ +0001b060: 6275 7420 7665 7279 2075 7365 6675 6c2e but very useful. │ │ │ │ +0001b070: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +0001b090: 3c61 2069 643d 226c 3030 3630 3922 206e 609 // It │ │ │ │ +0001b0f0: 7461 6b65 7320 6120 7261 6e67 6520 6f66 takes a range of │ │ │ │ +0001b100: 206c 6162 656c 7320 616e 6420 6120 636f labels and a co │ │ │ │ +0001b110: 7272 6573 706f 6e64 696e 6720 7261 6e67 rresponding rang │ │ │ │ +0001b120: 6520 6f66 2076 616c 7565 732c 3c2f 7370 e of values,
    .
    610 │ │ │ │ +0001b190: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // and cre │ │ │ │ +0001b1b0: 6174 6573 2061 2064 6563 6973 696f 6e20 ates a decision │ │ │ │ +0001b1c0: 7472 6565 2c20 6173 2066 6f6c 6c6f 7773 tree, as follows │ │ │ │ +0001b1d0: 3a3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c :
    .< │ │ │ │ +0001b1e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001b1f0: 3e3c 6120 6964 3d22 6c30 3036 3131 2220 > 611 // - │ │ │ │ +0001b250: 6966 2074 6865 7265 2069 7320 6f6e 6c79 if there is only │ │ │ │ +0001b260: 206f 6e65 206c 6162 656c 2c20 6372 6561 one label, crea │ │ │ │ +0001b270: 7465 7320 6120 6368 6f69 6365 206e 6f64 tes a choice nod │ │ │ │ +0001b280: 6520 7769 7468 2076 616c 7565 7320 696e e with values in │ │ │ │ +0001b290: 206c 6561 7665 733c 2f73 7061 6e3e 3c2f leaves.
    6 │ │ │ │ +0001b2f0: 3132 3c2f 7370 616e 3e20 203c 7370 616e 12 // - otherwise, │ │ │ │ +0001b320: 2069 7420 6576 656e 6c79 2073 706c 6974 it evenly split │ │ │ │ +0001b330: 7320 7570 2074 6865 2072 616e 6765 206f s up the range o │ │ │ │ +0001b340: 6620 7661 6c75 6573 2061 6e64 2063 7265 f values and cre │ │ │ │ +0001b350: 6174 6573 2061 2074 7265 6520 666f 723c ates a tree for< │ │ │ │ +0001b360: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +0001b380: 6120 6964 3d22 6c30 3036 3133 2220 6e61 a id="l00613" na │ │ │ │ +0001b390: 6d65 3d22 6c30 3036 3133 223e 3c2f 613e me="l00613"> │ │ │ │ +0001b3a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 613 // ea │ │ │ │ +0001b3e0: 6368 2073 7562 2d72 616e 6765 2c20 616e ch sub-range, an │ │ │ │ +0001b3f0: 6420 6173 7369 676e 7320 7468 6174 2074 d assigns that t │ │ │ │ +0001b400: 7265 6520 746f 2066 6972 7374 206c 6162 ree to first lab │ │ │ │ +0001b410: 656c 2623 3339 3b73 2063 686f 6963 6573 el's choices │ │ │ │ +0001b420: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +0001b440: 3c61 2069 643d 226c 3030 3631 3422 206e 614 // Exa │ │ │ │ +0001b4a0: 6d70 6c65 3a3c 2f73 7061 6e3e 3c2f 6469 mple:.
    615 │ │ │ │ +0001b500: 3c2f 7370 616e 3e20 203c 7370 616e 2063 / │ │ │ │ +0001b520: 2f20 6372 6561 7465 285b 4220 415d 2c5b / create([B A],[ │ │ │ │ +0001b530: 3120 3220 3320 345d 2920 776f 756c 6420 1 2 3 4]) would │ │ │ │ +0001b540: 6361 6c6c 3c2f 7370 616e 3e3c 2f64 6976 call
    .
    616< │ │ │ │ +0001b5a0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ +0001b5c0: 2020 2063 7265 6174 6528 5b41 5d2c 5b31 create([A],[1 │ │ │ │ +0001b5d0: 2032 5d29 3c2f 7370 616e 3e3c 2f64 6976 2])
    .
    617< │ │ │ │ +0001b630: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ +0001b650: 2020 2063 7265 6174 6528 5b41 5d2c 5b33 create([A],[3 │ │ │ │ +0001b660: 2034 5d29 3c2f 7370 616e 3e3c 2f64 6976 4])
    .
    618< │ │ │ │ +0001b6c0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ +0001b6e0: 2061 6e64 2070 726f 6475 6365 3c2f 7370 and produce
    .
    619 │ │ │ │ +0001b750: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // B=0
    .
    620 │ │ │ │ +0001b7d0: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // A=0: 1 │ │ │ │ +0001b7f0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +0001b810: 3c61 2069 643d 226c 3030 3632 3122 206e 621 // A= │ │ │ │ +0001b870: 313a 2032 3c2f 7370 616e 3e3c 2f64 6976 1: 2
    .
    622< │ │ │ │ +0001b8d0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ +0001b8f0: 2042 3d31 3c2f 7370 616e 3e3c 2f64 6976 B=1
    .
    623< │ │ │ │ +0001b950: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> // │ │ │ │ +0001b970: 2020 413d 303a 2033 3c2f 7370 616e 3e3c A=0: 3< │ │ │ │ +0001b980: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0001b9d0: 3632 343c 2f73 7061 6e3e 2020 3c73 7061 624 // A=1: 4
    .
    625 │ │ │ │ +0001ba60: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // Note, t │ │ │ │ +0001ba80: 6872 6f75 6768 2074 6865 206d 6167 6963 hrough the magic │ │ │ │ +0001ba90: 206f 6620 2671 756f 743b 636f 6d70 6f73 of "compos │ │ │ │ +0001baa0: 6526 7175 6f74 3b2c 2063 7265 6174 6528 e", create( │ │ │ │ +0001bab0: 5b41 2042 5d2c 5b31 2033 2032 2034 5d29 [A B],[1 3 2 4]) │ │ │ │ +0001bac0: 2077 696c 6c20 7072 6f64 7563 653c 2f73 will produce
    .
    626 │ │ │ │ +0001bb30: 203c 7370 616e 2063 6c61 7373 3d22 636f // exactl │ │ │ │ +0001bb50: 7920 7468 6520 7361 6d65 2074 7265 6520 y the same tree │ │ │ │ +0001bb60: 6173 2061 626f 7665 3a20 7468 6520 6869 as above: the hi │ │ │ │ +0001bb70: 6768 6573 7420 6c61 6265 6c20 6973 2061 ghest label is a │ │ │ │ +0001bb80: 6c77 6179 7320 7468 6520 726f 6f74 2e3c lways the root.< │ │ │ │ +0001bb90: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +0001bbb0: 6120 6964 3d22 6c30 3036 3237 2220 6e61 a id="l00627" na │ │ │ │ +0001bbc0: 6d65 3d22 6c30 3036 3237 223e 3c2f 613e me="l00627"> │ │ │ │ +0001bbd0: 3c73 7061 6e20 636c 6173 733d 226c 696e 627 // Howe │ │ │ │ +0001bc10: 7665 722c 2069 7420 7769 6c6c 2062 6520 ver, it will be │ │ │ │ +0001bc20: 2a77 6179 2a20 6661 7374 6572 2069 6620 *way* faster if │ │ │ │ +0001bc30: 6c61 6265 6c73 2061 7265 2067 6976 656e labels are given │ │ │ │ +0001bc40: 2068 6967 6865 7374 2074 6f20 6c6f 7765 highest to lowe │ │ │ │ +0001bc50: 7374 2e3c 2f73 7061 6e3e 3c2f 6469 763e st.
    │ │ │ │ +0001bc60: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001bc90: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 628 tem │ │ │ │ +0001bcd0: 706c 6174 653c 2f73 7061 6e3e 266c 743b plate< │ │ │ │ +0001bce0: 3c73 7061 6e20 636c 6173 733d 226b 6579 typename L, t │ │ │ │ +0001bd20: 7970 656e 616d 653c 2f73 7061 6e3e 2059 ypename Y │ │ │ │ +0001bd30: 2667 743b 3c2f 6469 763e 0a3c 6469 7620 >
    .
    629 │ │ │ │ +0001bd90: 203c 7370 616e 2063 6c61 7373 3d22 6b65 template< │ │ │ │ +0001bdb0: 2f73 7061 6e3e 266c 743b 3c73 7061 6e20 /span>< │ │ │ │ +0001bdd0: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +0001bde0: 4974 2c20 3c73 7061 6e20 636c 6173 733d It, typena │ │ │ │ +0001be00: 6d65 3c2f 7370 616e 3e20 5661 6c75 6549 me ValueI │ │ │ │ +0001be10: 7426 6774 3b3c 2f64 6976 3e0a 3c64 6976 t>
    .
    ..
    631 │ │ │ │ +0001c070: 2020 2020 4974 2062 6567 696e 2c20 4974 It begin, It │ │ │ │ +0001c080: 2065 6e64 2c20 5661 6c75 6549 7420 6265 end, ValueIt be │ │ │ │ +0001c090: 6769 6e59 2c20 5661 6c75 6549 7420 656e ginY, ValueIt en │ │ │ │ +0001c0a0: 6459 293c 7370 616e 2063 6c61 7373 3d22 dY) const │ │ │ │ +0001c0c0: 3c2f 7370 616e 3e7b 3c2f 6469 763e 0a3c {
    .< │ │ │ │ +0001c0d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001c0e0: 3e3c 6120 6964 3d22 6c30 3036 3332 2220 > 632 // │ │ │ │ +0001c140: 6765 7420 6372 7563 6961 6c20 636f 756e get crucial coun │ │ │ │ +0001c150: 7473 3c2f 7370 616e 3e3c 2f64 6976 3e0a ts
    . │ │ │ │ +0001c160: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +0001c190: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 633 size_t │ │ │ │ +0001c1e0: 6e72 4368 6f69 6365 7320 3d20 6265 6769 nrChoices = begi │ │ │ │ +0001c1f0: 6e2d 2667 743b 7365 636f 6e64 3b3c 2f64 n->second;.
    63 │ │ │ │ +0001c250: 343c 2f73 7061 6e3e 2020 2020 3c73 7061 4 size_t size = endY │ │ │ │ +0001c290: 2d20 6265 6769 6e59 3b3c 2f64 6976 3e0a - beginY;
    . │ │ │ │ +0001c2a0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ +0001c330: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0001c340: 6e6f 223e 2020 3633 363c 2f73 7061 6e3e no"> 636 │ │ │ │ +0001c350: 2020 2020 3c73 7061 6e20 636c 6173 733d // Fin │ │ │ │ +0001c370: 6420 7468 6520 6e65 7874 206b 6579 2074 d the next key t │ │ │ │ +0001c380: 6f20 776f 726b 206f 6e3c 2f73 7061 6e3e o work on │ │ │ │ +0001c390: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001c3e0: 2036 3337 3c2f 7370 616e 3e20 2020 2049 637 I │ │ │ │ +0001c3f0: 7420 6c61 6265 6c43 203d 2062 6567 696e t labelC = begin │ │ │ │ +0001c400: 202b 2031 3b3c 2f64 6976 3e0a 3c64 6976 + 1;
    .
    < │ │ │ │ +0001c440: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0001c450: 6e6f 223e 2020 3633 383c 2f73 7061 6e3e no"> 638 │ │ │ │ +0001c460: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ +0001c480: 3c2f 7370 616e 3e20 286c 6162 656c 4320 (labelC │ │ │ │ +0001c490: 3d3d 2065 6e64 2920 7b3c 2f64 6976 3e0a == end) {
    . │ │ │ │ +0001c4a0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .< │ │ │ │ +0001c550: 6120 6964 3d22 6c30 3036 3430 2220 6e61 a id="l00640" na │ │ │ │ +0001c560: 6d65 3d22 6c30 3036 3430 223e 3c2f 613e me="l00640"> │ │ │ │ +0001c570: 3c73 7061 6e20 636c 6173 733d 226c 696e 640 // │ │ │ │ +0001c5b0: 4372 6561 7465 2061 2073 696d 706c 6520 Create a simple │ │ │ │ +0001c5c0: 6368 6f69 6365 206e 6f64 6520 7769 7468 choice node with │ │ │ │ +0001c5d0: 2076 616c 7565 7320 6173 206c 6561 7665 values as leave │ │ │ │ +0001c5e0: 732e 3c2f 7370 616e 3e3c 2f64 6976 3e0a s.
    . │ │ │ │ +0001c5f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    642 │ │ │ │ +0001c6e0: 2020 2020 2020 7374 643a 3a63 6f75 7420 std::cout │ │ │ │ +0001c6f0: 266c 743b 266c 743b 203c 7370 616e 2063 << "Tryin │ │ │ │ +0001c720: 6720 746f 2063 7265 6174 6520 4444 206f g to create DD o │ │ │ │ +0001c730: 6e20 2671 756f 743b 3c2f 7370 616e 3e20 n " │ │ │ │ +0001c740: 266c 743b 266c 743b 2062 6567 696e 2d26 << begin-& │ │ │ │ +0001c750: 6774 3b66 6972 7374 2026 6c74 3b26 6c74 gt;first << │ │ │ │ +0001c760: 3b20 7374 643a 3a65 6e64 6c3b 3c2f 6469 ; std::endl;.
    643 │ │ │ │ +0001c7c0: 3c2f 7370 616e 3e20 2020 2020 2020 2073 s │ │ │ │ +0001c7d0: 7464 3a3a 636f 7574 2026 6c74 3b26 6c74 td::cout << │ │ │ │ +0001c7e0: 3b20 626f 6f73 743a 3a66 6f72 6d61 7428 ; boost::format( │ │ │ │ +0001c7f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001c840: 2036 3434 3c2f 7370 616e 3e20 2020 2020 644 │ │ │ │ +0001c850: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0001c860: 2020 2020 3c73 7061 6e20 636c 6173 733d │ │ │ │ +0001c880: 2671 756f 743b 4465 6369 7369 6f6e 5472 "DecisionTr │ │ │ │ +0001c890: 6565 3a3a 6372 6561 7465 3a20 6578 7065 ee::create: expe │ │ │ │ +0001c8a0: 6374 6564 2025 6420 7661 6c75 6573 2062 cted %d values b │ │ │ │ +0001c8b0: 7574 2067 6f74 2025 6420 2671 756f 743b ut got %d " │ │ │ │ +0001c8c0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +0001c8e0: 3c61 2069 643d 226c 3030 3634 3522 206e 645 │ │ │ │ +0001c930: 2020 2020 2020 2020 2020 203c 7370 616e "ins │ │ │ │ +0001c960: 7465 6164 2671 756f 743b 3c2f 7370 616e tead") %
    .
    646 │ │ │ │ +0001c9d0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0001c9e0: 2020 2020 2020 2020 6e72 4368 6f69 6365 nrChoice │ │ │ │ +0001c9f0: 7320 2520 7369 7a65 3c2f 6469 763e 0a3c s % size
    .< │ │ │ │ +0001ca00: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001ca10: 3e3c 6120 6964 3d22 6c30 3036 3437 2220 > 647 │ │ │ │ +0001ca60: 2020 2020 2026 6c74 3b26 6c74 3b20 7374 << st │ │ │ │ +0001ca70: 643a 3a65 6e64 6c3b 3c2f 6469 763e 0a3c d::endl;
    .< │ │ │ │ +0001ca80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001ca90: 3e3c 6120 6964 3d22 6c30 3036 3438 2220 > 648 throw std::invalid_a │ │ │ │ +0001cb10: 7267 756d 656e 7428 3c73 7061 6e20 636c rgument("Decisi │ │ │ │ +0001cb40: 6f6e 5472 6565 3a3a 6372 6561 7465 2069 onTree::create i │ │ │ │ +0001cb50: 6e76 616c 6964 2061 7267 756d 656e 7426 nvalid argument& │ │ │ │ +0001cb60: 7175 6f74 3b3c 2f73 7061 6e3e 293b 3c2f quot;);.
    6 │ │ │ │ +0001cbc0: 3439 3c2f 7370 616e 3e20 2020 2020 207d 49 } │ │ │ │ +0001cbd0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001cc20: 2036 3530 3c2f 7370 616e 3e20 2020 2020 650 │ │ │ │ +0001cc30: 203c 7370 616e 2063 6c61 7373 3d22 6b65 auto choice = boos │ │ │ │ +0001cc60: 743a 3a6d 616b 655f 7368 6172 6564 266c t::make_shared&l │ │ │ │ +0001cc70: 743b 4368 6f69 6365 2667 743b 2862 6567 t;Choice>(beg │ │ │ │ +0001cc80: 696e 2d26 6774 3b66 6972 7374 2c20 656e in->first, en │ │ │ │ +0001cc90: 6459 202d 2062 6567 696e 5929 3b3c 2f64 dY - beginY);.
    65 │ │ │ │ +0001ccf0: 313c 2f73 7061 6e3e 2020 2020 2020 3c73 1 for (ValueIt y = │ │ │ │ +0001cd30: 6265 6769 6e59 3b20 7920 213d 2065 6e64 beginY; y != end │ │ │ │ +0001cd40: 593b 2079 2b2b 293c 2f64 6976 3e0a 3c64 Y; y++)
    . │ │ │ │ +0001cd60: 3c61 2069 643d 226c 3030 3635 3222 206e 652 choice │ │ │ │ +0001cdb0: 2d26 6774 3b70 7573 685f 6261 636b 283c ->push_back(< │ │ │ │ +0001cdc0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0001cdd0: 5f74 7970 6564 6566 2220 6872 6566 3d22 _typedef" href=" │ │ │ │ +0001cde0: 6130 3237 3936 2e68 746d 6c23 6133 3661 a02796.html#a36a │ │ │ │ +0001cdf0: 3938 6636 3162 3062 3764 6630 3432 3432 98f61b0b7df04242 │ │ │ │ +0001ce00: 3762 3133 3262 3331 3364 3766 6422 3e4e 7b132b313d7fd">N │ │ │ │ +0001ce10: 6f64 6550 7472 3c2f 613e 283c 7370 616e odePtr(new Leaf(*y)));
    .< │ │ │ │ +0001ce80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001ce90: 3e3c 6120 6964 3d22 6c30 3036 3533 2220 > 653 return │ │ │ │ +0001cf00: 2043 686f 6963 653a 3a55 6e69 7175 6528 Choice::Unique( │ │ │ │ +0001cf10: 6368 6f69 6365 293b 3c2f 6469 763e 0a3c choice);
    .< │ │ │ │ +0001cf20: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001cf30: 3e3c 6120 6964 3d22 6c30 3036 3534 2220 > 654 }
    .< │ │ │ │ +0001cf80: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001cf90: 3e3c 6120 6964 3d22 6c30 3036 3535 2220 > 655
    .
    656 │ │ │ │ +0001d030: 2020 203c 7370 616e 2063 6c61 7373 3d22 // Recu │ │ │ │ +0001d050: 7273 6976 6520 6361 7365 3a20 7065 7266 rsive case: perf │ │ │ │ +0001d060: 6f72 6d20 2671 756f 743b 5368 616e 6e6f orm "Shanno │ │ │ │ +0001d070: 6e20 6578 7061 6e73 696f 6e26 7175 6f74 n expansion" │ │ │ │ +0001d080: 3b3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c ;
    .< │ │ │ │ +0001d090: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001d0a0: 3e3c 6120 6964 3d22 6c30 3036 3537 2220 > 657 // │ │ │ │ +0001d100: 4372 6561 7465 7320 6f6e 6520 7472 6565 Creates one tree │ │ │ │ +0001d110: 2028 692e 652e 2c66 756e 6374 696f 6e29 (i.e.,function) │ │ │ │ +0001d120: 2066 6f72 2065 6163 6820 6368 6f69 6365 for each choice │ │ │ │ +0001d130: 206f 6620 6375 7272 656e 7420 6b65 793c of current key< │ │ │ │ +0001d140: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +0001d160: 6120 6964 3d22 6c30 3036 3538 2220 6e61 a id="l00658" na │ │ │ │ +0001d170: 6d65 3d22 6c30 3036 3538 223e 3c2f 613e me="l00658"> │ │ │ │ +0001d180: 3c73 7061 6e20 636c 6173 733d 226c 696e 658 // by │ │ │ │ +0001d1c0: 2063 616c 6c69 6e67 2063 7265 6174 6520 calling create │ │ │ │ +0001d1d0: 7265 6375 7273 6976 656c 792c 2061 6e64 recursively, and │ │ │ │ +0001d1e0: 2074 6865 6e20 7075 7473 2074 6865 6d20 then puts them │ │ │ │ +0001d1f0: 616c 6c20 746f 6765 7468 6572 2e3c 2f73 all together.
    .
    659 │ │ │ │ +0001d260: 2020 2073 7464 3a3a 7665 6374 6f72 266c std::vector&l │ │ │ │ +0001d270: 743b 4465 6369 7369 6f6e 5472 6565 2667 t;DecisionTree&g │ │ │ │ +0001d280: 743b 2066 756e 6374 696f 6e73 3b3c 2f64 t; functions;.
    66 │ │ │ │ +0001d2e0: 303c 2f73 7061 6e3e 2020 2020 3c73 7061 0 size_t split = │ │ │ │ +0001d370: 2073 697a 6520 2f20 6e72 4368 6f69 6365 size / nrChoice │ │ │ │ +0001d380: 733b 3c2f 6469 763e 0a3c 6469 7620 636c s;
    .
    661 │ │ │ │ +0001d3e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 for (size_t i │ │ │ │ +0001d430: 203d 2030 3b20 6920 266c 743b 206e 7243 = 0; i < nrC │ │ │ │ +0001d440: 686f 6963 6573 3b20 692b 2b2c 2062 6567 hoices; i++, beg │ │ │ │ +0001d450: 696e 5920 2b3d 203c 6120 636c 6173 733d inY += split) {
    .
    662 │ │ │ │ +0001d510: 2020 2020 203c 6120 636c 6173 733d 2263 NodePtr │ │ │ │ +0001d570: 2066 203d 2063 7265 6174 6526 6c74 3b49 f = create<I │ │ │ │ +0001d580: 742c 2056 616c 7565 4974 2667 743b 286c t, ValueIt>(l │ │ │ │ +0001d590: 6162 656c 432c 2065 6e64 2c20 6265 6769 abelC, end, begi │ │ │ │ +0001d5a0: 6e59 2c20 6265 6769 6e59 202b 203c 6120 nY, beginY + sp │ │ │ │ +0001d600: 6c69 743c 2f61 3e29 3b3c 2f64 6976 3e0a lit);
    . │ │ │ │ +0001d610: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    664 │ │ │ │ +0001d6e0: 2020 207d 3c2f 6469 763e 0a3c 6469 7620 }
    .
    665 │ │ │ │ +0001d740: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ +0001d760: 7572 6e3c 2f73 7061 6e3e 2063 6f6d 706f urn compo │ │ │ │ +0001d770: 7365 2866 756e 6374 696f 6e73 2e62 6567 se(functions.beg │ │ │ │ +0001d780: 696e 2829 2c20 6675 6e63 7469 6f6e 732e in(), functions. │ │ │ │ +0001d790: 656e 6428 292c 2062 6567 696e 2d26 6774 end(), begin-> │ │ │ │ +0001d7a0: 3b66 6972 7374 293b 3c2f 6469 763e 0a3c ;first);
    .< │ │ │ │ +0001d7b0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001d7c0: 3e3c 6120 6964 3d22 6c30 3036 3636 2220 > 666 }
    ..
    66 │ │ │ │ +0001d860: 373c 2f73 7061 6e3e 203c 2f64 6976 3e0a 7
    . │ │ │ │ +0001d870: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +0001d980: 2020 3636 393c 2f73 7061 6e3e 2020 3c73 669 template <ty │ │ │ │ +0001d9d0: 7065 6e61 6d65 3c2f 7370 616e 3e20 4c2c pename L, │ │ │ │ +0001d9e0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 typename< │ │ │ │ +0001da00: 2f73 7061 6e3e 2059 2667 743b 3c2f 6469 /span> Y>.
    670 │ │ │ │ +0001da60: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +0001da80: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ +0001da90: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ +0001dab0: 653c 2f73 7061 6e3e 204d 2c20 3c73 7061 e M, typename X>
    ..
    671 typenam │ │ │ │ +0001dbf0: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e Decisio │ │ │ │ +0001dc50: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ +0001dc60: 3b3a 3a4e 6f64 6550 7472 3c2f 613e 203c ;::NodePtr < │ │ │ │ +0001dc70: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0001dc80: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ +0001dc90: 2261 3032 3739 362e 6874 6d6c 2361 3134 "a02796.html#a14 │ │ │ │ +0001dca0: 3936 3661 3365 3231 6334 3239 3063 3430 966a3e21c4290c40 │ │ │ │ +0001dcb0: 6133 3338 3833 3133 3437 3862 6638 223e a3388313478bf8"> │ │ │ │ +0001dcc0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ +0001dcd0: 4c2c 2059 2667 743b 3a3a 636f 6e76 6572 L, Y>::conver │ │ │ │ +0001dce0: 7446 726f 6d3c 2f61 3e28 3c2f 6469 763e tFrom(
    │ │ │ │ +0001dcf0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    │ │ │ │ +0001de60: 2020 3637 333c 2f73 7061 6e3e 2020 2020 673 │ │ │ │ +0001de70: 2020 7374 643a 3a66 756e 6374 696f 6e26 std::function& │ │ │ │ +0001de80: 6c74 3b4c 283c 7370 616e 2063 6c61 7373 lt;L(const │ │ │ │ +0001dea0: 3c2f 7370 616e 3e20 4d26 616d 703b 2926 M&)& │ │ │ │ +0001deb0: 6774 3b20 4c5f 6f66 5f4d 2c3c 2f64 6976 gt; L_of_M,
    .
    674< │ │ │ │ +0001df10: 2f73 7061 6e3e 2020 2020 2020 7374 643a /span> std: │ │ │ │ +0001df20: 3a66 756e 6374 696f 6e26 6c74 3b59 283c :function<Y(< │ │ │ │ +0001df30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0001df40: 6f72 6422 3e63 6f6e 7374 3c2f 7370 616e ord">const X&)> Y_ │ │ │ │ +0001df60: 6f66 5f58 293c 7370 616e 2063 6c61 7373 of_X) cons │ │ │ │ +0001df80: 7420 3c2f 7370 616e 3e7b 3c2f 6469 763e t {
    │ │ │ │ +0001df90: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001dfc0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 675 u │ │ │ │ +0001e000: 7369 6e67 203c 2f73 7061 6e3e 4c59 203d sing LY = │ │ │ │ +0001e010: 203c 6120 636c 6173 733d 2263 6f64 6520 Dec │ │ │ │ +0001e040: 6973 696f 6e54 7265 6526 6c74 3b4c 2c20 isionTree<L, │ │ │ │ +0001e050: 5926 6774 3b3c 2f61 3e3b 3c2f 6469 763e Y>;
    │ │ │ │ +0001e060: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +0001e0d0: 6120 6964 3d22 6c30 3036 3737 2220 6e61 a id="l00677" na │ │ │ │ +0001e0e0: 6d65 3d22 6c30 3036 3737 223e 3c2f 613e me="l00677"> │ │ │ │ +0001e0f0: 3c73 7061 6e20 636c 6173 733d 226c 696e 677 // Ug │ │ │ │ +0001e130: 6c69 6e65 7373 2062 656c 6f77 2062 6563 liness below bec │ │ │ │ +0001e140: 6175 7365 2061 7070 6172 656e 746c 7920 ause apparently │ │ │ │ +0001e150: 7765 2063 616e 2623 3339 3b74 2068 6176 we can't hav │ │ │ │ +0001e160: 6520 7465 6d70 6c61 7465 6420 7669 7274 e templated virt │ │ │ │ +0001e170: 7561 6c3c 2f73 7061 6e3e 3c2f 6469 763e ual
    │ │ │ │ +0001e180: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001e1b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 678 / │ │ │ │ +0001e1f0: 2f20 6675 6e63 7469 6f6e 732e 3c2f 7370 / functions.
    .
    679 │ │ │ │ +0001e260: 2020 3c73 7061 6e20 636c 6173 733d 2263 // If le │ │ │ │ +0001e280: 6166 2c20 6170 706c 7920 756e 6172 7920 af, apply unary │ │ │ │ +0001e290: 636f 6e76 6572 7369 6f6e 2026 7175 6f74 conversion " │ │ │ │ +0001e2a0: 3b6f 7026 7175 6f74 3b20 616e 6420 6372 ;op" and cr │ │ │ │ +0001e2b0: 6561 7465 2061 2075 6e69 7175 6520 6c65 eate a unique le │ │ │ │ +0001e2c0: 6166 2e3c 2f73 7061 6e3e 3c2f 6469 763e af.
    │ │ │ │ +0001e2d0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001e300: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 680 u │ │ │ │ +0001e340: 7369 6e67 203c 2f73 7061 6e3e 4d58 4c65 sing MXLe │ │ │ │ +0001e350: 6166 203d 203c 7370 616e 2063 6c61 7373 af = typen │ │ │ │ +0001e370: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decision │ │ │ │ +0001e3b0: 5472 6565 266c 743b 4d2c 2058 2667 743b Tree<M, X> │ │ │ │ +0001e3c0: 3a3a 4c65 6166 3c2f 613e 3b3c 2f64 6976 ::Leaf;
    .
    681< │ │ │ │ +0001e420: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> if (< │ │ │ │ +0001e450: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0001e460: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ +0001e470: 206c 6561 6620 3d20 626f 6f73 743a 3a64 leaf = boost::d │ │ │ │ +0001e480: 796e 616d 6963 5f70 6f69 6e74 6572 5f63 ynamic_pointer_c │ │ │ │ +0001e490: 6173 7426 6c74 3b63 6f6e 7374 204d 584c ast<const MXL │ │ │ │ +0001e4a0: 6561 6626 6774 3b28 6629 2920 7b3c 2f64 eaf>(f)) {.
    68 │ │ │ │ +0001e500: 323c 2f73 7061 6e3e 2020 2020 2020 3c73 2 return NodePtr(new │ │ │ │ +0001e5e0: 4c65 6166 3c2f 613e 2859 5f6f 665f 5828 Leaf(Y_of_X( │ │ │ │ +0001e5f0: 6c65 6166 2d26 6774 3b63 6f6e 7374 616e leaf->constan │ │ │ │ +0001e600: 7428 2929 2c20 6c65 6166 2d26 6774 3b6e t()), leaf->n │ │ │ │ +0001e610: 7241 7373 6967 6e6d 656e 7473 2829 2929 rAssignments())) │ │ │ │ +0001e620: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0001e670: 2020 3638 333c 2f73 7061 6e3e 2020 2020 683 │ │ │ │ +0001e680: 7d3c 2f64 6976 3e0a 3c64 6976 2063 6c61 }
    .
    │ │ │ │ +0001e6d0: 2020 3638 343c 2f73 7061 6e3e 203c 2f64 684 .
    68 │ │ │ │ +0001e730: 353c 2f73 7061 6e3e 2020 2020 3c73 7061 5 // Check if Ch │ │ │ │ +0001e760: 6f69 6365 3c2f 7370 616e 3e3c 2f64 6976 oice
    .
    686< │ │ │ │ +0001e7c0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> │ │ │ │ +0001e7e0: 7573 696e 6720 3c2f 7370 616e 3e4d 5843 using MXC │ │ │ │ +0001e7f0: 686f 6963 6520 3d20 3c73 7061 6e20 636c hoice = ty │ │ │ │ +0001e810: 7065 6e61 6d65 3c2f 7370 616e 3e20 3c61 pename Decis │ │ │ │ +0001e850: 696f 6e54 7265 6526 6c74 3b4d 2c20 5826 ionTree<M, X& │ │ │ │ +0001e860: 6774 3b3a 3a43 686f 6963 653c 2f61 3e3b gt;::Choice; │ │ │ │ +0001e870: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001e8c0: 2036 3837 3c2f 7370 616e 3e20 2020 203c 687 < │ │ │ │ +0001e8d0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0001e8e0: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ +0001e8f0: 2063 686f 6963 6520 3d20 626f 6f73 743a choice = boost: │ │ │ │ +0001e900: 3a64 796e 616d 6963 5f70 6f69 6e74 6572 :dynamic_pointer │ │ │ │ +0001e910: 5f63 6173 7426 6c74 3b63 6f6e 7374 204d _cast<const M │ │ │ │ +0001e920: 5843 686f 6963 6526 6774 3b28 6629 3b3c XChoice>(f);< │ │ │ │ +0001e930: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0001e980: 3638 383c 2f73 7061 6e3e 2020 2020 3c73 688 if (!choice) throw std::invalid_ │ │ │ │ +0001e9f0: 6172 6775 6d65 6e74 283c 2f64 6976 3e0a argument(
    . │ │ │ │ +0001ea00: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 . │ │ │ │ +0001ead0: 3c61 2069 643d 226c 3030 3639 3022 206e 690
    .
    691 │ │ │ │ +0001eb70: 2020 3c73 7061 6e20 636c 6173 733d 2263 // get n │ │ │ │ +0001eb90: 6577 206c 6162 656c 3c2f 7370 616e 3e3c ew label< │ │ │ │ +0001eba0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0001ebf0: 3639 323c 2f73 7061 6e3e 2020 2020 3c73 692 const │ │ │ │ +0001ec20: 204d 206f 6c64 4c61 6265 6c20 3d20 6368 M oldLabel = ch │ │ │ │ +0001ec30: 6f69 6365 2d26 6774 3b6c 6162 656c 2829 oice->label() │ │ │ │ +0001ec40: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0001ec90: 2020 3639 333c 2f73 7061 6e3e 2020 2020 693 │ │ │ │ +0001eca0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const L newLabel = │ │ │ │ +0001ecd0: 4c5f 6f66 5f4d 286f 6c64 4c61 6265 6c29 L_of_M(oldLabel) │ │ │ │ +0001ece0: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +0001ed30: 2020 3639 343c 2f73 7061 6e3e 203c 2f64 694 .
    69 │ │ │ │ +0001ed90: 353c 2f73 7061 6e3e 2020 2020 3c73 7061 5 // put togethe │ │ │ │ +0001edc0: 7220 7669 6120 5368 616e 6e6f 6e20 6578 r via Shannon ex │ │ │ │ +0001edd0: 7061 6e73 696f 6e20 6f74 6865 7277 6973 pansion otherwis │ │ │ │ +0001ede0: 6520 6e6f 7420 736f 7274 6564 2e3c 2f73 e not sorted.
    .
    696 │ │ │ │ +0001ee50: 2020 2073 7464 3a3a 7665 6374 6f72 266c std::vector&l │ │ │ │ +0001ee60: 743b 4c59 2667 743b 2066 756e 6374 696f t;LY> functio │ │ │ │ +0001ee70: 6e73 3b3c 2f64 6976 3e0a 3c64 6976 2063 ns;
    .
    697 │ │ │ │ +0001eed0: 2020 3c73 7061 6e20 636c 6173 733d 226b for< │ │ │ │ +0001eef0: 2f73 7061 6e3e 2028 3c73 7061 6e20 636c /span> (au │ │ │ │ +0001ef10: 746f 3c2f 7370 616e 3e26 616d 703b 2661 to&&a │ │ │ │ +0001ef20: 6d70 3b20 6272 616e 6368 203a 2063 686f mp; branch : cho │ │ │ │ +0001ef30: 6963 652d 2667 743b 6272 616e 6368 6573 ice->branches │ │ │ │ +0001ef40: 2829 2920 7b3c 2f64 6976 3e0a 3c64 6976 ()) {
    .
    < │ │ │ │ +0001ef80: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0001ef90: 6e6f 223e 2020 3639 383c 2f73 7061 6e3e no"> 698 │ │ │ │ +0001efa0: 2020 2020 2020 6675 6e63 7469 6f6e 732e functions. │ │ │ │ +0001efb0: 656d 706c 6163 655f 6261 636b 2863 6f6e emplace_back(con │ │ │ │ +0001efc0: 7665 7274 4672 6f6d 266c 743b 4d2c 2058 vertFrom<M, X │ │ │ │ +0001efd0: 2667 743b 2862 7261 6e63 682c 204c 5f6f >(branch, L_o │ │ │ │ +0001efe0: 665f 4d2c 2059 5f6f 665f 5829 293b 3c2f f_M, Y_of_X));.
    6 │ │ │ │ +0001f040: 3939 3c2f 7370 616e 3e20 2020 207d 3c2f 99 }.
    7 │ │ │ │ +0001f0a0: 3030 3c2f 7370 616e 3e20 2020 203c 7370 00 return LY::compose │ │ │ │ +0001f0e0: 2866 756e 6374 696f 6e73 2e62 6567 696e (functions.begin │ │ │ │ +0001f0f0: 2829 2c20 6675 6e63 7469 6f6e 732e 656e (), functions.en │ │ │ │ +0001f100: 6428 292c 206e 6577 4c61 6265 6c29 3b3c d(), newLabel);< │ │ │ │ +0001f110: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0001f160: 3730 313c 2f73 7061 6e3e 2020 7d3c 2f64 701 }.
    .
    702 │ │ │ │ +0001f1d0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001f220: 2037 3033 3c2f 7370 616e 3e3c 7370 616e 703 /************ │ │ │ │ +0001f250: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001f260: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001f270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001f280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0001f290: 2f3c 2f73 7061 6e3e 3c2f 6469 763e 0a3c /
    .< │ │ │ │ +0001f2a0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0001f2b0: 3e3c 6120 6964 3d22 6c30 3037 3134 2220 > 714 templ │ │ │ │ +0001f310: 6174 653c 2f73 7061 6e3e 2026 6c74 3b3c ate << │ │ │ │ +0001f320: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0001f330: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename
    L, ty │ │ │ │ +0001f360: 7065 6e61 6d65 3c2f 7370 616e 3e20 5926 pename Y& │ │ │ │ +0001f370: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    │ │ │ │ +0001f3c0: 0a3c 6469 7620 636c 6173 733d 226c 696e . │ │ │ │ +0001f4a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0001f4d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 716 u │ │ │ │ +0001f510: 7369 6e67 203c 2f73 7061 6e3e 4620 3d20 sing F = │ │ │ │ +0001f520: 7374 643a 3a66 756e 6374 696f 6e26 6c74 std::function< │ │ │ │ +0001f530: 3b76 6f69 6428 3c73 7061 6e20 636c 6173 ;void(cons │ │ │ │ +0001f550: 743c 2f73 7061 6e3e 2059 2661 6d70 3b29 t Y&) │ │ │ │ +0001f560: 2667 743b 3b3c 2f64 6976 3e0a 3c64 6976 >;
    .
    < │ │ │ │ +0001f5a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0001f5b0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 717 explicit Visit(F │ │ │ │ +0001f690: 3c61 2063 6c61 7373 3d22 636f 6465 2068 f) : f │ │ │ │ +0001f740: 283c 6120 636c 6173 733d 2263 6f64 6520 (f) {} .
    .
    718 F f │ │ │ │ +0001f8e0: 3c2f 613e 3b20 2020 2020 2020 2020 2020 ; │ │ │ │ +0001f8f0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0001f900: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0001f950: 2037 3139 3c2f 7370 616e 3e20 3c2f 6469 719 .
    .
    721< │ │ │ │ +0001fa40: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> void │ │ │ │ +0001fa70: 3c61 2063 6c61 7373 3d22 636f 6465 2068 operator()( │ │ │ │ +0001fad0: 3c73 7061 6e20 636c 6173 733d 226b 6579 const typenam │ │ │ │ +0001fb10: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e Decisio │ │ │ │ +0001fb70: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ +0001fb80: 3b3a 3a4e 6f64 6550 7472 3c2f 613e 2661 ;::NodePtr&a │ │ │ │ +0001fb90: 6d70 3b20 6e6f 6465 293c 7370 616e 2063 mp; node) │ │ │ │ +0001fbb0: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    7 │ │ │ │ +0001fc10: 3232 3c2f 7370 616e 3e20 2020 2020 203c 22 < │ │ │ │ +0001fc20: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0001fc30: 6f72 6422 3e75 7369 6e67 203c 2f73 7061 ord">using Leaf = t │ │ │ │ +0001fc60: 7970 656e 616d 653c 2f73 7061 6e3e 203c ypename < │ │ │ │ +0001fc70: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +0001fc80: 5f73 7472 7563 7422 2068 7265 663d 2261 _struct" href="a │ │ │ │ +0001fc90: 3032 3737 362e 6874 6d6c 223e 4465 6369 02776.html">Deci │ │ │ │ +0001fca0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ +0001fcb0: 2667 743b 3a3a 4c65 6166 3c2f 613e 3b3c >::Leaf;< │ │ │ │ +0001fcc0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0001fd10: 3732 333c 2f73 7061 6e3e 2020 2020 2020 723 │ │ │ │ +0001fd20: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (auto< │ │ │ │ +0001fd60: 2f73 7061 6e3e 206c 6561 6620 3d20 626f /span> leaf = bo │ │ │ │ +0001fd70: 6f73 743a 3a64 796e 616d 6963 5f70 6f69 ost::dynamic_poi │ │ │ │ +0001fd80: 6e74 6572 5f63 6173 7426 6c74 3b63 6f6e nter_cast<con │ │ │ │ +0001fd90: 7374 204c 6561 6626 6774 3b28 6e6f 6465 st Leaf>(node │ │ │ │ +0001fda0: 2929 3c2f 6469 763e 0a3c 6469 7620 636c ))
    .
    724 │ │ │ │ +0001fe00: 2020 2020 203c 7370 616e 2063 6c61 7373 r │ │ │ │ +0001fe20: 6574 7572 6e3c 2f73 7061 6e3e 203c 6120 eturn f< │ │ │ │ +0001fe80: 2f61 3e28 6c65 6166 2d26 6774 3b63 6f6e /a>(leaf->con │ │ │ │ +0001fe90: 7374 616e 7428 2929 3b3c 2f64 6976 3e0a stant());
    . │ │ │ │ +0001fea0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ +0001ff30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +0001ff40: 6e6f 223e 2020 3732 363c 2f73 7061 6e3e no"> 726 │ │ │ │ +0001ff50: 2020 2020 2020 3c73 7061 6e20 636c 6173 usin │ │ │ │ +0001ff70: 6720 3c2f 7370 616e 3e43 686f 6963 6520 g Choice │ │ │ │ +0001ff80: 3d20 3c73 7061 6e20 636c 6173 733d 226b = typename │ │ │ │ +0001ffa0: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 DecisionTre │ │ │ │ +0001ffe0: 6526 6c74 3b4c 2c20 5926 6774 3b3a 3a43 e<L, Y>::C │ │ │ │ +0001fff0: 686f 6963 653c 2f61 3e3b 3c2f 6469 763e hoice;
    │ │ │ │ +00020000: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00020030: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 727 auto cho │ │ │ │ +00020080: 6963 6520 3d20 626f 6f73 743a 3a64 796e ice = boost::dyn │ │ │ │ +00020090: 616d 6963 5f70 6f69 6e74 6572 5f63 6173 amic_pointer_cas │ │ │ │ +000200a0: 7426 6c74 3b63 6f6e 7374 2043 686f 6963 t<const Choic │ │ │ │ +000200b0: 6526 6774 3b28 6e6f 6465 293b 3c2f 6469 e>(node);.
    728 │ │ │ │ +00020110: 3c2f 7370 616e 3e20 2020 2020 203c 7370 if │ │ │ │ +00020140: 2028 2163 686f 6963 6529 3c2f 6469 763e (!choice)
    │ │ │ │ +00020150: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00020180: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 729 throw std::invalid │ │ │ │ +000201e0: 5f61 7267 756d 656e 7428 3c73 7061 6e20 _argument("Deci │ │ │ │ +00020210: 7369 6f6e 5472 6565 3a3a 5669 7369 743a sionTree::Visit: │ │ │ │ +00020220: 2049 6e76 616c 6964 204e 6f64 6550 7472 Invalid NodePtr │ │ │ │ +00020230: 2671 756f 743b 3c2f 7370 616e 3e29 3b3c ");< │ │ │ │ +00020240: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00020290: 3733 303c 2f73 7061 6e3e 2020 2020 2020 730 │ │ │ │ +000202a0: 3c73 7061 6e20 636c 6173 733d 226b 6579 for (auto │ │ │ │ +000202e0: 3c2f 7370 616e 3e26 616d 703b 2661 6d70 && │ │ │ │ +000202f0: 3b20 6272 616e 6368 203a 2063 686f 6963 ; branch : choic │ │ │ │ +00020300: 652d 2667 743b 6272 616e 6368 6573 2829 e->branches() │ │ │ │ +00020310: 2920 282a 3c73 7061 6e20 636c 6173 733d ) (*this)(branch); │ │ │ │ +00020340: 203c 7370 616e 2063 6c61 7373 3d22 636f // recurs │ │ │ │ +00020360: 6521 3c2f 7370 616e 3e3c 2f64 6976 3e0a e!
    . │ │ │ │ +00020370: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ +000203d0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00020420: 2037 3332 3c2f 7370 616e 3e20 207d 3b3c 732 };< │ │ │ │ +00020430: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00020480: 3733 333c 2f73 7061 6e3e 203c 2f64 6976 733
    .
    734< │ │ │ │ +000204e0: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ +00020500: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ +00020510: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ +00020530: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ +00020560: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    .< │ │ │ │ +00020580: 6120 6964 3d22 6c30 3037 3335 2220 6e61 a id="l00735" na │ │ │ │ +00020590: 6d65 3d22 6c30 3037 3335 223e 3c2f 613e me="l00735"> │ │ │ │ +000205a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 735 templat │ │ │ │ +000205e0: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename Func>
    .
    .
    │ │ │ │ +000206b0: 3c61 2063 6c61 7373 3d22 6c69 6e65 2220 736 void De │ │ │ │ +00020780: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00020790: 2059 2667 743b 3a3a 7669 7369 743c 2f61 Y>::visit(Func f) │ │ │ │ +000207c0: 636f 6e73 7420 3c2f 7370 616e 3e7b 3c2f const {.
    7 │ │ │ │ +00020820: 3337 3c2f 7370 616e 3e20 2020 203c 6120 37 Visit& │ │ │ │ +00020860: 6c74 3b4c 2c20 5926 6774 3b3c 2f61 3e20 lt;L, Y> │ │ │ │ +00020870: 7669 7369 7428 6629 3b3c 2f64 6976 3e0a visit(f);
    . │ │ │ │ +00020880: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ +00020920: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00020930: 6e6f 223e 2020 3733 393c 2f73 7061 6e3e no"> 739 │ │ │ │ +00020940: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +00020950: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +000209c0: 6120 6964 3d22 6c30 3037 3431 2220 6e61 a id="l00741" na │ │ │ │ +000209d0: 6d65 3d22 6c30 3037 3431 223e 3c2f 613e me="l00741"> │ │ │ │ +000209e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 741 /****** │ │ │ │ +00020a20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00020a30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00020a40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00020a50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00020a60: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    7 │ │ │ │ +00020ac0: 3531 3c2f 7370 616e 3e20 203c 7370 616e 51 template │ │ │ │ +00020af0: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ +00020b10: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ +00020b50: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ +00020be0: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00020bf0: 7265 663d 2261 3032 3738 382e 6874 6d6c ref="a02788.html │ │ │ │ +00020c00: 223e 2020 3735 323c 2f61 3e3c 2f73 7061 "> 752 struct │ │ │ │ +00020c30: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 VisitLeaf {
    .
    753 │ │ │ │ +00020cd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 using < │ │ │ │ +00020cf0: 2f73 7061 6e3e 4620 3d20 7374 643a 3a66 /span>F = std::f │ │ │ │ +00020d00: 756e 6374 696f 6e26 6c74 3b76 6f69 6428 unction<void( │ │ │ │ +00020d10: 3c73 7061 6e20 636c 6173 733d 226b 6579 const typenam │ │ │ │ +00020d50: 653c 2f73 7061 6e3e 203c 6120 636c 6173 e DecisionTr │ │ │ │ +00020d90: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ +00020da0: 4c65 6166 3c2f 613e 2661 6d70 3b29 2667 Leaf&)&g │ │ │ │ +00020db0: 743b 3b3c 2f64 6976 3e0a 3c64 6976 2063 t;;
    .
    754 │ │ │ │ +00020e50: 3c2f 7370 616e 3e20 2020 203c 7370 616e explicit │ │ │ │ +00020e80: 203c 6120 636c 6173 733d 2263 6f64 6520 VisitLeaf( │ │ │ │ +00020ee0: 4620 3c61 2063 6c61 7373 3d22 636f 6465 F f) : f(f) {} < │ │ │ │ +00020ff0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .< │ │ │ │ +00021050: 6120 6964 3d22 6c30 3037 3535 2220 6e61 a id="l00755" na │ │ │ │ +00021060: 6d65 3d22 6c30 3037 3535 223e 3c2f 613e me="l00755"> │ │ │ │ +00021070: 3c73 7061 6e20 636c 6173 733d 226c 696e 755< │ │ │ │ +000210d0: 2f61 3e3c 2f73 7061 6e3e 2020 2020 4620 /a> F │ │ │ │ +000210e0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 f; │ │ │ │ +00021140: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00021150: 2020 3c2f 6469 763e 0a3c 6469 7620 636c
    .
    756 .
    .
    758 void operator()(const typen │ │ │ │ +00021360: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decis │ │ │ │ +000213c0: 696f 6e54 7265 6526 6c74 3b4c 2c20 5926 ionTree<L, Y& │ │ │ │ +000213d0: 6774 3b3a 3a4e 6f64 6550 7472 3c2f 613e gt;::NodePtr │ │ │ │ +000213e0: 2661 6d70 3b20 6e6f 6465 293c 7370 616e & node) const { │ │ │ │ +00021410: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00021460: 2037 3539 3c2f 7370 616e 3e20 2020 2020 759 │ │ │ │ +00021470: 203c 7370 616e 2063 6c61 7373 3d22 6b65 using Leaf = typename │ │ │ │ +000214c0: 203c 6120 636c 6173 733d 2263 6f64 6520 De │ │ │ │ +000214f0: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00021500: 2059 2667 743b 3a3a 4c65 6166 3c2f 613e Y>::Leaf │ │ │ │ +00021510: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +00021560: 2020 3736 303c 2f73 7061 6e3e 2020 2020 760 │ │ │ │ +00021570: 2020 3c73 7061 6e20 636c 6173 733d 226b if (aut │ │ │ │ +000215b0: 6f3c 2f73 7061 6e3e 206c 6561 6620 3d20 o leaf = │ │ │ │ +000215c0: 626f 6f73 743a 3a64 796e 616d 6963 5f70 boost::dynamic_p │ │ │ │ +000215d0: 6f69 6e74 6572 5f63 6173 7426 6c74 3b63 ointer_cast<c │ │ │ │ +000215e0: 6f6e 7374 204c 6561 6626 6774 3b28 6e6f onst Leaf>(no │ │ │ │ +000215f0: 6465 2929 3c2f 6469 763e 0a3c 6469 7620 de))
    .
    761 │ │ │ │ +00021650: 2020 2020 2020 203c 7370 616e 2063 6c61 return < │ │ │ │ +00021680: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00021690: 5f76 6172 6961 626c 6522 2068 7265 663d _variable" href= │ │ │ │ +000216a0: 2261 3032 3738 382e 6874 6d6c 2361 6330 "a02788.html#ac0 │ │ │ │ +000216b0: 6231 6136 6637 3233 3434 6638 6363 3436 b1a6f72344f8cc46 │ │ │ │ +000216c0: 3038 6130 3564 6162 3433 3961 3461 223e 08a05dab439a4a"> │ │ │ │ +000216d0: 663c 2f61 3e28 2a6c 6561 6629 3b3c 2f64 f(*leaf);.
    76 │ │ │ │ +00021730: 323c 2f73 7061 6e3e 203c 2f64 6976 3e0a 2
    . │ │ │ │ +00021740: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00021770: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 763 │ │ │ │ +000217b0: 7573 696e 6720 3c2f 7370 616e 3e43 686f using Cho │ │ │ │ +000217c0: 6963 6520 3d20 3c73 7061 6e20 636c 6173 ice = type │ │ │ │ +000217e0: 6e61 6d65 3c2f 7370 616e 3e20 3c61 2063 name Decisio │ │ │ │ +00021820: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ +00021830: 3b3a 3a43 686f 6963 653c 2f61 3e3b 3c2f ;::Choice;.
    7 │ │ │ │ +00021890: 3634 3c2f 7370 616e 3e20 2020 2020 203c 64 < │ │ │ │ +000218a0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000218b0: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ +000218c0: 2063 686f 6963 6520 3d20 626f 6f73 743a choice = boost: │ │ │ │ +000218d0: 3a64 796e 616d 6963 5f70 6f69 6e74 6572 :dynamic_pointer │ │ │ │ +000218e0: 5f63 6173 7426 6c74 3b63 6f6e 7374 2043 _cast<const C │ │ │ │ +000218f0: 686f 6963 6526 6774 3b28 6e6f 6465 293b hoice>(node); │ │ │ │ +00021900: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00021950: 2037 3635 3c2f 7370 616e 3e20 2020 2020 765 │ │ │ │ +00021960: 203c 7370 616e 2063 6c61 7373 3d22 6b65 if (!choice).
    7 │ │ │ │ +000219e0: 3636 3c2f 7370 616e 3e20 2020 2020 2020 66 │ │ │ │ +000219f0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 throw │ │ │ │ +00021a10: 3c2f 7370 616e 3e20 7374 643a 3a69 6e76 std::inv │ │ │ │ +00021a20: 616c 6964 5f61 7267 756d 656e 7428 3c73 alid_argument(" │ │ │ │ +00021a50: 4465 6369 7369 6f6e 5472 6565 3a3a 5669 DecisionTree::Vi │ │ │ │ +00021a60: 7369 744c 6561 663a 2049 6e76 616c 6964 sitLeaf: Invalid │ │ │ │ +00021a70: 204e 6f64 6550 7472 2671 756f 743b 3c2f NodePtr");
    . │ │ │ │ +00021aa0: 3c61 2069 643d 226c 3030 3736 3722 206e 767 for (auto& │ │ │ │ +00021b30: 616d 703b 2661 6d70 3b20 6272 616e 6368 amp;& branch │ │ │ │ +00021b40: 203a 2063 686f 6963 652d 2667 743b 6272 : choice->br │ │ │ │ +00021b50: 616e 6368 6573 2829 2920 282a 3c73 7061 anches()) (*this)(b │ │ │ │ +00021b80: 7261 6e63 6829 3b20 203c 7370 616e 2063 ranch); / │ │ │ │ +00021ba0: 2f20 7265 6375 7273 6521 3c2f 7370 616e / recurse!
    .
    │ │ │ │ +00021c00: 2020 3736 383c 2f73 7061 6e3e 2020 2020 768 │ │ │ │ +00021c10: 7d3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c }
    .
    .< │ │ │ │ +00021c20: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00021c30: 3e3c 6120 6964 3d22 6c30 3037 3639 2220 > 769 };
    . │ │ │ │ +00021c90: 3c61 2069 643d 226c 3030 3737 3022 206e 770
    .
    771 │ │ │ │ +00021d30: 3c73 7061 6e20 636c 6173 733d 226b 6579 template < │ │ │ │ +00021d70: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +00021d80: 4c2c 203c 7370 616e 2063 6c61 7373 3d22 L, typenam │ │ │ │ +00021da0: 653c 2f73 7061 6e3e 2059 2667 743b 3c2f e Y>.
    7 │ │ │ │ +00021e00: 3732 3c2f 7370 616e 3e20 203c 7370 616e 72 template │ │ │ │ +00021e30: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ +00021e50: 616d 653c 2f73 7061 6e3e 2046 756e 6326 ame Func& │ │ │ │ +00021e60: 6774 3b3c 2f64 6976 3e0a 3c64 6976 2063 gt;
    .
    . │ │ │ │ +00021eb0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    < │ │ │ │ +00021ee0: 2f61 3e3c 7370 616e 2063 6c61 7373 3d22 /a> 7 │ │ │ │ +00021f40: 3733 3c2f 613e 3c2f 7370 616e 3e20 203c 73 < │ │ │ │ +00021f50: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00021f60: 6f72 6474 7970 6522 3e76 6f69 643c 2f73 ordtype">void DecisionTr │ │ │ │ +00021fd0: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ +00021fe0: 7669 7369 744c 6561 663c 2f61 3e28 4675 visitLeaf(Fu │ │ │ │ +00021ff0: 6e63 2066 293c 7370 616e 2063 6c61 7373 nc f) cons │ │ │ │ +00022010: 7420 3c2f 7370 616e 3e7b 3c2f 6469 763e t {
    │ │ │ │ +00022020: 0a3c 6469 7620 636c 6173 733d 226c 696e .. │ │ │ │ +000220d0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    < │ │ │ │ +00022170: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00022180: 6e6f 223e 2020 3737 363c 2f73 7061 6e3e no"> 776 │ │ │ │ +00022190: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +000221a0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00022210: 6120 6964 3d22 6c30 3037 3738 2220 6e61 a id="l00778" na │ │ │ │ +00022220: 6d65 3d22 6c30 3037 3738 223e 3c2f 613e me="l00778"> │ │ │ │ +00022230: 3c73 7061 6e20 636c 6173 733d 226c 696e 778 /****** │ │ │ │ +00022270: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00022280: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00022290: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000222a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000222b0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    7 │ │ │ │ +00022310: 3835 3c2f 7370 616e 3e20 203c 7370 616e 85 template │ │ │ │ +00022340: 2026 6c74 3b3c 7370 616e 2063 6c61 7373 <typen │ │ │ │ +00022360: 616d 653c 2f73 7061 6e3e 204c 2c20 3c73 ame L, typename Y>
    . │ │ │ │ +000223a0: 3c64 6976 2063 6c61 7373 3d22 666f 6c64
    .
    < │ │ │ │ +00022430: 6120 636c 6173 733d 226c 696e 6522 2068 a class="line" h │ │ │ │ +00022440: 7265 663d 2261 3032 3739 322e 6874 6d6c ref="a02792.html │ │ │ │ +00022450: 223e 2020 3738 363c 2f61 3e3c 2f73 7061 "> 786 struct │ │ │ │ +00022480: 203c 2f73 7061 6e3e 3c61 2063 6c61 7373 VisitWith {
    .
    787 │ │ │ │ +00022520: 2020 203c 7370 616e 2063 6c61 7373 3d22 using < │ │ │ │ +00022540: 2f73 7061 6e3e 4620 3d20 7374 643a 3a66 /span>F = std::f │ │ │ │ +00022550: 756e 6374 696f 6e26 6c74 3b76 6f69 6428 unction<void( │ │ │ │ +00022560: 3c73 7061 6e20 636c 6173 733d 226b 6579 const A │ │ │ │ +000225b0: 7373 6967 6e6d 656e 7426 6c74 3b4c 2667 ssignment<L&g │ │ │ │ +000225c0: 743b 3c2f 613e 2661 6d70 3b2c 203c 7370 t;&, const │ │ │ │ +000225f0: 5926 616d 703b 2926 6774 3b3b 3c2f 6469 Y&)>;.
    │ │ │ │ +00022690: 2020 3738 383c 2f61 3e3c 2f73 7061 6e3e 788 │ │ │ │ +000226a0: 2020 2020 3c73 7061 6e20 636c 6173 733d explic │ │ │ │ +000226c0: 6974 3c2f 7370 616e 3e20 3c61 2063 6c61 it Visit │ │ │ │ +00022720: 5769 7468 3c2f 613e 2846 203c 6120 636c With(F f) : f(f) {}
    .< │ │ │ │ +00022840: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ +00022850: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ +00022860: 6e30 3037 3839 2220 6461 7461 2d73 7461 n00789" data-sta │ │ │ │ +00022870: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ +00022880: 227d 3b22 3e0a 3c64 6976 2063 6c61 7373 "};">.
    789 Assignment< │ │ │ │ +00022960: 3b4c 2667 743b 3c2f 613e 203c 6120 636c ;L> assi │ │ │ │ +000229c0: 676e 6d65 6e74 3c2f 613e 3b20 203c 2f64 gnment; .
    790 F f; │ │ │ │ +00022ad0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00022ae0: 2020 2020 203c 2f64 6976 3e0a 3c64 6976
    .
    < │ │ │ │ +00022b20: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00022b30: 6e6f 223e 2020 3739 313c 2f73 7061 6e3e no"> 791 │ │ │ │ +00022b40: 203c 2f64 6976 3e0a 3c64 6976 2063 6c61
    .
    . │ │ │ │ +00022ba0: 3c61 2069 643d 226c 3030 3739 3322 206e 793 │ │ │ │ +00022c20: 3c2f 613e 3c2f 7370 616e 3e20 2020 203c < │ │ │ │ +00022c30: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00022c40: 6f72 6474 7970 6522 3e76 6f69 643c 2f73 ordtype">void operator() │ │ │ │ +00022cb0: 3c2f 613e 283c 7370 616e 2063 6c61 7373 (const │ │ │ │ +00022cd0: 3c2f 7370 616e 3e20 3c73 7061 6e20 636c ty │ │ │ │ +00022cf0: 7065 6e61 6d65 3c2f 7370 616e 3e20 3c61 pename De │ │ │ │ +00022d50: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00022d60: 2059 2667 743b 3a3a 4e6f 6465 5074 723c Y>::NodePtr< │ │ │ │ +00022d70: 2f61 3e26 616d 703b 206e 6f64 6529 207b /a>& node) { │ │ │ │ +00022d80: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00022dd0: 2037 3934 3c2f 7370 616e 3e20 2020 2020 794 │ │ │ │ +00022de0: 203c 7370 616e 2063 6c61 7373 3d22 6b65 using Leaf = typename │ │ │ │ +00022e30: 203c 6120 636c 6173 733d 2263 6f64 6520 De │ │ │ │ +00022e60: 6369 7369 6f6e 5472 6565 266c 743b 4c2c cisionTree<L, │ │ │ │ +00022e70: 2059 2667 743b 3a3a 4c65 6166 3c2f 613e Y>::Leaf │ │ │ │ +00022e80: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +00022ed0: 2020 3739 353c 2f73 7061 6e3e 2020 2020 795 │ │ │ │ +00022ee0: 2020 3c73 7061 6e20 636c 6173 733d 226b if (aut │ │ │ │ +00022f20: 6f3c 2f73 7061 6e3e 206c 6561 6620 3d20 o leaf = │ │ │ │ +00022f30: 626f 6f73 743a 3a64 796e 616d 6963 5f70 boost::dynamic_p │ │ │ │ +00022f40: 6f69 6e74 6572 5f63 6173 7426 6c74 3b63 ointer_cast<c │ │ │ │ +00022f50: 6f6e 7374 204c 6561 6626 6774 3b28 6e6f onst Leaf>(no │ │ │ │ +00022f60: 6465 2929 3c2f 6469 763e 0a3c 6469 7620 de))
    .
    796 │ │ │ │ +00022fc0: 2020 2020 2020 203c 7370 616e 2063 6c61 return < │ │ │ │ +00022ff0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00023000: 5f76 6172 6961 626c 6522 2068 7265 663d _variable" href= │ │ │ │ +00023010: 2261 3032 3739 322e 6874 6d6c 2361 6463 "a02792.html#adc │ │ │ │ +00023020: 3463 6662 3532 6434 3430 3932 6364 6165 4cfb52d44092cdae │ │ │ │ +00023030: 3966 3136 6365 6461 3835 3066 3535 223e 9f16ceda850f55"> │ │ │ │ +00023040: 663c 2f61 3e28 3c61 2063 6c61 7373 3d22 f(assignmen │ │ │ │ +000230a0: 743c 2f61 3e2c 206c 6561 662d 2667 743b t, leaf-> │ │ │ │ +000230b0: 636f 6e73 7461 6e74 2829 293b 3c2f 6469 constant());.
    797 │ │ │ │ +00023110: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +00023120: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00023130: 3e3c 6120 6964 3d22 6c30 3037 3938 2220 > 798 u │ │ │ │ +00023190: 7369 6e67 203c 2f73 7061 6e3e 4368 6f69 sing Choi │ │ │ │ +000231a0: 6365 203d 203c 7370 616e 2063 6c61 7373 ce = typen │ │ │ │ +000231c0: 616d 653c 2f73 7061 6e3e 203c 6120 636c ame Decision │ │ │ │ +00023200: 5472 6565 266c 743b 4c2c 2059 2667 743b Tree<L, Y> │ │ │ │ +00023210: 3a3a 4368 6f69 6365 3c2f 613e 3b3c 2f64 ::Choice;.
    79 │ │ │ │ +00023270: 393c 2f73 7061 6e3e 2020 2020 2020 3c73 9 auto │ │ │ │ +000232a0: 6368 6f69 6365 203d 2062 6f6f 7374 3a3a choice = boost:: │ │ │ │ +000232b0: 6479 6e61 6d69 635f 706f 696e 7465 725f dynamic_pointer_ │ │ │ │ +000232c0: 6361 7374 266c 743b 636f 6e73 7420 4368 cast<const Ch │ │ │ │ +000232d0: 6f69 6365 2667 743b 286e 6f64 6529 3b3c oice>(node);< │ │ │ │ +000232e0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00023330: 3830 303c 2f73 7061 6e3e 2020 2020 2020 800 │ │ │ │ +00023340: 3c73 7061 6e20 636c 6173 733d 226b 6579 if (!choice).
    80 │ │ │ │ +000233c0: 313c 2f73 7061 6e3e 2020 2020 2020 2020 1 │ │ │ │ +000233d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 throw< │ │ │ │ +000233f0: 2f73 7061 6e3e 2073 7464 3a3a 696e 7661 /span> std::inva │ │ │ │ +00023400: 6c69 645f 6172 6775 6d65 6e74 283c 7370 lid_argument("D │ │ │ │ +00023430: 6563 6973 696f 6e54 7265 653a 3a56 6973 ecisionTree::Vis │ │ │ │ +00023440: 6974 5769 7468 3a20 496e 7661 6c69 6420 itWith: Invalid │ │ │ │ +00023450: 4e6f 6465 5074 7226 7175 6f74 3b3c 2f73 NodePtr");
    .< │ │ │ │ +00023480: 6120 6964 3d22 6c30 3038 3032 2220 6e61 a id="l00802" na │ │ │ │ +00023490: 6d65 3d22 6c30 3038 3032 223e 3c2f 613e me="l00802"> │ │ │ │ +000234a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 802 for (size_t
    i = 0; i &l │ │ │ │ +00023520: 743b 2063 686f 6963 652d 2667 743b 6e72 t; choice->nr │ │ │ │ +00023530: 4368 6f69 6365 7328 293b 2069 2b2b 2920 Choices(); i++) │ │ │ │ +00023540: 7b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 {
    ..
    804< │ │ │ │ +000236b0: 2f73 7061 6e3e 203c 2f64 6976 3e0a 3c64 /span>
    . │ │ │ │ +000236d0: 3c61 2069 643d 226c 3030 3830 3522 206e 805 (*this │ │ │ │ +00023720: 2928 6368 6f69 6365 2d26 6774 3b62 7261 )(choice->bra │ │ │ │ +00023730: 6e63 6865 7328 295b 695d 293b 2020 3c73 nches()[i]); // recurse!< │ │ │ │ +00023760: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00023780: 6120 6964 3d22 6c30 3038 3036 2220 6e61 a id="l00806" na │ │ │ │ +00023790: 6d65 3d22 6c30 3038 3036 223e 3c2f 613e me="l00806"> │ │ │ │ +000237a0: 3c73 7061 6e20 636c 6173 733d 226c 696e 806
    .
    807 │ │ │ │ +00023820: 2020 2020 203c 7370 616e 2063 6c61 7373 // Re │ │ │ │ +00023840: 6d6f 7665 2074 6865 2063 686f 6963 6520 move the choice │ │ │ │ +00023850: 736f 2077 6520 6172 6520 6261 636b 7472 so we are backtr │ │ │ │ +00023860: 6163 6b69 6e67 3c2f 7370 616e 3e3c 2f64 acking.
    80 │ │ │ │ +000238c0: 383c 2f73 7061 6e3e 2020 2020 2020 2020 8 │ │ │ │ +000238d0: 3c73 7061 6e20 636c 6173 733d 226b 6579 auto choice_it = a │ │ │ │ +00023950: 7373 6967 6e6d 656e 743c 2f61 3e2e 6669 ssignment.fi │ │ │ │ +00023960: 6e64 2863 686f 6963 652d 2667 743b 6c61 nd(choice->la │ │ │ │ +00023970: 6265 6c28 2929 3b3c 2f64 6976 3e0a 3c64 bel());
    . │ │ │ │ +00023990: 3c61 2069 643d 226c 3030 3830 3922 206e 809
    assig │ │ │ │ +00023a30: 6e6d 656e 743c 2f61 3e2e 6572 6173 6528 nment.erase( │ │ │ │ +00023a40: 6368 6f69 6365 5f69 7429 3b3c 2f64 6976 choice_it);
    .
    810< │ │ │ │ +00023aa0: 2f73 7061 6e3e 2020 2020 2020 7d3c 2f64 /span> }.
    81 │ │ │ │ +00023b00: 313c 2f73 7061 6e3e 2020 2020 7d3c 2f64 1 }.
    .
    812 │ │ │ │ +00023b70: 207d 3b3c 2f64 6976 3e0a 3c64 6976 2063 };
    .
    813 < │ │ │ │ +00023bd0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00023c20: 3831 343c 2f73 7061 6e3e 2020 3c73 7061 814 template <type │ │ │ │ +00023c70: 6e61 6d65 3c2f 7370 616e 3e20 4c2c 203c name L, < │ │ │ │ +00023c80: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00023c90: 6f72 6422 3e74 7970 656e 616d 653c 2f73 ord">typename Y>
    │ │ │ │ +00023cb0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00023ce0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 815 tem │ │ │ │ +00023d20: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ +00023d30: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ +00023d50: 2f73 7061 6e3e 2046 756e 6326 6774 3b3c /span> Func>< │ │ │ │ +00023d60: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    .
    < │ │ │ │ +00023de0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00023df0: 6e6f 223e 3c61 2063 6c61 7373 3d22 6c69 no"> 816 void │ │ │ │ +00023e70: 203c 6120 636c 6173 733d 2263 6f64 6520 DecisionTree&l │ │ │ │ +00023ed0: 743b 4c2c 2059 2667 743b 3a3a 7669 7369 t;L, Y>::visi │ │ │ │ +00023ee0: 7457 6974 683c 2f61 3e28 4675 6e63 2066 tWith(Func f │ │ │ │ +00023ef0: 293c 7370 616e 2063 6c61 7373 3d22 6b65 ) const {
    .< │ │ │ │ +00023f30: 6120 6964 3d22 6c30 3038 3137 2220 6e61 a id="l00817" na │ │ │ │ +00023f40: 6d65 3d22 6c30 3038 3137 223e 3c2f 613e me="l00817"> │ │ │ │ +00023f50: 3c73 7061 6e20 636c 6173 733d 226c 696e 817 VisitWith<L │ │ │ │ +00023fb0: 2c20 5926 6774 3b3c 2f61 3e20 7669 7369 , Y> visi │ │ │ │ +00023fc0: 7428 6629 3b3c 2f64 6976 3e0a 3c64 6976 t(f);
    .
    < │ │ │ │ +00024000: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00024010: 6e6f 223e 2020 3831 383c 2f73 7061 6e3e no"> 818 │ │ │ │ +00024020: 2020 2020 7669 7369 7428 726f 6f74 5f29 visit(root_) │ │ │ │ +00024030: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    │ │ │ │ +00024080: 2020 3831 393c 2f73 7061 6e3e 2020 7d3c 819 }< │ │ │ │ +00024090: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ +000240b0: 6120 6964 3d22 6c30 3038 3230 2220 6e61 a id="l00820" na │ │ │ │ +000240c0: 6d65 3d22 6c30 3038 3230 223e 3c2f 613e me="l00820"> │ │ │ │ +000240d0: 3c73 7061 6e20 636c 6173 733d 226c 696e 820
    .
    821 /********** │ │ │ │ +00024170: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024180: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024190: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000241a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000241b0: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
    │ │ │ │ +000241c0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000241f0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 822 tem │ │ │ │ +00024230: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ +00024240: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ +00024260: 2f73 7061 6e3e 204c 2c20 3c73 7061 6e20 /span> L, │ │ │ │ +00024280: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +00024290: 5926 6774 3b3c 2f64 6976 3e0a 3c64 6976 Y>
    .
    .
    │ │ │ │ +00024370: 2038 3233 3c2f 613e 3c2f 7370 616e 3e20 823 │ │ │ │ +00024380: 203c 7370 616e 2063 6c61 7373 3d22 6b65 size_ │ │ │ │ +000243a0: 743c 2f73 7061 6e3e 203c 6120 636c 6173 t Decisi │ │ │ │ +00024400: 6f6e 5472 6565 266c 743b 4c2c 2059 2667 onTree<L, Y&g │ │ │ │ +00024410: 743b 3a3a 6e72 4c65 6176 6573 3c2f 613e t;::nrLeaves │ │ │ │ +00024420: 2829 3c73 7061 6e20 636c 6173 733d 226b () const < │ │ │ │ +00024440: 2f73 7061 6e3e 7b3c 2f64 6976 3e0a 3c64 /span>{
    . │ │ │ │ +00024460: 3c61 2069 643d 226c 3030 3832 3422 206e 824 │ │ │ │ +000244c0: 7369 7a65 5f74 3c2f 7370 616e 3e20 746f size_t to │ │ │ │ +000244d0: 7461 6c20 3d20 303b 3c2f 6469 763e 0a3c tal = 0;
    .< │ │ │ │ +000244e0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000244f0: 3e3c 6120 6964 3d22 6c30 3038 3235 2220 > 825 visit([&a │ │ │ │ +00024540: 6d70 3b74 6f74 616c 5d28 3c73 7061 6e20 mp;total]( │ │ │ │ +00024560: 636f 6e73 743c 2f73 7061 6e3e 2059 2661 const Y&a │ │ │ │ +00024570: 6d70 3b20 6e6f 6465 2920 7b20 746f 7461 mp; node) { tota │ │ │ │ +00024580: 6c20 2b3d 2031 3b20 7d29 3b3c 2f64 6976 l += 1; });
    .
    826< │ │ │ │ +000245e0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> return total;
    .< │ │ │ │ +00024620: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00024630: 3e3c 6120 6964 3d22 6c30 3038 3237 2220 > 827 }
    ..
    82 │ │ │ │ +000246d0: 383c 2f73 7061 6e3e 203c 2f64 6976 3e0a 8
    . │ │ │ │ +000246e0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +000247f0: 2020 3833 303c 2f73 7061 6e3e 2020 3c73 830 // fold is j │ │ │ │ +00024820: 7573 7420 646f 6e65 2077 6974 6820 6120 ust done with a │ │ │ │ +00024830: 7669 7369 743c 2f73 7061 6e3e 3c2f 6469 visit.
    831 │ │ │ │ +00024890: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +000248b0: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ +000248c0: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ +000248e0: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    . │ │ │ │ +00024930: 3c61 2069 643d 226c 3030 3833 3222 206e 832 templa │ │ │ │ +00024990: 7465 3c2f 7370 616e 3e20 266c 743b 3c73 te <typename Func, │ │ │ │ +000249e0: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +000249f0: 5826 6774 3b3c 2f64 6976 3e0a 3c64 6976 X>
    ..
    │ │ │ │ +00024d10: 2020 3833 363c 2f73 7061 6e3e 2020 7d3c 836 }< │ │ │ │ +00024d20: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 6469 /div>.
    .< │ │ │ │ +00024d40: 6120 6964 3d22 6c30 3038 3337 2220 6e61 a id="l00837" na │ │ │ │ +00024d50: 6d65 3d22 6c30 3038 3337 223e 3c2f 613e me="l00837"> │ │ │ │ +00024d60: 3c73 7061 6e20 636c 6173 733d 226c 696e 837
    .
    838 /********** │ │ │ │ +00024e00: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024e10: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024e20: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024e30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00024e40: 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 763e **/
    │ │ │ │ +00024e50: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00024e80: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 852 tem │ │ │ │ +00024ec0: 706c 6174 653c 2f73 7061 6e3e 2026 6c74 plate < │ │ │ │ +00024ed0: 3b3c 7370 616e 2063 6c61 7373 3d22 6b65 ;typename< │ │ │ │ +00024ef0: 2f73 7061 6e3e 204c 2c20 3c73 7061 6e20 /span> L, │ │ │ │ +00024f10: 7479 7065 6e61 6d65 3c2f 7370 616e 3e20 typename │ │ │ │ +00024f20: 5926 6774 3b3c 2f64 6976 3e0a 3c64 6976 Y>
    .
    .
    │ │ │ │ +00025000: 2038 3533 3c2f 613e 3c2f 7370 616e 3e20 853 │ │ │ │ +00025010: 2073 7464 3a3a 7365 7426 6c74 3b4c 2667 std::set<L&g │ │ │ │ +00025020: 743b 203c 6120 636c 6173 733d 2263 6f64 t; DecisionTree │ │ │ │ +00025080: 266c 743b 4c2c 2059 2667 743b 3a3a 6c61 <L, Y>::la │ │ │ │ +00025090: 6265 6c73 3c2f 613e 2829 3c73 7061 6e20 bels() │ │ │ │ +000250b0: 2063 6f6e 7374 203c 2f73 7061 6e3e 7b3c const {< │ │ │ │ +000250c0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00025110: 3835 343c 2f73 7061 6e3e 2020 2020 7374 854 st │ │ │ │ +00025120: 643a 3a73 6574 266c 743b 4c26 6774 3b20 d::set<L> │ │ │ │ +00025130: 756e 6971 7565 3b3c 2f64 6976 3e0a 3c64 unique;
    . │ │ │ │ +00025150: 3c61 2069 643d 226c 3030 3835 3522 206e 855 auto │ │ │ │ +000251b0: 3c2f 7370 616e 3e20 6620 3d20 5b26 616d f = [&am │ │ │ │ +000251c0: 703b 5d28 3c73 7061 6e20 636c 6173 733d p;](const< │ │ │ │ +000251e0: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> Assignment< │ │ │ │ +00025220: 3b4c 2667 743b 3c2f 613e 2661 6d70 3b20 ;L>& │ │ │ │ +00025230: 6173 7369 676e 6d65 6e74 2c20 3c73 7061 assignment, const Y │ │ │ │ +00025260: 2661 6d70 3b29 207b 3c2f 6469 763e 0a3c &) {
    .< │ │ │ │ +00025270: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00025280: 3e3c 6120 6964 3d22 6c30 3038 3536 2220 > 856 for (< │ │ │ │ +000252f0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00025300: 6f72 6422 3e61 7574 6f3c 2f73 7061 6e3e ord">auto │ │ │ │ +00025310: 2661 6d70 3b26 616d 703b 206b 7620 3a20 && kv : │ │ │ │ +00025320: 6173 7369 676e 6d65 6e74 2920 7b3c 2f64 assignment) {.
    85 │ │ │ │ +00025380: 373c 2f73 7061 6e3e 2020 2020 2020 2020 7 │ │ │ │ +00025390: 756e 6971 7565 2e69 6e73 6572 7428 6b76 unique.insert(kv │ │ │ │ +000253a0: 2e66 6972 7374 293b 3c2f 6469 763e 0a3c .first);
    .< │ │ │ │ +000253b0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +000253c0: 3e3c 6120 6964 3d22 6c30 3038 3538 2220 > 858 }
    │ │ │ │ +00025410: 0a3c 6469 7620 636c 6173 733d 226c 696e ..
    860< │ │ │ │ +000254c0: 2f73 7061 6e3e 2020 2020 7669 7369 7457 /span> visitW │ │ │ │ +000254d0: 6974 6828 6629 3b3c 2f64 6976 3e0a 3c64 ith(f);
    . │ │ │ │ +000254f0: 3c61 2069 643d 226c 3030 3836 3122 206e 861 │ │ │ │ +00025550: 7265 7475 726e 3c2f 7370 616e 3e20 756e return un │ │ │ │ +00025560: 6971 7565 3b3c 2f64 6976 3e0a 3c64 6976 ique;
    .
    < │ │ │ │ +000255a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000255b0: 6e6f 223e 2020 3836 323c 2f73 7061 6e3e no"> 862 │ │ │ │ +000255c0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +000255d0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00025640: 6120 6964 3d22 6c30 3038 3634 2220 6e61 a id="l00864" na │ │ │ │ +00025650: 6d65 3d22 6c30 3038 3634 223e 3c2f 613e me="l00864"> │ │ │ │ +00025660: 3c73 7061 6e20 636c 6173 733d 226c 696e 864/******** │ │ │ │ +000256a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000256b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000256c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000256d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000256e0: 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f 6469 ****/.
    865 │ │ │ │ +00025740: 3c2f 7370 616e 3e20 203c 7370 616e 2063 t │ │ │ │ +00025760: 656d 706c 6174 653c 2f73 7061 6e3e 2026 emplate & │ │ │ │ +00025770: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ +00025790: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    . │ │ │ │ +000257e0: 3c61 2069 643d 226c 3030 3836 3622 206e 866 bo │ │ │ │ +00025840: 6f6c 3c2f 7370 616e 3e20 3c61 2063 6c61 ol DecisionTr │ │ │ │ +00025880: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ +00025890: 6571 7561 6c73 3c2f 613e 283c 7370 616e equals(const Decisi │ │ │ │ +000258f0: 6f6e 5472 6565 3c2f 613e 2661 6d70 3b20 onTree& │ │ │ │ +00025900: 6f74 6865 722c 3c2f 6469 763e 0a3c 6469 other,
    .< │ │ │ │ +00025920: 6120 6964 3d22 6c30 3038 3637 2220 6e61 a id="l00867" na │ │ │ │ +00025930: 6d65 3d22 6c30 3038 3637 223e 3c2f 613e me="l00867"> │ │ │ │ +00025940: 3c73 7061 6e20 636c 6173 733d 226c 696e 867 │ │ │ │ +00025970: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00025980: 2020 203c 7370 616e 2063 6c61 7373 3d22 const CompareFun │ │ │ │ +000259b0: 6326 616d 703b 2063 6f6d 7061 7265 293c c& compare)< │ │ │ │ +000259c0: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +000259d0: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    868 │ │ │ │ +00025a40: 2020 203c 7370 616e 2063 6c61 7373 3d22 ret │ │ │ │ +00025a60: 7572 6e3c 2f73 7061 6e3e 2072 6f6f 745f urn root_ │ │ │ │ +00025a70: 2d26 6774 3b65 7175 616c 7328 2a6f 7468 ->equals(*oth │ │ │ │ +00025a80: 6572 2e3c 6120 636c 6173 733d 2263 6f64 er.root_, c │ │ │ │ +00025ae0: 6f6d 7061 7265 293b 3c2f 6469 763e 0a3c ompare);
    .< │ │ │ │ +00025af0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00025b00: 3e3c 6120 6964 3d22 6c30 3038 3639 2220 > 869 }
    .< │ │ │ │ +00025b60: 6120 6964 3d22 6c30 3038 3730 2220 6e61 a id="l00870" na │ │ │ │ +00025b70: 6d65 3d22 6c30 3038 3730 223e 3c2f 613e me="l00870"> │ │ │ │ +00025b80: 3c73 7061 6e20 636c 6173 733d 226c 696e 870
    .
    871 < │ │ │ │ +00025c00: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00025c10: 6f72 6422 3e74 656d 706c 6174 653c 2f73 ord">template <t │ │ │ │ +00025c40: 7970 656e 616d 653c 2f73 7061 6e3e 204c ypename L │ │ │ │ +00025c50: 2c20 3c73 7061 6e20 636c 6173 733d 226b , typename │ │ │ │ +00025c70: 3c2f 7370 616e 3e20 5926 6774 3b3c 2f64 Y>.
    .
    872 │ │ │ │ +00025d60: 3c2f 7370 616e 3e20 203c 7370 616e 2063 void < │ │ │ │ +00025d90: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +00025da0: 5f66 756e 6374 696f 6e22 2068 7265 663d _function" href= │ │ │ │ +00025db0: 2261 3032 3739 362e 6874 6d6c 2361 3930 "a02796.html#a90 │ │ │ │ +00025dc0: 3835 3439 3336 3135 3938 6562 6364 3661 8549361598ebcd6a │ │ │ │ +00025dd0: 6434 3061 3565 3565 6562 6337 3363 223e d40a5e5eebc73c"> │ │ │ │ +00025de0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ +00025df0: 4c2c 2059 2667 743b 3a3a 7072 696e 743c L, Y>::print< │ │ │ │ +00025e00: 2f61 3e28 3c73 7061 6e20 636c 6173 733d /a>(const< │ │ │ │ +00025e20: 2f73 7061 6e3e 2073 7464 3a3a 7374 7269 /span> std::stri │ │ │ │ +00025e30: 6e67 2661 6d70 3b20 732c 3c2f 6469 763e ng& s,
    │ │ │ │ +00025e40: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +00025e70: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 873 │ │ │ │ +00025ea0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00025eb0: 2020 2020 2020 3c73 7061 6e20 636c 6173 cons │ │ │ │ +00025ed0: 743c 2f73 7061 6e3e 204c 6162 656c 466f t LabelFo │ │ │ │ +00025ee0: 726d 6174 7465 7226 616d 703b 206c 6162 rmatter& lab │ │ │ │ +00025ef0: 656c 466f 726d 6174 7465 722c 3c2f 6469 elFormatter,.
    874 │ │ │ │ +00025f50: 3c2f 7370 616e 3e20 2020 2020 2020 2020 │ │ │ │ +00025f60: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00025f70: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ +00025f90: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ +00025fa0: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ +00025fb0: 616c 7565 466f 726d 6174 7465 7229 3c73 alueFormatter) const {
    .
    875 │ │ │ │ +00026040: 2020 726f 6f74 5f2d 2667 743b 7072 696e root_->prin │ │ │ │ +00026050: 7428 732c 206c 6162 656c 466f 726d 6174 t(s, labelFormat │ │ │ │ +00026060: 7465 722c 2076 616c 7565 466f 726d 6174 ter, valueFormat │ │ │ │ +00026070: 7465 7229 3b3c 2f64 6976 3e0a 3c64 6976 ter);
    .
    < │ │ │ │ +000260b0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000260c0: 6e6f 223e 2020 3837 363c 2f73 7061 6e3e no"> 876 │ │ │ │ +000260d0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +000260e0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00026150: 6120 6964 3d22 6c30 3038 3738 2220 6e61 a id="l00878" na │ │ │ │ +00026160: 6d65 3d22 6c30 3038 3738 223e 3c2f 613e me="l00878"> │ │ │ │ +00026170: 3c73 7061 6e20 636c 6173 733d 226c 696e 878 templat │ │ │ │ +000261b0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ +00026200: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ +00026210: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ +00026270: 6120 6964 3d22 6c30 3038 3739 2220 6e61 a id="l00879" na │ │ │ │ +00026280: 6d65 3d22 6c30 3038 3739 223e 3c2f 613e me="l00879"> │ │ │ │ +00026290: 3c73 7061 6e20 636c 6173 733d 226c 696e 879< │ │ │ │ +000262f0: 2f61 3e3c 2f73 7061 6e3e 2020 3c73 7061 /a> bool DecisionTree& │ │ │ │ +00026380: 6c74 3b4c 2c20 5926 6774 3b3a 3a6f 7065 lt;L, Y>::ope │ │ │ │ +00026390: 7261 746f 723d 3d3c 2f61 3e28 3c73 7061 rator==(const < │ │ │ │ +000263c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +000263d0: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ +000263e0: 3237 3936 2e68 746d 6c22 3e44 6563 6973 2796.html">Decis │ │ │ │ +000263f0: 696f 6e54 7265 653c 2f61 3e26 616d 703b ionTree& │ │ │ │ +00026400: 206f 7468 6572 293c 7370 616e 2063 6c61 other) co │ │ │ │ +00026420: 6e73 7420 3c2f 7370 616e 3e7b 3c2f 6469 nst {.
    880 │ │ │ │ +00026480: 3c2f 7370 616e 3e20 2020 203c 7370 616e return root_->equ │ │ │ │ +000264c0: 616c 7328 2a6f 7468 6572 2e3c 6120 636c als(*other.root │ │ │ │ +00026520: 5f3c 2f61 3e29 3b3c 2f64 6976 3e0a 3c64 _);
    . │ │ │ │ +00026540: 3c61 2069 643d 226c 3030 3838 3122 206e 881 }
    ..
    882 │ │ │ │ +000265e0: 3c2f 7370 616e 3e20 3c2f 6469 763e 0a3c
    .< │ │ │ │ +000265f0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +00026600: 3e3c 6120 6964 3d22 6c30 3038 3833 2220 > 883 templ │ │ │ │ +00026660: 6174 653c 2f73 7061 6e3e 266c 743b 3c73 ate<typename L, typ │ │ │ │ +000266b0: 656e 616d 653c 2f73 7061 6e3e 2059 2667 ename Y&g │ │ │ │ +000266c0: 743b 3c2f 6469 763e 0a3c 6469 7620 636c t;
    ..
    < │ │ │ │ +00026920: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00026930: 6e6f 223e 2020 3838 353c 2f73 7061 6e3e no"> 885 │ │ │ │ +00026940: 2020 2020 3c73 7061 6e20 636c 6173 733d re │ │ │ │ +00026960: 7475 726e 3c2f 7370 616e 3e20 726f 6f74 turn root │ │ │ │ +00026970: 5f2d 2667 743b 6f70 6572 6174 6f72 2028 _->operator ( │ │ │ │ +00026980: 2928 7829 3b3c 2f64 6976 3e0a 3c64 6976 )(x);
    .
    < │ │ │ │ +000269c0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000269d0: 6e6f 223e 2020 3838 363c 2f73 7061 6e3e no"> 886 │ │ │ │ +000269e0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +000269f0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00026a60: 6120 6964 3d22 6c30 3038 3838 2220 6e61 a id="l00888" na │ │ │ │ +00026a70: 6d65 3d22 6c30 3038 3838 223e 3c2f 613e me="l00888"> │ │ │ │ +00026a80: 3c73 7061 6e20 636c 6173 733d 226c 696e 888 templat │ │ │ │ +00026ac0: 653c 2f73 7061 6e3e 266c 743b 3c73 7061 e<typename L, typen │ │ │ │ +00026b10: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ +00026b20: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    .< │ │ │ │ +00026b80: 6120 6964 3d22 6c30 3038 3839 2220 6e61 a id="l00889" na │ │ │ │ +00026b90: 6d65 3d22 6c30 3038 3839 223e 3c2f 613e me="l00889"> │ │ │ │ +00026ba0: 3c73 7061 6e20 636c 6173 733d 226c 696e 889< │ │ │ │ +00026c00: 2f61 3e3c 2f73 7061 6e3e 2020 3c61 2063 /a> Decision │ │ │ │ +00026c40: 5472 6565 266c 743b 4c2c 2059 2667 743b Tree<L, Y> │ │ │ │ +00026c50: 3c2f 613e 203c 6120 636c 6173 733d 2263 DecisionTr │ │ │ │ +00026cb0: 6565 266c 743b 4c2c 2059 2667 743b 3a3a ee<L, Y>:: │ │ │ │ +00026cc0: 6170 706c 793c 2f61 3e28 3c73 7061 6e20 apply( │ │ │ │ +00026ce0: 636f 6e73 743c 2f73 7061 6e3e 203c 6120 const Una │ │ │ │ +00026d40: 7279 3c2f 613e 2661 6d70 3b20 6f70 293c ry& op)< │ │ │ │ +00026d50: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00026d60: 6f72 6422 3e20 636f 6e73 7420 3c2f 7370 ord"> const {
    .
    890 │ │ │ │ +00026dd0: 2020 203c 7370 616e 2063 6c61 7373 3d22 // It i │ │ │ │ +00026df0: 7320 756e 636c 6561 7220 7768 6174 2073 s unclear what s │ │ │ │ +00026e00: 686f 756c 6420 6861 7070 656e 2069 6620 hould happen if │ │ │ │ +00026e10: 7472 6565 2069 7320 656d 7074 793a 3c2f tree is empty:
    .
    < │ │ │ │ +00026e60: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00026e70: 6e6f 223e 2020 3839 313c 2f73 7061 6e3e no"> 891 │ │ │ │ +00026e80: 2020 2020 3c73 7061 6e20 636c 6173 733d if │ │ │ │ +00026ea0: 3c2f 7370 616e 3e20 2865 6d70 7479 2829 (empty() │ │ │ │ +00026eb0: 2920 7b3c 2f64 6976 3e0a 3c64 6976 2063 ) {
    .
    892 │ │ │ │ +00026f10: 2020 2020 3c73 7061 6e20 636c 6173 733d th │ │ │ │ +00026f30: 726f 773c 2f73 7061 6e3e 2073 7464 3a3a row std:: │ │ │ │ +00026f40: 7275 6e74 696d 655f 6572 726f 7228 3c2f runtime_error(.
    8 │ │ │ │ +00026fa0: 3933 3c2f 7370 616e 3e20 2020 2020 2020 93 │ │ │ │ +00026fb0: 2020 203c 7370 616e 2063 6c61 7373 3d22 & │ │ │ │ +00026fd0: 7175 6f74 3b44 6563 6973 696f 6e54 7265 quot;DecisionTre │ │ │ │ +00026fe0: 653a 3a61 7070 6c79 2875 6e61 7279 206f e::apply(unary o │ │ │ │ +00026ff0: 7029 2075 6e64 6566 696e 6564 2066 6f72 p) undefined for │ │ │ │ +00027000: 2065 6d70 7479 2074 7265 652e 2671 756f empty tree.&quo │ │ │ │ +00027010: 743b 3c2f 7370 616e 3e29 3b3c 2f64 6976 t;);
    .
    894< │ │ │ │ +00027070: 2f73 7061 6e3e 2020 2020 7d3c 2f64 6976 /span> }
    .
    895< │ │ │ │ +000270d0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> return De │ │ │ │ +00027130: 6369 7369 6f6e 5472 6565 3c2f 613e 2872 cisionTree(r │ │ │ │ +00027140: 6f6f 745f 2d26 6774 3b61 7070 6c79 286f oot_->apply(o │ │ │ │ +00027150: 7029 293b 3c2f 6469 763e 0a3c 6469 7620 p));
    .
    896 │ │ │ │ +000271b0: 207d 3c2f 6469 763e 0a3c 2f64 6976 3e0a }
    .
    . │ │ │ │ +000271c0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65 .
    < │ │ │ │ +00027250: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00027260: 6e6f 223e 2020 3839 393c 2f73 7061 6e3e no"> 899 │ │ │ │ +00027270: 2020 3c73 7061 6e20 636c 6173 733d 226b template │ │ │ │ +00027290: 3c2f 7370 616e 3e20 266c 743b 3c73 7061 <typename L, typen │ │ │ │ +000272e0: 616d 653c 2f73 7061 6e3e 2059 2667 743b ame Y> │ │ │ │ +000272f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    . │ │ │ │ +000274a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000274d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 901 const Un │ │ │ │ +00027520: 6172 7941 7373 6967 6e6d 656e 7426 616d aryAssignment&am │ │ │ │ +00027530: 703b 206f 7029 3c73 7061 6e20 636c 6173 p; op) con │ │ │ │ +00027550: 7374 203c 2f73 7061 6e3e 7b3c 2f64 6976 st {
    .
    902< │ │ │ │ +000275b0: 2f73 7061 6e3e 2020 2020 3c73 7061 6e20 /span> │ │ │ │ +000275d0: 2f2f 2049 7420 6973 2075 6e63 6c65 6172 // It is unclear │ │ │ │ +000275e0: 2077 6861 7420 7368 6f75 6c64 2068 6170 what should hap │ │ │ │ +000275f0: 7065 6e20 6966 2074 7265 6520 6973 2065 pen if tree is e │ │ │ │ +00027600: 6d70 7479 3a3c 2f73 7061 6e3e 3c2f 6469 mpty:.
    903 │ │ │ │ +00027660: 3c2f 7370 616e 3e20 2020 203c 7370 616e if ( │ │ │ │ +00027690: 656d 7074 7928 2929 207b 3c2f 6469 763e empty()) {
    │ │ │ │ +000276a0: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000276d0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 904 throw std::runtime_e │ │ │ │ +00027730: 7272 6f72 283c 2f64 6976 3e0a 3c64 6976 rror(
    .
    < │ │ │ │ +00027770: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00027780: 6e6f 223e 2020 3930 353c 2f73 7061 6e3e no"> 905 │ │ │ │ +00027790: 2020 2020 2020 2020 2020 3c73 7061 6e20 "Deci │ │ │ │ +000277c0: 7369 6f6e 5472 6565 3a3a 6170 706c 7928 sionTree::apply( │ │ │ │ +000277d0: 756e 6172 7920 6f70 2920 756e 6465 6669 unary op) undefi │ │ │ │ +000277e0: 6e65 6420 666f 7220 656d 7074 7920 7472 ned for empty tr │ │ │ │ +000277f0: 6565 2e26 7175 6f74 3b3c 2f73 7061 6e3e ee." │ │ │ │ +00027800: 293b 3c2f 6469 763e 0a3c 6469 7620 636c );
    .
    906 │ │ │ │ +00027860: 207d 3c2f 6469 763e 0a3c 6469 7620 636c }
    .
    907 │ │ │ │ +000278c0: 203c 6120 636c 6173 733d 2263 6f64 6520 Ass │ │ │ │ +000278f0: 6967 6e6d 656e 7426 6c74 3b4c 2667 743b ignment<L> │ │ │ │ +00027900: 3c2f 613e 2061 7373 6967 6e6d 656e 743b assignment; │ │ │ │ +00027910: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00027960: 2039 3038 3c2f 7370 616e 3e20 2020 203c 908 < │ │ │ │ +00027970: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +00027980: 6f72 6466 6c6f 7722 3e72 6574 7572 6e3c ordflow">return< │ │ │ │ +00027990: 2f73 7061 6e3e 203c 6120 636c 6173 733d /span> DecisionTree< │ │ │ │ +000279d0: 2f61 3e28 726f 6f74 5f2d 2667 743b 6170 /a>(root_->ap │ │ │ │ +000279e0: 706c 7928 6f70 2c20 6173 7369 676e 6d65 ply(op, assignme │ │ │ │ +000279f0: 6e74 2929 3b3c 2f64 6976 3e0a 3c64 6976 nt));
    .
    < │ │ │ │ +00027a30: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +00027a40: 6e6f 223e 2020 3930 393c 2f73 7061 6e3e no"> 909 │ │ │ │ +00027a50: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +00027a60: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00027ad0: 6120 6964 3d22 6c30 3039 3131 2220 6e61 a id="l00911" na │ │ │ │ +00027ae0: 6d65 3d22 6c30 3039 3131 223e 3c2f 613e me="l00911"> │ │ │ │ +00027af0: 3c73 7061 6e20 636c 6173 733d 226c 696e 911 /****** │ │ │ │ +00027b30: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00027b40: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00027b50: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00027b60: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00027b70: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    9 │ │ │ │ +00027bd0: 3132 3c2f 7370 616e 3e20 203c 7370 616e 12 template │ │ │ │ +00027c00: 266c 743b 3c73 7061 6e20 636c 6173 733d <typena │ │ │ │ +00027c20: 6d65 3c2f 7370 616e 3e20 4c2c 203c 7370 me L, typename Y>
    .< │ │ │ │ +00027c60: 6469 7620 636c 6173 733d 2266 6f6c 646f div class="foldo │ │ │ │ +00027c70: 7065 6e22 2069 643d 2266 6f6c 646f 7065 pen" id="foldope │ │ │ │ +00027c80: 6e30 3039 3133 2220 6461 7461 2d73 7461 n00913" data-sta │ │ │ │ +00027c90: 7274 3d22 7b22 2064 6174 612d 656e 643d rt="{" data-end= │ │ │ │ +00027ca0: 227d 223e 0a3c 6469 7620 636c 6173 733d "}">.
    . │ │ │ │ +00027e70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +00027f70: 2020 3931 353c 2f73 7061 6e3e 2020 2020 915 │ │ │ │ +00027f80: 3c73 7061 6e20 636c 6173 733d 2263 6f6d // It is u │ │ │ │ +00027fa0: 6e63 6c65 6172 2077 6861 7420 7368 6f75 nclear what shou │ │ │ │ +00027fb0: 6c64 2068 6170 7065 6e20 6966 2065 6974 ld happen if eit │ │ │ │ +00027fc0: 6865 7220 7472 6565 2069 7320 656d 7074 her tree is empt │ │ │ │ +00027fd0: 793a 3c2f 7370 616e 3e3c 2f64 6976 3e0a y:
    . │ │ │ │ +00027fe0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ +000280d0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    918 │ │ │ │ +000281c0: 2020 2020 2020 2020 203c 7370 616e 2063 "Decis │ │ │ │ +000281f0: 696f 6e54 7265 653a 3a61 7070 6c79 2862 ionTree::apply(b │ │ │ │ +00028200: 696e 6172 7920 6f70 2920 756e 6465 6669 inary op) undefi │ │ │ │ +00028210: 6e65 6420 666f 7220 656d 7074 7920 7472 ned for empty tr │ │ │ │ +00028220: 6565 732e 2671 756f 743b 3c2f 7370 616e ees.");
    .
    919 │ │ │ │ +00028290: 2020 7d3c 2f64 6976 3e0a 3c64 6976 2063 }
    .
    920 │ │ │ │ +000282f0: 2020 3c73 7061 6e20 636c 6173 733d 2263 // apply │ │ │ │ +00028310: 2074 6865 206f 7065 7261 746f 6e20 6f6e the operaton on │ │ │ │ +00028320: 2074 6865 2072 6f6f 7420 6f66 2062 6f74 the root of bot │ │ │ │ +00028330: 6820 6469 6167 7261 6d73 3c2f 7370 616e h diagrams
    .
    │ │ │ │ +00028390: 2020 3932 313c 2f73 7061 6e3e 2020 2020 921 │ │ │ │ +000283a0: 3c61 2063 6c61 7373 3d22 636f 6465 2068 │ │ │ │ +000283f0: 4e6f 6465 5074 723c 2f61 3e20 6820 3d20 NodePtr h = │ │ │ │ +00028400: 726f 6f74 5f2d 2667 743b 6170 706c 795f root_->apply_ │ │ │ │ +00028410: 665f 6f70 5f67 282a 672e 3c61 2063 6c61 f_op_g(*g.root_ │ │ │ │ +00028470: 3c2f 613e 2c20 6f70 293b 3c2f 6469 763e , op);
    │ │ │ │ +00028480: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +000284b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 922 / │ │ │ │ +000284f0: 2f20 6372 6561 7465 2061 206e 6577 2063 / create a new c │ │ │ │ +00028500: 6c61 7373 2077 6974 6820 7468 6520 7265 lass with the re │ │ │ │ +00028510: 7375 6c74 696e 6720 726f 6f74 2026 7175 sulting root &qu │ │ │ │ +00028520: 6f74 3b68 2671 756f 743b 3c2f 7370 616e ot;h"
    .
    │ │ │ │ +00028580: 2020 3932 333c 2f73 7061 6e3e 2020 2020 923 │ │ │ │ +00028590: 3c61 2063 6c61 7373 3d22 636f 6465 2068 Deci │ │ │ │ +000285c0: 7369 6f6e 5472 6565 3c2f 613e 2072 6573 sionTree res │ │ │ │ +000285d0: 756c 7428 6829 3b3c 2f64 6976 3e0a 3c64 ult(h);
    . │ │ │ │ +000285f0: 3c61 2069 643d 226c 3030 3932 3422 206e 924 │ │ │ │ +00028650: 7265 7475 726e 3c2f 7370 616e 3e20 7265 return re │ │ │ │ +00028660: 7375 6c74 3b3c 2f64 6976 3e0a 3c64 6976 sult;
    .
    < │ │ │ │ +000286a0: 7370 616e 2063 6c61 7373 3d22 6c69 6e65 span class="line │ │ │ │ +000286b0: 6e6f 223e 2020 3932 353c 2f73 7061 6e3e no"> 925 │ │ │ │ +000286c0: 2020 7d3c 2f64 6976 3e0a 3c2f 6469 763e }
    .
    │ │ │ │ +000286d0: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00028740: 6120 6964 3d22 6c30 3039 3237 2220 6e61 a id="l00927" na │ │ │ │ +00028750: 6d65 3d22 6c30 3039 3237 223e 3c2f 613e me="l00927"> │ │ │ │ +00028760: 3c73 7061 6e20 636c 6173 733d 226c 696e 927 /****** │ │ │ │ +000287a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000287b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000287c0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000287d0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +000287e0: 2a2a 2a2a 2a2a 2f3c 2f73 7061 6e3e 3c2f ******/.
    9 │ │ │ │ +00028840: 3238 3c2f 7370 616e 3e20 203c 7370 616e 28 // The way this │ │ │ │ +00028870: 2077 6f72 6b73 3a3c 2f73 7061 6e3e 3c2f works:.
    9 │ │ │ │ +000288d0: 3239 3c2f 7370 616e 3e20 203c 7370 616e 29 // We have an A │ │ │ │ +00028900: 4454 2c20 7069 6374 7572 6520 6974 2061 DT, picture it a │ │ │ │ +00028910: 7320 6120 7472 6565 2e3c 2f73 7061 6e3e s a tree. │ │ │ │ +00028920: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00028970: 2039 3330 3c2f 7370 616e 3e20 203c 7370 930 // At a certa │ │ │ │ +000289a0: 696e 2064 6570 7468 2c20 7765 2068 6176 in depth, we hav │ │ │ │ +000289b0: 6520 6120 6272 616e 6368 206f 6e20 2671 e a branch on &q │ │ │ │ +000289c0: 756f 743b 6c61 6265 6c26 7175 6f74 3b2e uot;label". │ │ │ │ +000289d0: 3c2f 7370 616e 3e3c 2f64 6976 3e0a 3c64
    . │ │ │ │ +000289f0: 3c61 2069 643d 226c 3030 3933 3122 206e 931 // The │ │ │ │ +00028a50: 2066 756e 6374 696f 6e20 2671 756f 743b function " │ │ │ │ +00028a60: 6368 6f6f 7365 286c 6162 656c 2c69 6e64 choose(label,ind │ │ │ │ +00028a70: 6578 2926 7175 6f74 3b20 7769 6c6c 2072 ex)" will r │ │ │ │ +00028a80: 6574 7572 6e20 6120 7472 6565 206f 6620 eturn a tree of │ │ │ │ +00028a90: 6f6e 6520 6c65 7373 2064 6570 7468 2c3c one less depth,< │ │ │ │ +00028aa0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +00028ac0: 6120 6964 3d22 6c30 3039 3332 2220 6e61 a id="l00932" na │ │ │ │ +00028ad0: 6d65 3d22 6c30 3039 3332 223e 3c2f 613e me="l00932"> │ │ │ │ +00028ae0: 3c73 7061 6e20 636c 6173 733d 226c 696e 932 // wher │ │ │ │ +00028b20: 6520 7468 6572 6520 6973 206e 6f20 6d6f e there is no mo │ │ │ │ +00028b30: 7265 2062 7261 6e63 6820 6f6e 2026 7175 re branch on &qu │ │ │ │ +00028b40: 6f74 3b6c 6162 656c 2671 756f 743b 3a20 ot;label": │ │ │ │ +00028b50: 6f6e 6c79 2074 6865 2073 7562 7472 6565 only the subtree │ │ │ │ +00028b60: 2075 6e64 6572 2074 6861 743c 2f73 7061 under that
    .
    933 < │ │ │ │ +00028bd0: 7370 616e 2063 6c61 7373 3d22 636f 6d6d span class="comm │ │ │ │ +00028be0: 656e 7422 3e2f 2f20 6272 616e 6368 2070 ent">// branch p │ │ │ │ +00028bf0: 6f69 6e74 2063 6f72 7265 7370 6f6e 6469 oint correspondi │ │ │ │ +00028c00: 6e67 2074 6f20 7468 6520 7661 6c75 6520 ng to the value │ │ │ │ +00028c10: 2671 756f 743b 696e 6465 7826 7175 6f74 "index" │ │ │ │ +00028c20: 3b20 6973 206c 6566 7420 696e 7374 6561 ; is left instea │ │ │ │ +00028c30: 642e 3c2f 7370 616e 3e3c 2f64 6976 3e0a d.
    . │ │ │ │ +00028c40: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    . │ │ │ │ +00028d20: 3c61 2069 643d 226c 3030 3933 3522 206e 935 // Thi │ │ │ │ +00028d80: 7320 696d 706c 656d 656e 7473 206d 6172 s implements mar │ │ │ │ +00028d90: 6769 6e61 6c69 7a61 7469 6f6e 2069 6e20 ginalization in │ │ │ │ +00028da0: 4461 7277 6963 6865 3039 626f 6f6b 2c20 Darwiche09book, │ │ │ │ +00028db0: 7067 2033 3330 3c2f 7370 616e 3e3c 2f64 pg 330.
    93 │ │ │ │ +00028e10: 363c 2f73 7061 6e3e 2020 3c73 7061 6e20 6 │ │ │ │ +00028e30: 7465 6d70 6c61 7465 3c2f 7370 616e 3e26 template& │ │ │ │ +00028e40: 6c74 3b3c 7370 616e 2063 6c61 7373 3d22 lt;typenam │ │ │ │ +00028e60: 653c 2f73 7061 6e3e 204c 2c20 3c73 7061 e L, typename Y>
    ...
    938 │ │ │ │ +000290d0: 2020 2020 3c73 7061 6e20 636c 6173 733d si │ │ │ │ +000290f0: 7a65 5f74 3c2f 7370 616e 3e20 6361 7264 ze_t card │ │ │ │ +00029100: 696e 616c 6974 792c 203c 7370 616e 2063 inality, c │ │ │ │ +00029120: 6f6e 7374 3c2f 7370 616e 3e20 4269 6e61 onst Bina │ │ │ │ +00029130: 7279 2661 6d70 3b20 6f70 293c 7370 616e ry& op) const { │ │ │ │ +00029160: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +000291b0: 2039 3339 3c2f 7370 616e 3e20 2020 203c 939 < │ │ │ │ +000291c0: 6120 636c 6173 733d 2263 6f64 6520 686c a class="code hl │ │ │ │ +000291d0: 5f63 6c61 7373 2220 6872 6566 3d22 6130 _class" href="a0 │ │ │ │ +000291e0: 3237 3936 2e68 746d 6c22 3e44 6563 6973 2796.html">Decis │ │ │ │ +000291f0: 696f 6e54 7265 653c 2f61 3e20 7265 7375 ionTree resu │ │ │ │ +00029200: 6c74 203d 2063 686f 6f73 6528 6c61 6265 lt = choose(labe │ │ │ │ +00029210: 6c2c 2030 293b 3c2f 6469 763e 0a3c 6469 l, 0);
    .< │ │ │ │ +00029230: 6120 6964 3d22 6c30 3039 3430 2220 6e61 a id="l00940" na │ │ │ │ +00029240: 6d65 3d22 6c30 3039 3430 223e 3c2f 613e me="l00940"> │ │ │ │ +00029250: 3c73 7061 6e20 636c 6173 733d 226c 696e 940 f │ │ │ │ +00029290: 6f72 3c2f 7370 616e 3e20 283c 7370 616e or (size_t index = 1; in │ │ │ │ +000292d0: 6465 7820 266c 743b 2063 6172 6469 6e61 dex < cardina │ │ │ │ +000292e0: 6c69 7479 3b20 696e 6465 782b 2b29 207b lity; index++) { │ │ │ │ +000292f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00029340: 2039 3431 3c2f 7370 616e 3e20 2020 2020 941 │ │ │ │ +00029350: 203c 6120 636c 6173 733d 2263 6f64 6520 Dec │ │ │ │ +00029380: 6973 696f 6e54 7265 653c 2f61 3e20 6368 isionTree ch │ │ │ │ +00029390: 6f73 656e 203d 2063 686f 6f73 6528 6c61 osen = choose(la │ │ │ │ +000293a0: 6265 6c2c 2069 6e64 6578 293b 3c2f 6469 bel, index);.
    942 │ │ │ │ +00029400: 3c2f 7370 616e 3e20 2020 2020 2072 6573 res │ │ │ │ +00029410: 756c 7420 3d20 7265 7375 6c74 2e61 7070 ult = result.app │ │ │ │ +00029420: 6c79 2863 686f 7365 6e2c 206f 7029 3b3c ly(chosen, op);< │ │ │ │ +00029430: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00029480: 3934 333c 2f73 7061 6e3e 2020 2020 7d3c 943 }< │ │ │ │ +00029490: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +000294e0: 3934 343c 2f73 7061 6e3e 2020 2020 3c73 944 return result;.
    94 │ │ │ │ +00029570: 353c 2f73 7061 6e3e 2020 7d3c 2f64 6976 5 }
    .
    .
    946 .
    9 │ │ │ │ +00029630: 3437 3c2f 7370 616e 3e3c 7370 616e 2063 47 │ │ │ │ +00029650: 202f 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a /************** │ │ │ │ +00029660: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00029670: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00029680: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +00029690: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ +000296a0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +000296c0: 6120 6964 3d22 6c30 3039 3438 2220 6e61 a id="l00948" na │ │ │ │ +000296d0: 6d65 3d22 6c30 3039 3438 223e 3c2f 613e me="l00948"> │ │ │ │ +000296e0: 3c73 7061 6e20 636c 6173 733d 226c 696e 948 templat │ │ │ │ +00029720: 653c 2f73 7061 6e3e 2026 6c74 3b3c 7370 e <typename
    L, type │ │ │ │ +00029770: 6e61 6d65 3c2f 7370 616e 3e20 5926 6774 name Y> │ │ │ │ +00029780: 3b3c 2f64 6976 3e0a 3c64 6976 2063 6c61 ;
    .
    . │ │ │ │ +000297e0: 3c61 2069 643d 226c 3030 3934 3922 206e 949 │ │ │ │ +00029860: 3c2f 613e 3c2f 7370 616e 3e20 203c 7370 void DecisionTree │ │ │ │ +000298f0: 266c 743b 4c2c 2059 2667 743b 3a3a 646f <L, Y>::do │ │ │ │ +00029900: 743c 2f61 3e28 7374 643a 3a6f 7374 7265 t(std::ostre │ │ │ │ +00029910: 616d 2661 6d70 3b20 6f73 2c3c 2f64 6976 am& os,
    .
    950< │ │ │ │ +00029970: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +00029980: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00029990: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ +000299b0: 3c2f 7370 616e 3e20 4c61 6265 6c46 6f72 LabelFor │ │ │ │ +000299c0: 6d61 7474 6572 2661 6d70 3b20 6c61 6265 matter& labe │ │ │ │ +000299d0: 6c46 6f72 6d61 7474 6572 2c3c 2f64 6976 lFormatter,
    .
    951< │ │ │ │ +00029a30: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +00029a40: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00029a50: 2020 2020 203c 7370 616e 2063 6c61 7373 const │ │ │ │ +00029a70: 3c2f 7370 616e 3e20 5661 6c75 6546 6f72 ValueFor │ │ │ │ +00029a80: 6d61 7474 6572 2661 6d70 3b20 7661 6c75 matter& valu │ │ │ │ +00029a90: 6546 6f72 6d61 7474 6572 2c3c 2f64 6976 eFormatter,
    .
    952< │ │ │ │ +00029af0: 2f73 7061 6e3e 2020 2020 2020 2020 2020 /span> │ │ │ │ +00029b00: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +00029b10: 2020 2020 203c 7370 616e 2063 6c61 7373 b │ │ │ │ +00029b30: 6f6f 6c3c 2f73 7061 6e3e 2073 686f 775a ool showZ │ │ │ │ +00029b40: 6572 6f29 3c73 7061 6e20 636c 6173 733d ero) const │ │ │ │ +00029b60: 203c 2f73 7061 6e3e 7b3c 2f64 6976 3e0a {
    . │ │ │ │ +00029b70: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    .
    │ │ │ │ +00029c60: 2020 3935 343c 2f73 7061 6e3e 2020 2020 954 │ │ │ │ +00029c70: 726f 6f74 5f2d 2667 743b 646f 7428 6f73 root_->dot(os │ │ │ │ +00029c80: 2c20 6c61 6265 6c46 6f72 6d61 7474 6572 , labelFormatter │ │ │ │ +00029c90: 2c20 7661 6c75 6546 6f72 6d61 7474 6572 , valueFormatter │ │ │ │ +00029ca0: 2c20 7368 6f77 5a65 726f 293b 3c2f 6469 , showZero);.
    955 │ │ │ │ +00029d00: 3c2f 7370 616e 3e20 2020 206f 7320 266c os &l │ │ │ │ +00029d10: 743b 266c 743b 203c 7370 616e 2063 6c61 t;< " [order │ │ │ │ +00029d40: 696e 673d 6f75 745d 7d26 7175 6f74 3b3c ing=out]}"< │ │ │ │ +00029d50: 2f73 7061 6e3e 2026 6c74 3b26 6c74 3b20 /span> << │ │ │ │ +00029d60: 7374 643a 3a65 6e64 6c3b 3c2f 6469 763e std::endl;
    │ │ │ │ +00029d70: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +00029dd0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +00029e20: 3935 373c 2f73 7061 6e3e 203c 2f64 6976 957
    .
    958< │ │ │ │ +00029e80: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ +00029ea0: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ +00029eb0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ +00029ed0: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ +00029f00: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ +00029fe0: 2020 3935 393c 2f61 3e3c 2f73 7061 6e3e 959 │ │ │ │ +00029ff0: 2020 3c73 7061 6e20 636c 6173 733d 226b void │ │ │ │ +0002a010: 3c2f 7370 616e 3e20 3c61 2063 6c61 7373 Decisio │ │ │ │ +0002a070: 6e54 7265 6526 6c74 3b4c 2c20 5926 6774 nTree<L, Y> │ │ │ │ +0002a080: 3b3a 3a64 6f74 3c2f 613e 283c 7370 616e ;::dot(const st │ │ │ │ +0002a0b0: 643a 3a73 7472 696e 6726 616d 703b 206e d::string& n │ │ │ │ +0002a0c0: 616d 652c 3c2f 6469 763e 0a3c 6469 7620 ame,
    .
    960 │ │ │ │ +0002a120: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0002a130: 2020 2020 2020 2020 2020 2020 2020 3c73 const │ │ │ │ +0002a160: 204c 6162 656c 466f 726d 6174 7465 7226 LabelFormatter& │ │ │ │ +0002a170: 616d 703b 206c 6162 656c 466f 726d 6174 amp; labelFormat │ │ │ │ +0002a180: 7465 722c 3c2f 6469 763e 0a3c 6469 7620 ter,
    .
    961 │ │ │ │ +0002a1e0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0002a1f0: 2020 2020 2020 2020 2020 2020 2020 3c73 const │ │ │ │ +0002a220: 2056 616c 7565 466f 726d 6174 7465 7226 ValueFormatter& │ │ │ │ +0002a230: 616d 703b 2076 616c 7565 466f 726d 6174 amp; valueFormat │ │ │ │ +0002a240: 7465 722c 3c2f 6469 763e 0a3c 6469 7620 ter,
    .
    962 │ │ │ │ +0002a2a0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0002a2b0: 2020 2020 2020 2020 2020 2020 2020 3c73 bool showZero) const {
    .
    963 │ │ │ │ +0002a370: 2073 7464 3a3a 6f66 7374 7265 616d 206f std::ofstream o │ │ │ │ +0002a380: 7328 286e 616d 6520 2b20 3c73 7061 6e20 s((name + ".dot │ │ │ │ +0002a3b0: 2671 756f 743b 3c2f 7370 616e 3e29 2e63 ").c │ │ │ │ +0002a3c0: 5f73 7472 2829 293b 3c2f 6469 763e 0a3c _str());
    .< │ │ │ │ +0002a3d0: 6469 7620 636c 6173 733d 226c 696e 6522 div class="line" │ │ │ │ +0002a3e0: 3e3c 6120 6964 3d22 6c30 3039 3634 2220 > 964 dot( │ │ │ │ +0002a480: 6f73 2c20 6c61 6265 6c46 6f72 6d61 7474 os, labelFormatt │ │ │ │ +0002a490: 6572 2c20 7661 6c75 6546 6f72 6d61 7474 er, valueFormatt │ │ │ │ +0002a4a0: 6572 2c20 7368 6f77 5a65 726f 293b 3c2f er, showZero);.
    9 │ │ │ │ +0002a500: 3635 3c2f 7370 616e 3e20 2020 203c 7370 65 int result =
    │ │ │ │ +0002a540: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0002a570: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 966 sys │ │ │ │ +0002a5a0: 7465 6d28 283c 7370 616e 2063 6c61 7373 tem(("dot -Tpdf │ │ │ │ +0002a5d0: 2026 7175 6f74 3b3c 2f73 7061 6e3e 202b " + │ │ │ │ +0002a5e0: 206e 616d 6520 2b20 3c73 7061 6e20 636c name + ".dot - │ │ │ │ +0002a610: 6f20 2671 756f 743b 3c2f 7370 616e 3e20 o " │ │ │ │ +0002a620: 2b20 6e61 6d65 202b 203c 7370 616e 2063 + name + ".pdf │ │ │ │ +0002a650: 2667 743b 2661 6d70 3b20 2f64 6576 2f6e >& /dev/n │ │ │ │ +0002a660: 756c 6c26 7175 6f74 3b3c 2f73 7061 6e3e ull" │ │ │ │ +0002a670: 293c 2f64 6976 3e0a 3c64 6976 2063 6c61 )
    .
    │ │ │ │ +0002a6c0: 2020 3936 373c 2f73 7061 6e3e 2020 2020 967 │ │ │ │ +0002a6d0: 2020 2020 2020 2020 2020 2020 2020 202e . │ │ │ │ +0002a6e0: 635f 7374 7228 2929 3b3c 2f64 6976 3e0a c_str());
    . │ │ │ │ +0002a6f0: 3c64 6976 2063 6c61 7373 3d22 6c69 6e65
    │ │ │ │ +0002a780: 0a3c 6469 7620 636c 6173 733d 226c 696e .
    │ │ │ │ +0002a7b0: 3c2f 613e 3c73 7061 6e20 636c 6173 733d 969 throw std::runtime_e │ │ │ │ +0002a810: 7272 6f72 283c 7370 616e 2063 6c61 7373 rror("DecisionT │ │ │ │ +0002a840: 7265 653a 3a64 6f74 2073 7973 7465 6d20 ree::dot system │ │ │ │ +0002a850: 6361 6c6c 2066 6169 6c65 6426 7175 6f74 call failed" │ │ │ │ +0002a860: 3b3c 2f73 7061 6e3e 293b 3c2f 6469 763e ;);
    │ │ │ │ +0002a870: 0a3c 6469 7620 636c 6173 733d 226c 696e ..< │ │ │ │ +0002a8d0: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    │ │ │ │ +0002a920: 3937 313c 2f73 7061 6e3e 203c 2f64 6976 971
    .
    972< │ │ │ │ +0002a980: 2f73 7061 6e3e 2020 3c73 7061 6e20 636c /span> te │ │ │ │ +0002a9a0: 6d70 6c61 7465 3c2f 7370 616e 3e20 266c mplate &l │ │ │ │ +0002a9b0: 743b 3c73 7061 6e20 636c 6173 733d 226b t;typename │ │ │ │ +0002a9d0: 3c2f 7370 616e 3e20 4c2c 203c 7370 616e L, typename │ │ │ │ +0002aa00: 2059 2667 743b 3c2f 6469 763e 0a3c 6469 Y>
    ..
    │ │ │ │ +0002aae0: 2020 3937 333c 2f61 3e3c 2f73 7061 6e3e 973 │ │ │ │ +0002aaf0: 2020 7374 643a 3a73 7472 696e 6720 3c61 std::string D │ │ │ │ +0002ab50: 6563 6973 696f 6e54 7265 6526 6c74 3b4c ecisionTree<L │ │ │ │ +0002ab60: 2c20 5926 6774 3b3a 3a64 6f74 3c2f 613e , Y>::dot │ │ │ │ +0002ab70: 283c 7370 616e 2063 6c61 7373 3d22 6b65 (const LabelFormatt │ │ │ │ +0002aba0: 6572 2661 6d70 3b20 6c61 6265 6c46 6f72 er& labelFor │ │ │ │ +0002abb0: 6d61 7474 6572 2c3c 2f64 6976 3e0a 3c64 matter,
    . │ │ │ │ +0002abd0: 3c61 2069 643d 226c 3030 3937 3422 206e 974 │ │ │ │ +0002ac20: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0002ac30: 2020 2020 2020 2020 3c73 7061 6e20 636c co │ │ │ │ +0002ac50: 6e73 743c 2f73 7061 6e3e 2056 616c 7565 nst Value │ │ │ │ +0002ac60: 466f 726d 6174 7465 7226 616d 703b 2076 Formatter& v │ │ │ │ +0002ac70: 616c 7565 466f 726d 6174 7465 722c 3c2f alueFormatter,.
    9 │ │ │ │ +0002acd0: 3735 3c2f 7370 616e 3e20 2020 2020 2020 75 │ │ │ │ +0002ace0: 2020 2020 2020 2020 2020 2020 2020 2020 │ │ │ │ +0002acf0: 2020 2020 2020 2020 2020 2020 2020 203c < │ │ │ │ +0002ad00: 7370 616e 2063 6c61 7373 3d22 6b65 7977 span class="keyw │ │ │ │ +0002ad10: 6f72 6474 7970 6522 3e62 6f6f 6c3c 2f73 ordtype">bool showZero) const {
    .
    976 │ │ │ │ +0002adb0: 2020 7374 643a 3a73 7472 696e 6773 7472 std::stringstr │ │ │ │ +0002adc0: 6561 6d20 7373 3b3c 2f64 6976 3e0a 3c64 eam ss;
    . │ │ │ │ +0002ade0: 3c61 2069 643d 226c 3030 3937 3722 206e 977 dot(s │ │ │ │ +0002ae80: 732c 206c 6162 656c 466f 726d 6174 7465 s, labelFormatte │ │ │ │ +0002ae90: 722c 2076 616c 7565 466f 726d 6174 7465 r, valueFormatte │ │ │ │ +0002aea0: 722c 2073 686f 775a 6572 6f29 3b3c 2f64 r, showZero);.
    97 │ │ │ │ +0002af00: 383c 2f73 7061 6e3e 2020 2020 3c73 7061 8 return ss.str();.
    97 │ │ │ │ +0002af90: 393c 2f73 7061 6e3e 2020 7d3c 2f64 6976 9 }
    .
    .
    980 .
    9 │ │ │ │ +0002b050: 3831 3c2f 7370 616e 3e3c 7370 616e 2063 81/ │ │ │ │ +0002b070: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0002b080: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0002b090: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0002b0a0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a **************** │ │ │ │ +0002b0b0: 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2a2a 2f3c **************/< │ │ │ │ +0002b0c0: 2f73 7061 6e3e 3c2f 6469 763e 0a3c 6469 /span>
    .< │ │ │ │ +0002b0e0: 6120 6964 3d22 6c30 3039 3832 2220 6e61 a id="l00982" na │ │ │ │ +0002b0f0: 6d65 3d22 6c30 3039 3832 223e 3c2f 613e me="l00982"> │ │ │ │ +0002b100: 3c73 7061 6e20 636c 6173 733d 226c 696e 982
    .
    983 } │ │ │ │ +0002b180: 2020 3c73 7061 6e20 636c 6173 733d 2263 // names │ │ │ │ +0002b1a0: 7061 6365 2067 7473 616d 3c2f 7370 616e pace gtsam
    .
    .< │ │ │ │ +0002b1c0: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ..
    .
    .
    .
    . │ │ │ │ +0002b1f0: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c2f
    .
    ..
    ..
    .
    │ │ │ │ +0002b220: 0a3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c .
    .
    .< │ │ │ │ +0002b230: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ..
    .
    .
    .
    . │ │ │ │ +0002b260: 3c2f 6469 763e 0a3c 2f64 6976 3e0a 3c2f
    .
    ..
    ..
    .
    │ │ │ │ +0002b290: 0a3c 2f64 6976 3e0a 3c2f 6469 763e 0a3c .
    .
    .< │ │ │ │ +0002b2a0: 2f64 6976 3e0a 3c2f 6469 763e 0a3c 2f64 /div>.
    ...
    Global function │ │ │ │ +0002b3e0: 7320 696e 2061 2073 6570 6172 6174 6520 s in a separate │ │ │ │ +0002b3f0: 7465 7374 696e 6720 6e61 6d65 7370 6163 testing namespac │ │ │ │ +0002b400: 652e 3c2f 6469 763e 3c64 6976 2063 6c61 e.
    De │ │ │ │ +0002b420: 6669 6e69 7469 6f6e 3c2f 623e 2063 6861 finition cha │ │ │ │ +0002b430: 7274 5465 7374 696e 672e 683a 3238 3c2f rtTesting.h:28
    .
    │ │ │ │ +0002b490: 3c64 6976 2063 6c61 7373 3d22 7474 6e61
    gtsa │ │ │ │ +0002b4e0: 6d3a 3a73 706c 6974 3c2f 613e 3c2f 6469 m::split
    void split │ │ │ │ +0002b510: 2863 6f6e 7374 2047 2026 616d 703b 672c (const G &g, │ │ │ │ +0002b520: 2063 6f6e 7374 2050 7265 6465 6365 7373 const Predecess │ │ │ │ +0002b530: 6f72 4d61 7026 6c74 3b20 4b45 5920 2667 orMap< KEY &g │ │ │ │ +0002b540: 743b 2026 616d 703b 7472 6565 2c20 4720 t; &tree, G │ │ │ │ +0002b550: 2661 6d70 3b41 6231 2c20 4720 2661 6d70 &Ab1, G & │ │ │ │ +0002b560: 3b41 6232 293c 2f64 6976 3e3c 6469 7620 ;Ab2)
    Sp │ │ │ │ +0002b580: 6c69 7420 7468 6520 6772 6170 6820 696e lit the graph in │ │ │ │ +0002b590: 746f 2074 776f 2070 6172 7473 3a20 6f6e to two parts: on │ │ │ │ +0002b5a0: 6520 636f 7272 6573 706f 6e64 7320 746f e corresponds to │ │ │ │ +0002b5b0: 2074 6865 2067 6976 656e 2073 7061 6e6e the given spann │ │ │ │ +0002b5c0: 696e 6720 7472 6565 2c20 616e 6420 7468 ing tree, and th │ │ │ │ +0002b5d0: 6520 6f74 6865 7220 636f 7272 6573 706f e other correspo │ │ │ │ +0002b5e0: 6e64 732e 2e2e 3c2f 6469 763e 3c64 6976 nds...
    < │ │ │ │ +0002b600: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +0002b610: 2067 7261 7068 2d69 6e6c 2e68 3a32 3535 graph-inl.h:255 │ │ │ │ +0002b620: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ +0002b6c0: 7361 6d3a 3a64 6f74 3c2f 613e 3c2f 6469 sam::dot
    double dot │ │ │ │ +0002b6f0: 2863 6f6e 7374 2056 3120 2661 6d70 3b61 (const V1 &a │ │ │ │ +0002b700: 2c20 636f 6e73 7420 5632 2026 616d 703b , const V2 & │ │ │ │ +0002b710: 6229 3c2f 6469 763e 3c64 6976 2063 6c61 b)
    Dot p │ │ │ │ +0002b730: 726f 6475 6374 2e3c 2f64 6976 3e3c 6469 roduct.
    │ │ │ │ +0002b750: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition Vector.h:195
    .< │ │ │ │ +0002b7e0: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ +0002b7f0: 223e 5465 6d70 6c61 7465 2074 6f20 6372 ">Template to cr │ │ │ │ +0002b800: 6561 7465 2061 2062 696e 6172 7920 7072 eate a binary pr │ │ │ │ +0002b810: 6564 6963 6174 652e 3c2f 6469 763e 3c64 edicate.
    Definition Testable.h:11 │ │ │ │ +0002b850: 313c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 1
    .
    gtsam │ │ │ │ +0002b8b0: 3a3a 4173 7369 676e 6d65 6e74 3c2f 613e ::Assignment │ │ │ │ +0002b8c0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    An assi │ │ │ │ +0002b8e0: 676e 6d65 6e74 2066 726f 6d20 6c61 6265 gnment from labe │ │ │ │ +0002b8f0: 6c73 2074 6f20 7661 6c75 6520 696e 6465 ls to value inde │ │ │ │ +0002b900: 7820 2873 697a 655f 7429 2e3c 2f64 6976 x (size_t).
    Definitio │ │ │ │ +0002b930: 6e3c 2f62 3e20 4173 7369 676e 6d65 6e74 n Assignment │ │ │ │ +0002b940: 2e68 3a33 373c 2f64 6976 3e3c 2f64 6976 .h:37
    .
    g │ │ │ │ +0002b9a0: 7473 616d 3a3a 4465 6369 7369 6f6e 5472 tsam::DecisionTr │ │ │ │ +0002b9b0: 6565 3a3a 4c65 6166 3c2f 613e 3c2f 6469 ee::Leaf
    Definiti │ │ │ │ +0002b9e0: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ +0002b9f0: 7265 652d 696e 6c2e 683a 3532 3c2f 6469 ree-inl.h:52
    .
    gtsam: │ │ │ │ +0002baa0: 3a44 6563 6973 696f 6e54 7265 653a 3a4c :DecisionTree::L │ │ │ │ +0002bab0: 6561 663a 3a63 686f 6f73 653c 2f61 3e3c eaf::choose< │ │ │ │ +0002bac0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    NodePtr │ │ │ │ +0002bae0: 2063 686f 6f73 6528 636f 6e73 7420 4c20 choose(const L │ │ │ │ +0002baf0: 2661 6d70 3b6c 6162 656c 2c20 7369 7a65 &label, size │ │ │ │ +0002bb00: 5f74 2069 6e64 6578 2920 636f 6e73 7420 _t index) const │ │ │ │ +0002bb10: 6f76 6572 7269 6465 3c2f 6469 763e 3c64 override
    choose a branch │ │ │ │ +0002bb40: 2c20 6372 6561 7465 206e 6577 206d 656d , create new mem │ │ │ │ +0002bb50: 6f72 7920 213c 2f64 6976 3e3c 6469 7620 ory !
    Definition │ │ │ │ +0002bb80: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +0002bb90: 2e68 3a31 3439 3c2f 6469 763e 3c2f 6469 .h:149
    .
    gtsam::Decis │ │ │ │ +0002bc40: 696f 6e54 7265 653a 3a4c 6561 663a 3a6f ionTree::Leaf::o │ │ │ │ +0002bc50: 7065 7261 746f 7228 293c 2f61 3e3c 2f64 perator()
    const Y & │ │ │ │ +0002bc80: 616d 703b 206f 7065 7261 746f 7228 2928 amp; operator()( │ │ │ │ +0002bc90: 636f 6e73 7420 4173 7369 676e 6d65 6e74 const Assignment │ │ │ │ +0002bca0: 266c 743b 204c 2026 6774 3b20 2661 6d70 < L > & │ │ │ │ +0002bcb0: 3b78 2920 636f 6e73 7420 6f76 6572 7269 ;x) const overri │ │ │ │ +0002bcc0: 6465 3c2f 6469 763e 3c64 6976 2063 6c61 de
    evalu │ │ │ │ +0002bce0: 6174 653c 2f64 6976 3e3c 6469 7620 636c ate
    D │ │ │ │ +0002bd00: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +0002bd10: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ +0002bd20: 3a31 3130 3c2f 6469 763e 3c2f 6469 763e :110
    │ │ │ │ +0002bd30: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    │ │ │ │ +0002be00: 4e6f 6465 5074 7220 6170 706c 7928 636f NodePtr apply(co │ │ │ │ +0002be10: 6e73 7420 556e 6172 7941 7373 6967 6e6d nst UnaryAssignm │ │ │ │ +0002be20: 656e 7420 2661 6d70 3b6f 702c 2063 6f6e ent &op, con │ │ │ │ +0002be30: 7374 2041 7373 6967 6e6d 656e 7426 6c74 st Assignment< │ │ │ │ +0002be40: 3b20 4c20 2667 743b 2026 616d 703b 6173 ; L > &as │ │ │ │ +0002be50: 7369 676e 6d65 6e74 2920 636f 6e73 7420 signment) const │ │ │ │ +0002be60: 6f76 6572 7269 6465 3c2f 6469 763e 3c64 override
    Apply unary ope │ │ │ │ +0002be90: 7261 746f 7220 7769 7468 2061 7373 6967 rator with assig │ │ │ │ +0002bea0: 6e6d 656e 742e 3c2f 6469 763e 3c64 6976 nment.
    < │ │ │ │ +0002bec0: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +0002bed0: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ +0002bee0: 6c2e 683a 3132 313c 2f64 6976 3e3c 2f64 l.h:121
    .
    │ │ │ │ +0002bfb0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    bool equals( │ │ │ │ +0002bfd0: 636f 6e73 7420 4e6f 6465 2026 616d 703b const Node & │ │ │ │ +0002bfe0: 712c 2063 6f6e 7374 2043 6f6d 7061 7265 q, const Compare │ │ │ │ +0002bff0: 4675 6e63 2026 616d 703b 636f 6d70 6172 Func &compar │ │ │ │ +0002c000: 6529 2063 6f6e 7374 206f 7665 7272 6964 e) const overrid │ │ │ │ +0002c010: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    equali │ │ │ │ +0002c030: 7479 2075 7020 746f 2074 6f6c 6572 616e ty up to toleran │ │ │ │ +0002c040: 6365 3c2f 6469 763e 3c64 6976 2063 6c61 ce
    De │ │ │ │ +0002c060: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ +0002c070: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ +0002c080: 3837 3c2f 6469 763e 3c2f 6469 763e 0a3c 87
    .< │ │ │ │ +0002c090: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +0002c0a0: 6964 3d22 6161 3032 3737 365f 6874 6d6c id="aa02776_html │ │ │ │ +0002c0b0: 5f61 3763 3231 6461 6661 6262 3566 6261 _a7c21dafabb5fba │ │ │ │ +0002c0c0: 3661 3661 6263 6362 3839 6564 3230 3739 6a6abccb89ed2079 │ │ │ │ +0002c0d0: 3563 223e 3c64 6976 2063 6c61 7373 3d22 5c">Y constant_
    constant s │ │ │ │ +0002c190: 746f 7265 6420 696e 2074 6869 7320 6c65 tored in this le │ │ │ │ +0002c1a0: 6166 3c2f 6469 763e 3c64 6976 2063 6c61 af
    De │ │ │ │ +0002c1c0: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ +0002c1d0: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ +0002c1e0: 3534 3c2f 6469 763e 3c2f 6469 763e 0a3c 54
    .< │ │ │ │ +0002c1f0: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +0002c200: 6964 3d22 6161 3032 3737 365f 6874 6d6c id="aa02776_html │ │ │ │ +0002c210: 5f61 3835 6536 3464 6336 6431 6434 6534 _a85e64dc6d1d4e4 │ │ │ │ +0002c220: 3063 6135 3639 6439 3739 6236 3535 3433 0ca569d979b65543 │ │ │ │ +0002c230: 6535 223e 3c64 6976 2063 6c61 7373 3d22 e5">
    vo │ │ │ │ +0002c2c0: 6964 2070 7269 6e74 2863 6f6e 7374 2073 id print(const s │ │ │ │ +0002c2d0: 7464 3a3a 7374 7269 6e67 2026 616d 703b td::string & │ │ │ │ +0002c2e0: 732c 2063 6f6e 7374 204c 6162 656c 466f s, const LabelFo │ │ │ │ +0002c2f0: 726d 6174 7465 7220 2661 6d70 3b6c 6162 rmatter &lab │ │ │ │ +0002c300: 656c 466f 726d 6174 7465 722c 2063 6f6e elFormatter, con │ │ │ │ +0002c310: 7374 2056 616c 7565 466f 726d 6174 7465 st ValueFormatte │ │ │ │ +0002c320: 7220 2661 6d70 3b76 616c 7565 466f 726d r &valueForm │ │ │ │ +0002c330: 6174 7465 7229 2063 6f6e 7374 206f 7665 atter) const ove │ │ │ │ +0002c340: 7272 6964 653c 2f64 6976 3e3c 6469 7620 rride
    pr │ │ │ │ +0002c360: 696e 743c 2f64 6976 3e3c 6469 7620 636c int
    D │ │ │ │ +0002c380: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +0002c390: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ +0002c3a0: 3a39 343c 2f64 6976 3e3c 2f64 6976 3e0a :94
    . │ │ │ │ +0002c3b0: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    N │ │ │ │ +0002c480: 6f64 6550 7472 2061 7070 6c79 2863 6f6e odePtr apply(con │ │ │ │ +0002c490: 7374 2055 6e61 7279 2026 616d 703b 6f70 st Unary &op │ │ │ │ +0002c4a0: 2920 636f 6e73 7420 6f76 6572 7269 6465 ) const override │ │ │ │ +0002c4b0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    apply u │ │ │ │ +0002c4d0: 6e61 7279 206f 7065 7261 746f 723c 2f64 nary operator
    Definit │ │ │ │ +0002c500: 696f 6e3c 2f62 3e20 4465 6369 7369 6f6e ion Decision │ │ │ │ +0002c510: 5472 6565 2d69 6e6c 2e68 3a31 3135 3c2f Tree-inl.h:115
    .
    │ │ │ │ +0002c570: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 .
    < │ │ │ │ +0002c760: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ +0002c770: 6922 3e4c 6561 6628 636f 6e73 7420 5920 i">Leaf(const Y │ │ │ │ +0002c780: 2661 6d70 3b63 6f6e 7374 616e 742c 2073 &constant, s │ │ │ │ +0002c790: 697a 655f 7420 6e72 4173 7369 676e 6d65 ize_t nrAssignme │ │ │ │ +0002c7a0: 6e74 733d 3129 3c2f 6469 763e 3c64 6976 nts=1)
    C │ │ │ │ +0002c7c0: 6f6e 7374 7275 6374 6f72 2066 726f 6d20 onstructor from │ │ │ │ +0002c7d0: 636f 6e73 7461 6e74 2e3c 2f64 6976 3e3c constant.
    < │ │ │ │ +0002c7e0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ +0002c7f0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ +0002c800: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ +0002c810: 2d69 6e6c 2e68 3a36 353c 2f64 6976 3e3c -inl.h:65
    < │ │ │ │ +0002c820: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    < │ │ │ │ +0002c880: 6120 6872 6566 3d22 6130 3237 3736 2e68 a href="a02776.h │ │ │ │ +0002c890: 746d 6c23 6162 3932 3036 3930 3338 3030 tml#ab9206903800 │ │ │ │ +0002c8a0: 3830 3735 3165 6138 3037 6535 6233 6365 80751ea807e5b3ce │ │ │ │ +0002c8b0: 3962 3638 3322 3e67 7473 616d 3a3a 4465 9b683">gtsam::De │ │ │ │ +0002c8c0: 6369 7369 6f6e 5472 6565 3a3a 4c65 6166 cisionTree::Leaf │ │ │ │ +0002c8d0: 3a3a 6e72 4173 7369 676e 6d65 6e74 735f ::nrAssignments_ │ │ │ │ +0002c8e0: 3c2f 613e 3c2f 6469 763e 3c64 6976 2063
    si │ │ │ │ +0002c900: 7a65 5f74 206e 7241 7373 6967 6e6d 656e ze_t nrAssignmen │ │ │ │ +0002c910: 7473 5f3c 2f64 6976 3e3c 6469 7620 636c ts_
    The │ │ │ │ +0002c930: 6e75 6d62 6572 206f 6620 6173 7369 676e number of assign │ │ │ │ +0002c940: 6d65 6e74 7320 636f 6e74 6169 6e65 6420 ments contained │ │ │ │ +0002c950: 7769 7468 696e 2074 6869 7320 6c65 6166 within this leaf │ │ │ │ +0002c960: 2e3c 2f64 6976 3e3c 6469 7620 636c 6173 .
    Def │ │ │ │ +0002c980: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ +0002c990: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a35 sionTree-inl.h:5 │ │ │ │ +0002c9a0: 393c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 9
    .
    void │ │ │ │ +0002ca80: 646f 7428 7374 643a 3a6f 7374 7265 616d dot(std::ostream │ │ │ │ +0002ca90: 2026 616d 703b 6f73 2c20 636f 6e73 7420 &os, const │ │ │ │ +0002caa0: 4c61 6265 6c46 6f72 6d61 7474 6572 2026 LabelFormatter & │ │ │ │ +0002cab0: 616d 703b 6c61 6265 6c46 6f72 6d61 7474 amp;labelFormatt │ │ │ │ +0002cac0: 6572 2c20 636f 6e73 7420 5661 6c75 6546 er, const ValueF │ │ │ │ +0002cad0: 6f72 6d61 7474 6572 2026 616d 703b 7661 ormatter &va │ │ │ │ +0002cae0: 6c75 6546 6f72 6d61 7474 6572 2c20 626f lueFormatter, bo │ │ │ │ +0002caf0: 6f6c 2073 686f 775a 6572 6f29 2063 6f6e ol showZero) con │ │ │ │ +0002cb00: 7374 206f 7665 7272 6964 653c 2f64 6976 st override
    Write graphv │ │ │ │ +0002cb30: 697a 2066 6f72 6d61 7420 746f 2073 7472 iz format to str │ │ │ │ +0002cb40: 6561 6d20 6f73 2e3c 2f64 6976 3e3c 6469 eam os.
    │ │ │ │ +0002cb60: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ +0002cb80: 6e6c 2e68 3a31 3030 3c2f 6469 763e 3c2f nl.h:100
    .
    < │ │ │ │ +0002cc50: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ +0002cc60: 6922 3e4c 6561 6628 293c 2f64 6976 3e3c i">Leaf()
    < │ │ │ │ +0002cc70: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ +0002cc80: 223e 4465 6661 756c 7420 636f 6e73 7472 ">Default constr │ │ │ │ +0002cc90: 7563 746f 7220 666f 7220 7365 7269 616c uctor for serial │ │ │ │ +0002cca0: 697a 6174 696f 6e2e 3c2f 6469 763e 3c64 ization.
    Definition DecisionTree- │ │ │ │ +0002cce0: 696e 6c2e 683a 3632 3c2f 6469 763e 3c2f inl.h:62
    .
    gtsam::Dec │ │ │ │ +0002cd90: 6973 696f 6e54 7265 653a 3a4c 6561 663a isionTree::Leaf: │ │ │ │ +0002cda0: 3a73 616d 654c 6561 663c 2f61 3e3c 2f64 :sameLeaf
    bool same │ │ │ │ +0002cdd0: 4c65 6166 2863 6f6e 7374 204e 6f64 6520 Leaf(const Node │ │ │ │ +0002cde0: 2661 6d70 3b71 2920 636f 6e73 7420 6f76 &q) const ov │ │ │ │ +0002cdf0: 6572 7269 6465 3c2f 6469 763e 3c64 6976 erride
    p │ │ │ │ +0002ce10: 6f6c 796d 6f72 7068 6963 2065 7175 616c olymorphic equal │ │ │ │ +0002ce20: 6974 793a 2069 7320 7120 6120 6c65 6166 ity: is q a leaf │ │ │ │ +0002ce30: 2061 6e64 2069 7320 6974 2074 6865 2073 and is it the s │ │ │ │ +0002ce40: 616d 6520 6173 2074 6869 7320 6c65 6166 ame as this leaf │ │ │ │ +0002ce50: 3f3c 2f64 6976 3e3c 6469 7620 636c 6173 ?
    Def │ │ │ │ +0002ce70: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ +0002ce80: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a38 sionTree-inl.h:8 │ │ │ │ +0002ce90: 323c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 2
    .
    │ │ │ │ +0002cf70: 636f 6e73 7420 5920 2661 6d70 3b20 636f const Y & co │ │ │ │ +0002cf80: 6e73 7461 6e74 2829 2063 6f6e 7374 3c2f nstant() const
    Return th │ │ │ │ +0002cfb0: 6520 636f 6e73 7461 6e74 2e3c 2f64 6976 e constant.
    Definitio │ │ │ │ +0002cfe0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +0002cff0: 6565 2d69 6e6c 2e68 3a36 393c 2f64 6976 ee-inl.h:69
    .
    s │ │ │ │ +0002d0e0: 697a 655f 7420 6e72 4173 7369 676e 6d65 ize_t nrAssignme │ │ │ │ +0002d0f0: 6e74 7328 2920 636f 6e73 743c 2f64 6976 nts() const
    Return the n │ │ │ │ +0002d120: 756d 6265 7220 6f66 2061 7373 6967 6e6d umber of assignm │ │ │ │ +0002d130: 656e 7473 2063 6f6e 7461 696e 6564 2077 ents contained w │ │ │ │ +0002d140: 6974 6869 6e20 7468 6973 206c 6561 662e ithin this leaf. │ │ │ │ +0002d150: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ +0002d170: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ +0002d180: 696f 6e54 7265 652d 696e 6c2e 683a 3734 ionTree-inl.h:74 │ │ │ │ +0002d190: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    . │ │ │ │ +0002d1c0: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 < │ │ │ │ +0002d210: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ +0002d220: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ +0002d230: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ +0002d240: 2d69 6e6c 2e68 3a31 3732 3c2f 6469 763e -inl.h:172
    │ │ │ │ +0002d250: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0002d2b0: 3c61 2068 7265 663d 2261 3032 3738 302e gtsam::D │ │ │ │ +0002d2f0: 6563 6973 696f 6e54 7265 653a 3a43 686f ecisionTree::Cho │ │ │ │ +0002d300: 6963 653a 3a61 7070 6c79 3c2f 613e 3c2f ice::apply
    NodePtr │ │ │ │ +0002d330: 6170 706c 7928 636f 6e73 7420 556e 6172 apply(const Unar │ │ │ │ +0002d340: 7920 2661 6d70 3b6f 7029 2063 6f6e 7374 y &op) const │ │ │ │ +0002d350: 206f 7665 7272 6964 653c 2f64 6976 3e3c override
    < │ │ │ │ +0002d360: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ +0002d370: 223e 6170 706c 7920 756e 6172 7920 6f70 ">apply unary op │ │ │ │ +0002d380: 6572 6174 6f72 2e3c 2f64 6976 3e3c 6469 erator.
    │ │ │ │ +0002d3a0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ +0002d3c0: 6e6c 2e68 3a33 3839 3c2f 6469 763e 3c2f nl.h:389
    .
    gtsam::Dec │ │ │ │ +0002d470: 6973 696f 6e54 7265 653a 3a43 686f 6963 isionTree::Choic │ │ │ │ +0002d480: 653a 3a43 686f 6963 653c 2f61 3e3c 2f64 e::Choice
    Choice(co │ │ │ │ +0002d4b0: 6e73 7420 4c20 2661 6d70 3b6c 6162 656c nst L &label │ │ │ │ +0002d4c0: 2c20 636f 6e73 7420 4368 6f69 6365 2026 , const Choice & │ │ │ │ +0002d4d0: 616d 703b 662c 2063 6f6e 7374 2055 6e61 amp;f, const Una │ │ │ │ +0002d4e0: 7279 4173 7369 676e 6d65 6e74 2026 616d ryAssignment &am │ │ │ │ +0002d4f0: 703b 6f70 2c20 636f 6e73 7420 4173 7369 p;op, const Assi │ │ │ │ +0002d500: 676e 6d65 6e74 266c 743b 204c 2026 6774 gnment< L > │ │ │ │ +0002d510: 3b20 2661 6d70 3b61 7373 6967 6e6d 656e ; &assignmen │ │ │ │ +0002d520: 7429 3c2f 6469 763e 3c64 6976 2063 6c61 t)
    Const │ │ │ │ +0002d540: 7275 6374 6f72 2077 6869 6368 2061 6363 ructor which acc │ │ │ │ +0002d550: 6570 7473 2061 2055 6e61 7279 4173 7369 epts a UnaryAssi │ │ │ │ +0002d560: 676e 6d65 6e74 206f 7020 616e 6420 7468 gnment op and th │ │ │ │ +0002d570: 6520 636f 7272 6573 706f 6e64 696e 6720 e corresponding │ │ │ │ +0002d580: 6173 7369 676e 6d65 6e74 2e3c 2f64 6976 assignment.
    Definitio │ │ │ │ +0002d5b0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +0002d5c0: 6565 2d69 6e6c 2e68 3a33 3639 3c2f 6469 ee-inl.h:369
    .
    const │ │ │ │ +0002d6b0: 4c20 2661 6d70 3b20 6c61 6265 6c28 2920 L & label() │ │ │ │ +0002d6c0: 636f 6e73 743c 2f64 6976 3e3c 6469 7620 const
    Re │ │ │ │ +0002d6e0: 7475 726e 2074 6865 206c 6162 656c 206f turn the label o │ │ │ │ +0002d6f0: 6620 7468 6973 2063 686f 6963 6520 6e6f f this choice no │ │ │ │ +0002d700: 6465 2e3c 2f64 6976 3e3c 6469 7620 636c de.
    D │ │ │ │ +0002d720: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +0002d730: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ +0002d740: 3a32 3538 3c2f 6469 763e 3c2f 6469 763e :258
    │ │ │ │ +0002d750: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    void print(con │ │ │ │ +0002d830: 7374 2073 7464 3a3a 7374 7269 6e67 2026 st std::string & │ │ │ │ +0002d840: 616d 703b 732c 2063 6f6e 7374 204c 6162 amp;s, const Lab │ │ │ │ +0002d850: 656c 466f 726d 6174 7465 7220 2661 6d70 elFormatter & │ │ │ │ +0002d860: 3b6c 6162 656c 466f 726d 6174 7465 722c ;labelFormatter, │ │ │ │ +0002d870: 2063 6f6e 7374 2056 616c 7565 466f 726d const ValueForm │ │ │ │ +0002d880: 6174 7465 7220 2661 6d70 3b76 616c 7565 atter &value │ │ │ │ +0002d890: 466f 726d 6174 7465 7229 2063 6f6e 7374 Formatter) const │ │ │ │ +0002d8a0: 206f 7665 7272 6964 653c 2f64 6976 3e3c override
    < │ │ │ │ +0002d8b0: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ +0002d8c0: 223e 7072 696e 7420 2861 7320 6120 7472 ">print (as a tr │ │ │ │ +0002d8d0: 6565 292e 3c2f 6469 763e 3c64 6976 2063 ee).
    │ │ │ │ +0002d8f0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +0002d900: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ +0002d910: 683a 3238 303c 2f64 6976 3e3c 2f64 6976 h:280
    .
    < │ │ │ │ +0002d9e0: 6469 7620 636c 6173 733d 2274 7464 6563 div class="ttdec │ │ │ │ +0002d9f0: 6922 3e4e 6f64 6550 7472 2061 7070 6c79 i">NodePtr apply │ │ │ │ +0002da00: 2863 6f6e 7374 2055 6e61 7279 4173 7369 (const UnaryAssi │ │ │ │ +0002da10: 676e 6d65 6e74 2026 616d 703b 6f70 2c20 gnment &op, │ │ │ │ +0002da20: 636f 6e73 7420 4173 7369 676e 6d65 6e74 const Assignment │ │ │ │ +0002da30: 266c 743b 204c 2026 6774 3b20 2661 6d70 < L > & │ │ │ │ +0002da40: 3b61 7373 6967 6e6d 656e 7429 2063 6f6e ;assignment) con │ │ │ │ +0002da50: 7374 206f 7665 7272 6964 653c 2f64 6976 st override
    Apply unary │ │ │ │ +0002da80: 6f70 6572 6174 6f72 2077 6974 6820 6173 operator with as │ │ │ │ +0002da90: 7369 676e 6d65 6e74 2e3c 2f64 6976 3e3c signment.
    < │ │ │ │ +0002daa0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ +0002dab0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ +0002dac0: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ +0002dad0: 2d69 6e6c 2e68 3a33 3935 3c2f 6469 763e -inl.h:395
    │ │ │ │ +0002dae0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +0002db40: 3c61 2068 7265 663d 2261 3032 3738 302e gtsam::D │ │ │ │ +0002db80: 6563 6973 696f 6e54 7265 653a 3a43 686f ecisionTree::Cho │ │ │ │ +0002db90: 6963 653a 3a6c 6162 656c 5f3c 2f61 3e3c ice::label_< │ │ │ │ +0002dba0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    L label │ │ │ │ +0002dbc0: 5f3c 2f64 6976 3e3c 6469 7620 636c 6173 _
    the la │ │ │ │ +0002dbe0: 6265 6c20 6f66 2074 6865 2076 6172 6961 bel of the varia │ │ │ │ +0002dbf0: 626c 6520 6f6e 2077 6869 6368 2077 6520 ble on which we │ │ │ │ +0002dc00: 7370 6c69 743c 2f64 6976 3e3c 6469 7620 split
    Definition │ │ │ │ +0002dc30: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +0002dc40: 2e68 3a31 3734 3c2f 6469 763e 3c2f 6469 .h:174
    .
    gtsam::Decis │ │ │ │ +0002dcf0: 696f 6e54 7265 653a 3a43 686f 6963 653a ionTree::Choice: │ │ │ │ +0002dd00: 3a73 616d 654c 6561 663c 2f61 3e3c 2f64 :sameLeaf
    bool same │ │ │ │ +0002dd30: 4c65 6166 2863 6f6e 7374 204e 6f64 6520 Leaf(const Node │ │ │ │ +0002dd40: 2661 6d70 3b71 2920 636f 6e73 7420 6f76 &q) const ov │ │ │ │ +0002dd50: 6572 7269 6465 3c2f 6469 763e 3c64 6976 erride
    p │ │ │ │ +0002dd70: 6f6c 796d 6f72 7068 6963 2065 7175 616c olymorphic equal │ │ │ │ +0002dd80: 6974 793a 2069 6620 7120 6973 2061 206c ity: if q is a l │ │ │ │ +0002dd90: 6561 662c 2063 6f75 6c64 2062 652e 2e2e eaf, could be... │ │ │ │ +0002dda0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ +0002ddc0: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ +0002ddd0: 696f 6e54 7265 652d 696e 6c2e 683a 3331 ionTree-inl.h:31 │ │ │ │ +0002dde0: 383c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 8
    .
    │ │ │ │ +0002dec0: 4368 6f69 6365 2863 6f6e 7374 2043 686f Choice(const Cho │ │ │ │ +0002ded0: 6963 6520 2661 6d70 3b66 2c20 636f 6e73 ice &f, cons │ │ │ │ +0002dee0: 7420 4368 6f69 6365 2026 616d 703b 672c t Choice &g, │ │ │ │ +0002def0: 2063 6f6e 7374 2042 696e 6172 7920 2661 const Binary &a │ │ │ │ +0002df00: 6d70 3b6f 7029 3c2f 6469 763e 3c64 6976 mp;op)
    C │ │ │ │ +0002df20: 6f6e 7374 7275 6374 2066 726f 6d20 6170 onstruct from ap │ │ │ │ +0002df30: 706c 7969 6e67 2062 696e 6172 7920 6f70 plying binary op │ │ │ │ +0002df40: 2074 6f20 7477 6f20 4368 6f69 6365 206e to two Choice n │ │ │ │ +0002df50: 6f64 6573 2e3c 2f64 6976 3e3c 6469 7620 odes.
    Definition │ │ │ │ +0002df80: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +0002df90: 2e68 3a32 3330 3c2f 6469 763e 3c2f 6469 .h:230
    .
    gtsam::Decis │ │ │ │ +0002e040: 696f 6e54 7265 653a 3a43 686f 6963 653a ionTree::Choice: │ │ │ │ +0002e050: 3a70 7573 685f 6261 636b 3c2f 613e 3c2f :push_back
    void pus │ │ │ │ +0002e080: 685f 6261 636b 2863 6f6e 7374 204e 6f64 h_back(const Nod │ │ │ │ +0002e090: 6550 7472 2026 616d 703b 6e6f 6465 293c ePtr &node)< │ │ │ │ +0002e0a0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    add a br │ │ │ │ +0002e0c0: 616e 6368 3a20 544f 444f 206d 6572 6765 anch: TODO merge │ │ │ │ +0002e0d0: 2069 6e74 6f20 636f 6e73 7472 7563 746f into constructo │ │ │ │ +0002e0e0: 723c 2f64 6976 3e3c 6469 7620 636c 6173 r
    Def │ │ │ │ +0002e100: 696e 6974 696f 6e3c 2f62 3e20 4465 6369 inition Deci │ │ │ │ +0002e110: 7369 6f6e 5472 6565 2d69 6e6c 2e68 3a32 sionTree-inl.h:2 │ │ │ │ +0002e120: 3731 3c2f 6469 763e 3c2f 6469 763e 0a3c 71
    .< │ │ │ │ +0002e130: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +0002e140: 6964 3d22 6161 3032 3738 305f 6874 6d6c id="aa02780_html │ │ │ │ +0002e150: 5f61 6131 3132 3136 6433 3066 6131 6430 _aa11216d30fa1d0 │ │ │ │ +0002e160: 6162 6632 6634 6432 3666 3832 6331 6637 abf2f4d26f82c1f7 │ │ │ │ +0002e170: 3832 223e 3c64 6976 2063 6c61 7373 3d22 82"> │ │ │ │ +0002e1f0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    std::vector& │ │ │ │ +0002e210: 6c74 3b20 4e6f 6465 5074 7220 2667 743b lt; NodePtr > │ │ │ │ +0002e220: 2062 7261 6e63 6865 735f 3c2f 6469 763e branches_
    │ │ │ │ +0002e230: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    The children │ │ │ │ +0002e250: 6f66 2074 6869 7320 4368 6f69 6365 206e of this Choice n │ │ │ │ +0002e260: 6f64 652e 3c2f 6469 763e 3c64 6976 2063 ode.
    │ │ │ │ +0002e280: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +0002e290: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ +0002e2a0: 683a 3137 373c 2f64 6976 3e3c 2f64 6976 h:177
    .
    │ │ │ │ +0002e370: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    Choice()
    Default con │ │ │ │ +0002e3b0: 7374 7275 6374 6f72 2066 6f72 2073 6572 structor for ser │ │ │ │ +0002e3c0: 6961 6c69 7a61 7469 6f6e 2e3c 2f64 6976 ialization.
    Definitio │ │ │ │ +0002e3f0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +0002e400: 6565 2d69 6e6c 2e68 3a31 3930 3c2f 6469 ee-inl.h:190
    .
    c │ │ │ │ +0002e4f0: 6f6e 7374 2059 2026 616d 703b 206f 7065 onst Y & ope │ │ │ │ +0002e500: 7261 746f 7228 2928 636f 6e73 7420 4173 rator()(const As │ │ │ │ +0002e510: 7369 676e 6d65 6e74 266c 743b 204c 2026 signment< L & │ │ │ │ +0002e520: 6774 3b20 2661 6d70 3b78 2920 636f 6e73 gt; &x) cons │ │ │ │ +0002e530: 7420 6f76 6572 7269 6465 3c2f 6469 763e t override
    │ │ │ │ +0002e540: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    evaluate
    Definitio │ │ │ │ +0002e580: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +0002e590: 6565 2d69 6e6c 2e68 3a33 3336 3c2f 6469 ee-inl.h:336
    .
    Choic │ │ │ │ +0002e680: 6528 636f 6e73 7420 4c20 2661 6d70 3b6c e(const L &l │ │ │ │ +0002e690: 6162 656c 2c20 7369 7a65 5f74 2063 6f75 abel, size_t cou │ │ │ │ +0002e6a0: 6e74 293c 2f64 6976 3e3c 6469 7620 636c nt)
    Cons │ │ │ │ +0002e6c0: 7472 7563 746f 722c 2067 6976 656e 2063 tructor, given c │ │ │ │ +0002e6d0: 686f 6963 6520 6c61 6265 6c20 616e 6420 hoice label and │ │ │ │ +0002e6e0: 6d61 6e64 6174 6f72 7920 6578 7065 6374 mandatory expect │ │ │ │ +0002e6f0: 6564 2062 7261 6e63 6820 636f 756e 742e ed branch count. │ │ │ │ +0002e700: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ +0002e720: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ +0002e730: 696f 6e54 7265 652d 696e 6c2e 683a 3232 ionTree-inl.h:22 │ │ │ │ +0002e740: 343c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 4
    .
    │ │ │ │ +0002e820: 4e6f 6465 5074 7220 6368 6f6f 7365 2863 NodePtr choose(c │ │ │ │ +0002e830: 6f6e 7374 204c 2026 616d 703b 6c61 6265 onst L &labe │ │ │ │ +0002e840: 6c2c 2073 697a 655f 7420 696e 6465 7829 l, size_t index) │ │ │ │ +0002e850: 2063 6f6e 7374 206f 7665 7272 6964 653c const override< │ │ │ │ +0002e860: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    choose a │ │ │ │ +0002e880: 2062 7261 6e63 682c 2072 6563 7572 7369 branch, recursi │ │ │ │ +0002e890: 7665 6c79 3c2f 6469 763e 3c64 6976 2063 vely
    │ │ │ │ +0002e8b0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +0002e8c0: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ +0002e8d0: 683a 3433 343c 2f64 6976 3e3c 2f64 6976 h:434
    .
    void dot(std::o │ │ │ │ +0002e9c0: 7374 7265 616d 2026 616d 703b 6f73 2c20 stream &os, │ │ │ │ +0002e9d0: 636f 6e73 7420 4c61 6265 6c46 6f72 6d61 const LabelForma │ │ │ │ +0002e9e0: 7474 6572 2026 616d 703b 6c61 6265 6c46 tter &labelF │ │ │ │ +0002e9f0: 6f72 6d61 7474 6572 2c20 636f 6e73 7420 ormatter, const │ │ │ │ +0002ea00: 5661 6c75 6546 6f72 6d61 7474 6572 2026 ValueFormatter & │ │ │ │ +0002ea10: 616d 703b 7661 6c75 6546 6f72 6d61 7474 amp;valueFormatt │ │ │ │ +0002ea20: 6572 2c20 626f 6f6c 2073 686f 775a 6572 er, bool showZer │ │ │ │ +0002ea30: 6f29 2063 6f6e 7374 206f 7665 7272 6964 o) const overrid │ │ │ │ +0002ea40: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    output │ │ │ │ +0002ea60: 2074 6f20 6772 6170 6876 697a 2028 6173 to graphviz (as │ │ │ │ +0002ea70: 2061 2061 2067 7261 7068 293c 2f64 6976 a a graph)
    Definitio │ │ │ │ +0002eaa0: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +0002eab0: 6565 2d69 6e6c 2e68 3a32 3930 3c2f 6469 ee-inl.h:290
    .< │ │ │ │ +0002ec50: 2f64 6976 3e0a 3c64 6976 2063 6c61 7373 /div>.
    < │ │ │ │ +0002ecb0: 6120 6872 6566 3d22 6130 3237 3830 2e68 a href="a02780.h │ │ │ │ +0002ecc0: 746d 6c23 6166 3936 6332 3331 3633 3662 tml#af96c231636b │ │ │ │ +0002ecd0: 3534 3862 6234 6466 3934 3165 3564 3137 548bb4df941e5d17 │ │ │ │ +0002ece0: 3265 3032 6622 3e67 7473 616d 3a3a 4465 2e02f">gtsam::De │ │ │ │ +0002ecf0: 6369 7369 6f6e 5472 6565 3a3a 4368 6f69 cisionTree::Choi │ │ │ │ +0002ed00: 6365 3a3a 556e 6971 7565 3c2f 613e 3c2f ce::Unique
    static N │ │ │ │ +0002ed30: 6f64 6550 7472 2055 6e69 7175 6528 636f odePtr Unique(co │ │ │ │ +0002ed40: 6e73 7420 4368 6f69 6365 5074 7220 2661 nst ChoicePtr &a │ │ │ │ +0002ed50: 6d70 3b66 293c 2f64 6976 3e3c 6469 7620 mp;f)
    If │ │ │ │ +0002ed70: 2061 6c6c 2062 7261 6e63 6865 7320 6f66 all branches of │ │ │ │ +0002ed80: 2061 2063 686f 6963 6520 6e6f 6465 2066 a choice node f │ │ │ │ +0002ed90: 2061 7265 2074 6865 2073 616d 652c 206a are the same, j │ │ │ │ +0002eda0: 7573 7420 7265 7475 726e 2061 2062 7261 ust return a bra │ │ │ │ +0002edb0: 6e63 682e 3c2f 6469 763e 3c64 6976 2063 nch.
    │ │ │ │ +0002edd0: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +0002ede0: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ +0002edf0: 683a 3230 303c 2f64 6976 3e3c 2f64 6976 h:200
    .
    │ │ │ │ +0002eec0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    bool equals( │ │ │ │ +0002eee0: 636f 6e73 7420 4e6f 6465 2026 616d 703b const Node & │ │ │ │ +0002eef0: 712c 2063 6f6e 7374 2043 6f6d 7061 7265 q, const Compare │ │ │ │ +0002ef00: 4675 6e63 2026 616d 703b 636f 6d70 6172 Func &compar │ │ │ │ +0002ef10: 6529 2063 6f6e 7374 206f 7665 7272 6964 e) const overrid │ │ │ │ +0002ef20: 653c 2f64 6976 3e3c 6469 7620 636c 6173 e
    equali │ │ │ │ +0002ef40: 7479 3c2f 6469 763e 3c64 6976 2063 6c61 ty
    De │ │ │ │ +0002ef60: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ +0002ef70: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ +0002ef80: 3332 333c 2f64 6976 3e3c 2f64 6976 3e0a 323
    . │ │ │ │ +0002ef90: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    gts │ │ │ │ +0002efe0: 616d 3a3a 5669 7369 743c 2f61 3e3c 2f64 am::Visit
    Functor pe │ │ │ │ +0002f010: 7266 6f72 6d69 6e67 2064 6570 7468 2d66 rforming depth-f │ │ │ │ +0002f020: 6972 7374 2076 6973 6974 2074 6f20 6561 irst visit to ea │ │ │ │ +0002f030: 6368 206c 6561 6620 7769 7468 2074 6865 ch leaf with the │ │ │ │ +0002f040: 206c 6561 6620 7661 6c75 6520 6173 2074 leaf value as t │ │ │ │ +0002f050: 6865 2061 7267 756d 656e 742e 3c2f 6469 he argument.
    Definiti │ │ │ │ +0002f080: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ +0002f090: 7265 652d 696e 6c2e 683a 3731 353c 2f64 ree-inl.h:715
    .
    < │ │ │ │ +0002f0f0: 6469 7620 636c 6173 733d 2274 746e 616d div class="ttnam │ │ │ │ +0002f100: 6522 3e3c 6120 6872 6566 3d22 6130 3237 e">gtsam │ │ │ │ +0002f140: 3a3a 5669 7369 743a 3a66 3c2f 613e 3c2f ::Visit::f
    F f
    folding func │ │ │ │ +0002f190: 7469 6f6e 206f 626a 6563 742e 3c2f 6469 tion object.
    Definiti │ │ │ │ +0002f1c0: 6f6e 3c2f 623e 2044 6563 6973 696f 6e54 on DecisionT │ │ │ │ +0002f1d0: 7265 652d 696e 6c2e 683a 3731 383c 2f64 ree-inl.h:718
    .
    < │ │ │ │ +0002f230: 6469 7620 636c 6173 733d 2274 746e 616d div class="ttnam │ │ │ │ +0002f240: 6522 3e3c 6120 6872 6566 3d22 6130 3237 e">gtsam │ │ │ │ +0002f280: 3a3a 5669 7369 743a 3a6f 7065 7261 746f ::Visit::operato │ │ │ │ +0002f290: 7228 293c 2f61 3e3c 2f64 6976 3e3c 6469 r()
    void operator() │ │ │ │ +0002f2c0: 2863 6f6e 7374 2074 7970 656e 616d 6520 (const typename │ │ │ │ +0002f2d0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ +0002f2e0: 204c 2c20 5920 2667 743b 3a3a 4e6f 6465 L, Y >::Node │ │ │ │ +0002f2f0: 5074 7220 2661 6d70 3b6e 6f64 6529 2063 Ptr &node) c │ │ │ │ +0002f300: 6f6e 7374 3c2f 6469 763e 3c64 6976 2063 onst
    Do │ │ │ │ +0002f320: 6120 6465 7074 682d 6669 7273 7420 7669 a depth-first vi │ │ │ │ +0002f330: 7369 7420 6f6e 2074 6865 2074 7265 6520 sit on the tree │ │ │ │ +0002f340: 726f 6f74 6564 2061 7420 6e6f 6465 2e3c rooted at node.< │ │ │ │ +0002f350: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ +0002f370: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ +0002f380: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3231 onTree-inl.h:721 │ │ │ │ +0002f390: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    V │ │ │ │ +0002f460: 6973 6974 2846 2066 293c 2f64 6976 3e3c isit(F f)
    < │ │ │ │ +0002f470: 6469 7620 636c 6173 733d 2274 7464 6f63 div class="ttdoc │ │ │ │ +0002f480: 223e 436f 6e73 7472 7563 7420 6672 6f6d ">Construct from │ │ │ │ +0002f490: 2066 6f6c 6469 6e67 2066 756e 6374 696f folding functio │ │ │ │ +0002f4a0: 6e2e 3c2f 6469 763e 3c64 6976 2063 6c61 n.
    De │ │ │ │ +0002f4c0: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ +0002f4d0: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ +0002f4e0: 3731 373c 2f64 6976 3e3c 2f64 6976 3e0a 717
    . │ │ │ │ +0002f4f0: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    Functo │ │ │ │ +0002f570: 7220 7065 7266 6f72 6d69 6e67 2064 6570 r performing dep │ │ │ │ +0002f580: 7468 2d66 6972 7374 2076 6973 6974 2074 th-first visit t │ │ │ │ +0002f590: 6f20 6561 6368 206c 6561 6620 7769 7468 o each leaf with │ │ │ │ +0002f5a0: 2074 6865 204c 6561 6620 6f62 6a65 6374 the Leaf object │ │ │ │ +0002f5b0: 2070 6173 7365 6420 6173 2061 6e20 6172 passed as an ar │ │ │ │ +0002f5c0: 6775 6d65 6e74 2e3c 2f64 6976 3e3c 6469 gument.
    │ │ │ │ +0002f5e0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ +0002f600: 6e6c 2e68 3a37 3532 3c2f 6469 763e 3c2f nl.h:752
    .
    V │ │ │ │ +0002f6e0: 6973 6974 4c65 6166 2846 2066 293c 2f64 isitLeaf(F f)
    Construct │ │ │ │ +0002f710: 6672 6f6d 2066 6f6c 6469 6e67 2066 756e from folding fun │ │ │ │ +0002f720: 6374 696f 6e2e 3c2f 6469 763e 3c64 6976 ction.
    < │ │ │ │ +0002f740: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +0002f750: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ +0002f760: 6c2e 683a 3735 343c 2f64 6976 3e3c 2f64 l.h:754
    .
    v │ │ │ │ +0002f840: 6f69 6420 6f70 6572 6174 6f72 2829 2863 oid operator()(c │ │ │ │ +0002f850: 6f6e 7374 2074 7970 656e 616d 6520 4465 onst typename De │ │ │ │ +0002f860: 6369 7369 6f6e 5472 6565 266c 743b 204c cisionTree< L │ │ │ │ +0002f870: 2c20 5920 2667 743b 3a3a 4e6f 6465 5074 , Y >::NodePt │ │ │ │ +0002f880: 7220 2661 6d70 3b6e 6f64 6529 2063 6f6e r &node) con │ │ │ │ +0002f890: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Do a │ │ │ │ +0002f8b0: 6465 7074 682d 6669 7273 7420 7669 7369 depth-first visi │ │ │ │ +0002f8c0: 7420 6f6e 2074 6865 2074 7265 6520 726f t on the tree ro │ │ │ │ +0002f8d0: 6f74 6564 2061 7420 6e6f 6465 2e3c 2f64 oted at node.
    Definit │ │ │ │ +0002f900: 696f 6e3c 2f62 3e20 4465 6369 7369 6f6e ion Decision │ │ │ │ +0002f910: 5472 6565 2d69 6e6c 2e68 3a37 3538 3c2f Tree-inl.h:758
    ..< │ │ │ │ +0002fa70: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +0002fa80: 6964 3d22 6161 3032 3739 325f 6874 6d6c id="aa02792_html │ │ │ │ +0002fa90: 223e 3c64 6976 2063 6c61 7373 3d22 7474 ">
    gtsa │ │ │ │ +0002fac0: 6d3a 3a56 6973 6974 5769 7468 3c2f 613e m::VisitWith │ │ │ │ +0002fad0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Functor │ │ │ │ +0002faf0: 2070 6572 666f 726d 696e 6720 6465 7074 performing dept │ │ │ │ +0002fb00: 682d 6669 7273 7420 7669 7369 7420 746f h-first visit to │ │ │ │ +0002fb10: 2065 6163 6820 6c65 6166 2077 6974 6820 each leaf with │ │ │ │ +0002fb20: 7468 6520 6c65 6166 2773 2041 7373 6967 the leaf's Assig │ │ │ │ +0002fb30: 6e6d 656e 7426 6c74 3b4c 2667 743b 2061 nment<L> a │ │ │ │ +0002fb40: 6e64 2076 616c 7565 2070 6173 7365 6420 nd value passed │ │ │ │ +0002fb50: 6173 2061 2e2e 2e3c 2f64 6976 3e3c 6469 as a...
    │ │ │ │ +0002fb70: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ +0002fb90: 6e6c 2e68 3a37 3836 3c2f 6469 763e 3c2f nl.h:786
    .
    V │ │ │ │ +0002fc70: 6973 6974 5769 7468 2846 2066 293c 2f64 isitWith(F f)
    Construct │ │ │ │ +0002fca0: 6672 6f6d 2066 6f6c 6469 6e67 2066 756e from folding fun │ │ │ │ +0002fcb0: 6374 696f 6e2e 3c2f 6469 763e 3c64 6976 ction.
    < │ │ │ │ +0002fcd0: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +0002fce0: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ +0002fcf0: 6c2e 683a 3738 383c 2f64 6976 3e3c 2f64 l.h:788
    .
    A │ │ │ │ +0002fdd0: 7373 6967 6e6d 656e 7426 6c74 3b20 4c20 ssignment< L │ │ │ │ +0002fde0: 2667 743b 2061 7373 6967 6e6d 656e 743c > assignment< │ │ │ │ +0002fdf0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Assignme │ │ │ │ +0002fe10: 6e74 2c20 6d75 7461 7469 6e67 2074 6872 nt, mutating thr │ │ │ │ +0002fe20: 6f75 6768 2072 6563 7572 7369 6f6e 2e3c ough recursion.< │ │ │ │ +0002fe30: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ +0002fe50: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ +0002fe60: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3839 onTree-inl.h:789 │ │ │ │ +0002fe70: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ +0002ff10: 7361 6d3a 3a56 6973 6974 5769 7468 3a3a sam::VisitWith:: │ │ │ │ +0002ff20: 6f70 6572 6174 6f72 2829 3c2f 613e 3c2f operator()
    void ope │ │ │ │ +0002ff50: 7261 746f 7228 2928 636f 6e73 7420 7479 rator()(const ty │ │ │ │ +0002ff60: 7065 6e61 6d65 2044 6563 6973 696f 6e54 pename DecisionT │ │ │ │ +0002ff70: 7265 6526 6c74 3b20 4c2c 2059 2026 6774 ree< L, Y > │ │ │ │ +0002ff80: 3b3a 3a4e 6f64 6550 7472 2026 616d 703b ;::NodePtr & │ │ │ │ +0002ff90: 6e6f 6465 293c 2f64 6976 3e3c 6469 7620 node)
    Do │ │ │ │ +0002ffb0: 2061 2064 6570 7468 2d66 6972 7374 2076 a depth-first v │ │ │ │ +0002ffc0: 6973 6974 206f 6e20 7468 6520 7472 6565 isit on the tree │ │ │ │ +0002ffd0: 2072 6f6f 7465 6420 6174 206e 6f64 652e rooted at node. │ │ │ │ +0002ffe0: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Defi │ │ │ │ +00030000: 6e69 7469 6f6e 3c2f 623e 2044 6563 6973 nition Decis │ │ │ │ +00030010: 696f 6e54 7265 652d 696e 6c2e 683a 3739 ionTree-inl.h:79 │ │ │ │ +00030020: 333c 2f64 6976 3e3c 2f64 6976 3e0a 3c64 3
    .
    │ │ │ │ +000300f0: 4620 663c 2f64 6976 3e3c 6469 7620 636c F f
    fold │ │ │ │ +00030110: 696e 6720 6675 6e63 7469 6f6e 206f 626a ing function obj │ │ │ │ +00030120: 6563 742e 3c2f 6469 763e 3c64 6976 2063 ect.
    │ │ │ │ +00030140: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +00030150: 6563 6973 696f 6e54 7265 652d 696e 6c2e ecisionTree-inl. │ │ │ │ +00030160: 683a 3739 303c 2f64 6976 3e3c 2f64 6976 h:790
    .
    a │ │ │ │ +000301f0: 2064 6563 6973 696f 6e20 7472 6565 2069 decision tree i │ │ │ │ +00030200: 7320 6120 6675 6e63 7469 6f6e 2066 726f s a function fro │ │ │ │ +00030210: 6d20 6173 7369 676e 6d65 6e74 7320 746f m assignments to │ │ │ │ +00030220: 2076 616c 7565 732e 3c2f 6469 763e 3c64 values.
    Definition DecisionTree. │ │ │ │ +00030260: 683a 3631 3c2f 6469 763e 3c2f 6469 763e h:61
    │ │ │ │ +00030270: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    gtsam::Decisio │ │ │ │ +00030310: 6e54 7265 653a 3a61 7070 6c79 3c2f 613e nTree::apply │ │ │ │ +00030320: 3c2f 6469 763e 3c64 6976 2063 6c61 7373
    Decisi │ │ │ │ +00030340: 6f6e 5472 6565 2061 7070 6c79 2863 6f6e onTree apply(con │ │ │ │ +00030350: 7374 2055 6e61 7279 2026 616d 703b 6f70 st Unary &op │ │ │ │ +00030360: 2920 636f 6e73 743c 2f64 6976 3e3c 6469 ) const
    │ │ │ │ +00030380: 6170 706c 7920 556e 6172 7920 6f70 6572 apply Unary oper │ │ │ │ +00030390: 6174 696f 6e20 2671 756f 743b 6f70 2671 ation "op&q │ │ │ │ +000303a0: 756f 743b 2074 6f20 663c 2f64 6976 3e3c uot; to f
    < │ │ │ │ +000303b0: 6469 7620 636c 6173 733d 2274 7464 6566 div class="ttdef │ │ │ │ +000303c0: 223e 3c62 3e44 6566 696e 6974 696f 6e3c ">Definition< │ │ │ │ +000303d0: 2f62 3e20 4465 6369 7369 6f6e 5472 6565 /b> DecisionTree │ │ │ │ +000303e0: 2d69 6e6c 2e68 3a38 3839 3c2f 6469 763e -inl.h:889
    │ │ │ │ +000303f0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
    .
    │ │ │ │ +00030450: 3c61 2068 7265 663d 2261 3032 3739 362e gtsam::D │ │ │ │ +00030490: 6563 6973 696f 6e54 7265 653a 3a63 6f6e ecisionTree::con │ │ │ │ +000304a0: 7665 7274 4672 6f6d 3c2f 613e 3c2f 6469 vertFrom
    NodePtr co │ │ │ │ +000304d0: 6e76 6572 7446 726f 6d28 636f 6e73 7420 nvertFrom(const │ │ │ │ +000304e0: 7479 7065 6e61 6d65 2044 6563 6973 696f typename Decisio │ │ │ │ +000304f0: 6e54 7265 6526 6c74 3b20 4d2c 2058 2026 nTree< M, X & │ │ │ │ +00030500: 6774 3b3a 3a4e 6f64 6550 7472 2026 616d gt;::NodePtr &am │ │ │ │ +00030510: 703b 662c 2073 7464 3a3a 6675 6e63 7469 p;f, std::functi │ │ │ │ +00030520: 6f6e 266c 743b 204c 2863 6f6e 7374 204d on< L(const M │ │ │ │ +00030530: 2026 616d 703b 2926 6774 3b20 4c5f 6f66 &)> L_of │ │ │ │ +00030540: 5f4d 2c20 7374 643a 3a66 756e 6374 696f _M, std::functio │ │ │ │ +00030550: 6e26 6c74 3b20 5928 636f 6e73 7420 5820 n< Y(const X │ │ │ │ +00030560: 2661 6d70 3b29 2667 743b 2059 5f6f 665f &)> Y_of_ │ │ │ │ +00030570: 5829 2063 6f6e 7374 3c2f 6469 763e 3c64 X) const
    Convert from a │ │ │ │ +000305a0: 4465 6369 7369 6f6e 5472 6565 266c 743b DecisionTree< │ │ │ │ +000305b0: 4d2c 2058 2667 743b 2074 6f20 4465 6369 M, X> to Deci │ │ │ │ +000305c0: 7369 6f6e 5472 6565 266c 743b 4c2c 2059 sionTree<L, Y │ │ │ │ +000305d0: 2667 743b 2e3c 2f64 6976 3e3c 6469 7620 >.
    Definition │ │ │ │ +00030600: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +00030610: 2e68 3a36 3731 3c2f 6469 763e 3c2f 6469 .h:671
    ..
    │ │ │ │ +000308b0: 7479 7065 6e61 6d65 204e 6f64 653a 3a50 typename Node::P │ │ │ │ +000308c0: 7472 204e 6f64 6550 7472 3c2f 6469 763e tr NodePtr
    │ │ │ │ +000308d0: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    ------------- │ │ │ │ +000308f0: 2d2d 2d2d 2d2d 2d2d e280 9420 4e6f 6465 --------... Node │ │ │ │ +00030900: 2062 6173 6520 636c 6173 7320 2d2d 2d2d base class ---- │ │ │ │ +00030910: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00030920: 2d2d 2d2d e280 943c 2f64 6976 3e3c 6469 ----...
    │ │ │ │ +00030940: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree.h │ │ │ │ +00030960: 3a31 3433 3c2f 6469 763e 3c2f 6469 763e :143
    │ │ │ │ +00030970: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    std:: │ │ │ │ +00030a40: 7365 7426 6c74 3b20 4c20 2667 743b 206c set< L > l │ │ │ │ +00030a50: 6162 656c 7328 2920 636f 6e73 743c 2f64 abels() const
    Retrieve a │ │ │ │ +00030a80: 6c6c 2075 6e69 7175 6520 6c61 6265 6c73 ll unique labels │ │ │ │ +00030a90: 2061 7320 6120 7365 742e 3c2f 6469 763e as a set.
    │ │ │ │ +00030aa0: 3c64 6976 2063 6c61 7373 3d22 7474 6465
    Definition │ │ │ │ +00030ac0: 3c2f 623e 2044 6563 6973 696f 6e54 7265 DecisionTre │ │ │ │ +00030ad0: 652d 696e 6c2e 683a 3835 333c 2f64 6976 e-inl.h:853
    .bool empty() co │ │ │ │ +00030bc0: 6e73 743c 2f64 6976 3e3c 6469 7620 636c nst
    Chec │ │ │ │ +00030be0: 6b20 6966 2074 7265 6520 6973 2065 6d70 k if tree is emp │ │ │ │ +00030bf0: 7479 2e3c 2f64 6976 3e3c 6469 7620 636c ty.
    D │ │ │ │ +00030c10: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +00030c20: 6369 7369 6f6e 5472 6565 2e68 3a32 3537 cisionTree.h:257 │ │ │ │ +00030c30: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ +00030cd0: 7361 6d3a 3a44 6563 6973 696f 6e54 7265 sam::DecisionTre │ │ │ │ +00030ce0: 653a 3a76 6973 6974 3c2f 613e 3c2f 6469 e::visit
    void visit │ │ │ │ +00030d10: 2846 756e 6320 6629 2063 6f6e 7374 3c2f (Func f) const
    Visit all │ │ │ │ +00030d40: 206c 6561 7665 7320 696e 2064 6570 7468 leaves in depth │ │ │ │ +00030d50: 2d66 6972 7374 2066 6173 6869 6f6e 2e3c -first fashion.< │ │ │ │ +00030d60: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ +00030d80: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ +00030d90: 6f6e 5472 6565 2d69 6e6c 2e68 3a37 3336 onTree-inl.h:736 │ │ │ │ +00030da0: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    void v │ │ │ │ +00030e80: 6973 6974 4c65 6166 2846 756e 6320 6629 isitLeaf(Func f) │ │ │ │ +00030e90: 2063 6f6e 7374 3c2f 6469 763e 3c64 6976 const
    V │ │ │ │ +00030eb0: 6973 6974 2061 6c6c 206c 6561 7665 7320 isit all leaves │ │ │ │ +00030ec0: 696e 2064 6570 7468 2d66 6972 7374 2066 in depth-first f │ │ │ │ +00030ed0: 6173 6869 6f6e 2e3c 2f64 6976 3e3c 6469 ashion.
    │ │ │ │ +00030ef0: 3c62 3e44 6566 696e 6974 696f 6e3c 2f62 Definition DecisionTree-i │ │ │ │ +00030f10: 6e6c 2e68 3a37 3733 3c2f 6469 763e 3c2f nl.h:773
    .
    st │ │ │ │ +00030ff0: 643a 3a66 756e 6374 696f 6e26 6c74 3b20 d::function< │ │ │ │ +00031000: 5928 636f 6e73 7420 5920 2661 6d70 3b29 Y(const Y &) │ │ │ │ +00031010: 2667 743b 2055 6e61 7279 3c2f 6469 763e > Unary
    │ │ │ │ +00031020: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    Handy typedef │ │ │ │ +00031040: 7320 666f 7220 756e 6172 7920 616e 6420 s for unary and │ │ │ │ +00031050: 6269 6e61 7279 2066 756e 6374 696f 6e20 binary function │ │ │ │ +00031060: 7479 7065 732e 3c2f 6469 763e 3c64 6976 types.
    < │ │ │ │ +00031080: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +00031090: 2044 6563 6973 696f 6e54 7265 652e 683a DecisionTree.h: │ │ │ │ +000310a0: 3734 3c2f 6469 763e 3c2f 6469 763e 0a3c 74
    .< │ │ │ │ +000310b0: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +000310c0: 6964 3d22 6161 3032 3739 365f 6874 6d6c id="aa02796_html │ │ │ │ +000310d0: 5f61 3733 6361 3535 3833 6163 3264 6262 _a73ca5583ac2dbb │ │ │ │ +000310e0: 3236 6163 6366 6335 3464 6536 3961 3963 26accfc54de69a9c │ │ │ │ +000310f0: 3061 223e 3c64 6976 2063 6c61 7373 3d22 0a">
    │ │ │ │ +00031140: 6774 7361 6d3a 3a44 6563 6973 696f 6e54 gtsam::DecisionT │ │ │ │ +00031150: 7265 653a 3a66 6f6c 643c 2f61 3e3c 2f64 ree::fold
    X fold(Fu │ │ │ │ +00031180: 6e63 2066 2c20 5820 7830 2920 636f 6e73 nc f, X x0) cons │ │ │ │ +00031190: 743c 2f64 6976 3e3c 6469 7620 636c 6173 t
    Fold a │ │ │ │ +000311b0: 2062 696e 6172 7920 6675 6e63 7469 6f6e binary function │ │ │ │ +000311c0: 206f 7665 7220 7468 6520 7472 6565 2c20 over the tree, │ │ │ │ +000311d0: 7265 7475 726e 696e 6720 6163 6375 6d75 returning accumu │ │ │ │ +000311e0: 6c61 746f 722e 3c2f 6469 763e 3c64 6976 lator.
    < │ │ │ │ +00031200: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +00031210: 2044 6563 6973 696f 6e54 7265 652d 696e DecisionTree-in │ │ │ │ +00031220: 6c2e 683a 3833 333c 2f64 6976 3e3c 2f64 l.h:833
    ..
    │ │ │ │ +00031500: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    GTSAM-style p │ │ │ │ +00031520: 7269 6e74 2e3c 2f64 6976 3e3c 6469 7620 rint.
    Definition │ │ │ │ +00031550: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +00031560: 2e68 3a38 3732 3c2f 6469 763e 3c2f 6469 .h:872
    .
    De │ │ │ │ +00031640: 6369 7369 6f6e 5472 6565 2063 6f6d 6269 cisionTree combi │ │ │ │ +00031650: 6e65 2863 6f6e 7374 204c 2026 616d 703b ne(const L & │ │ │ │ +00031660: 6c61 6265 6c2c 2073 697a 655f 7420 6361 label, size_t ca │ │ │ │ +00031670: 7264 696e 616c 6974 792c 2063 6f6e 7374 rdinality, const │ │ │ │ +00031680: 2042 696e 6172 7920 2661 6d70 3b6f 7029 Binary &op) │ │ │ │ +00031690: 2063 6f6e 7374 3c2f 6469 763e 3c64 6976 const
    c │ │ │ │ +000316b0: 6f6d 6269 6e65 2073 7562 7472 6565 7320 ombine subtrees │ │ │ │ +000316c0: 6f6e 206b 6579 2077 6974 6820 6269 6e61 on key with bina │ │ │ │ +000316d0: 7279 206f 7065 7261 7469 6f6e 2026 7175 ry operation &qu │ │ │ │ +000316e0: 6f74 3b6f 7026 7175 6f74 3b3c 2f64 6976 ot;op"
    Definitio │ │ │ │ +00031710: 6e3c 2f62 3e20 4465 6369 7369 6f6e 5472 n DecisionTr │ │ │ │ +00031720: 6565 2d69 6e6c 2e68 3a39 3337 3c2f 6469 ee-inl.h:937
    .
    gtsam: │ │ │ │ +000317d0: 3a44 6563 6973 696f 6e54 7265 653a 3a76 :DecisionTree::v │ │ │ │ +000317e0: 6973 6974 5769 7468 3c2f 613e 3c2f 6469 isitWith
    void visit │ │ │ │ +00031810: 5769 7468 2846 756e 6320 6629 2063 6f6e With(Func f) con │ │ │ │ +00031820: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Visit │ │ │ │ +00031840: 2061 6c6c 206c 6561 7665 7320 696e 2064 all leaves in d │ │ │ │ +00031850: 6570 7468 2d66 6972 7374 2066 6173 6869 epth-first fashi │ │ │ │ +00031860: 6f6e 2e3c 2f64 6976 3e3c 6469 7620 636c on.
    D │ │ │ │ +00031880: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +00031890: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ +000318a0: 3a38 3136 3c2f 6469 763e 3c2f 6469 763e :816
    │ │ │ │ +000318b0: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    c │ │ │ │ +00031980: 6f6e 7374 2059 2026 616d 703b 206f 7065 onst Y & ope │ │ │ │ +00031990: 7261 746f 7228 2928 636f 6e73 7420 4173 rator()(const As │ │ │ │ +000319a0: 7369 676e 6d65 6e74 266c 743b 204c 2026 signment< L & │ │ │ │ +000319b0: 6774 3b20 2661 6d70 3b78 2920 636f 6e73 gt; &x) cons │ │ │ │ +000319c0: 743c 2f64 6976 3e3c 6469 7620 636c 6173 t
    evalua │ │ │ │ +000319e0: 7465 3c2f 6469 763e 3c64 6976 2063 6c61 te
    De │ │ │ │ +00031a00: 6669 6e69 7469 6f6e 3c2f 623e 2044 6563 finition Dec │ │ │ │ +00031a10: 6973 696f 6e54 7265 652d 696e 6c2e 683a isionTree-inl.h: │ │ │ │ +00031a20: 3838 343c 2f64 6976 3e3c 2f64 6976 3e0a 884
    . │ │ │ │ +00031a30: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    gtsam::Decision │ │ │ │ +00031ad0: 5472 6565 3a3a 646f 743c 2f61 3e3c 2f64 Tree::dot
    void dot( │ │ │ │ +00031b00: 7374 643a 3a6f 7374 7265 616d 2026 616d std::ostream &am │ │ │ │ +00031b10: 703b 6f73 2c20 636f 6e73 7420 4c61 6265 p;os, const Labe │ │ │ │ +00031b20: 6c46 6f72 6d61 7474 6572 2026 616d 703b lFormatter & │ │ │ │ +00031b30: 6c61 6265 6c46 6f72 6d61 7474 6572 2c20 labelFormatter, │ │ │ │ +00031b40: 636f 6e73 7420 5661 6c75 6546 6f72 6d61 const ValueForma │ │ │ │ +00031b50: 7474 6572 2026 616d 703b 7661 6c75 6546 tter &valueF │ │ │ │ +00031b60: 6f72 6d61 7474 6572 2c20 626f 6f6c 2073 ormatter, bool s │ │ │ │ +00031b70: 686f 775a 6572 6f3d 7472 7565 2920 636f howZero=true) co │ │ │ │ +00031b80: 6e73 743c 2f64 6976 3e3c 6469 7620 636c nst
    outp │ │ │ │ +00031ba0: 7574 2074 6f20 6772 6170 6876 697a 2066 ut to graphviz f │ │ │ │ +00031bb0: 6f72 6d61 742c 2073 7472 6561 6d20 7665 ormat, stream ve │ │ │ │ +00031bc0: 7273 696f 6e3c 2f64 6976 3e3c 6469 7620 rsion
    Definition │ │ │ │ +00031bf0: 4465 6369 7369 6f6e 5472 6565 2d69 6e6c DecisionTree-inl │ │ │ │ +00031c00: 2e68 3a39 3439 3c2f 6469 763e 3c2f 6469 .h:949
    .
    . │ │ │ │ +00031d80: 3c64 6976 2063 6c61 7373 3d22 7474 6322
    bo │ │ │ │ +00031e50: 6f6c 206f 7065 7261 746f 723d 3d28 636f ol operator==(co │ │ │ │ +00031e60: 6e73 7420 4465 6369 7369 6f6e 5472 6565 nst DecisionTree │ │ │ │ +00031e70: 2026 616d 703b 7129 2063 6f6e 7374 3c2f &q) const
    equality< │ │ │ │ +00031ea0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ +00031ec0: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ +00031ed0: 6f6e 5472 6565 2d69 6e6c 2e68 3a38 3739 onTree-inl.h:879 │ │ │ │ +00031ee0: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    .
    gt │ │ │ │ +00031f80: 7361 6d3a 3a44 6563 6973 696f 6e54 7265 sam::DecisionTre │ │ │ │ +00031f90: 653a 3a4c 6162 656c 433c 2f61 3e3c 2f64 e::LabelC
    std::pair │ │ │ │ +00031fc0: 266c 743b 204c 2c20 7369 7a65 5f74 2026 < L, size_t & │ │ │ │ +00031fd0: 6774 3b20 4c61 6265 6c43 3c2f 6469 763e gt; LabelC
    │ │ │ │ +00031fe0: 3c64 6976 2063 6c61 7373 3d22 7474 646f
    A label annot │ │ │ │ +00032000: 6174 6564 2077 6974 6820 6361 7264 696e ated with cardin │ │ │ │ +00032010: 616c 6974 792e 3c2f 6469 763e 3c64 6976 ality.
    < │ │ │ │ +00032030: 623e 4465 6669 6e69 7469 6f6e 3c2f 623e b>Definition │ │ │ │ +00032040: 2044 6563 6973 696f 6e54 7265 652e 683a DecisionTree.h: │ │ │ │ +00032050: 3739 3c2f 6469 763e 3c2f 6469 763e 0a3c 79
    .< │ │ │ │ +00032060: 6469 7620 636c 6173 733d 2274 7463 2220 div class="ttc" │ │ │ │ +00032070: 6964 3d22 6161 3032 3739 365f 6874 6d6c id="aa02796_html │ │ │ │ +00032080: 5f61 6538 3065 3635 6139 3066 6539 6163 _ae80e65a90fe9ac │ │ │ │ +00032090: 3430 6235 6335 6134 6430 3764 6662 6264 40b5c5a4d07dfbbd │ │ │ │ +000320a0: 6137 223e 3c64 6976 2063 6c61 7373 3d22 a7">
    size_ │ │ │ │ +00032130: 7420 6e72 4c65 6176 6573 2829 2063 6f6e t nrLeaves() con │ │ │ │ +00032140: 7374 3c2f 6469 763e 3c64 6976 2063 6c61 st
    Retur │ │ │ │ +00032160: 6e20 7468 6520 6e75 6d62 6572 206f 6620 n the number of │ │ │ │ +00032170: 6c65 6176 6573 2069 6e20 7468 6520 7472 leaves in the tr │ │ │ │ +00032180: 6565 2e3c 2f64 6976 3e3c 6469 7620 636c ee.
    D │ │ │ │ +000321a0: 6566 696e 6974 696f 6e3c 2f62 3e20 4465 efinition De │ │ │ │ +000321b0: 6369 7369 6f6e 5472 6565 2d69 6e6c 2e68 cisionTree-inl.h │ │ │ │ +000321c0: 3a38 3233 3c2f 6469 763e 3c2f 6469 763e :823
    │ │ │ │ +000321d0: 0a3c 6469 7620 636c 6173 733d 2274 7463 .
    DecisionTree()< │ │ │ │ +000322b0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Default │ │ │ │ +000322d0: 636f 6e73 7472 7563 746f 7220 2866 6f72 constructor (for │ │ │ │ +000322e0: 2073 6572 6961 6c69 7a61 7469 6f6e 293c serialization)< │ │ │ │ +000322f0: 2f64 6976 3e3c 6469 7620 636c 6173 733d /div>
    Defin │ │ │ │ +00032310: 6974 696f 6e3c 2f62 3e20 4465 6369 7369 ition Decisi │ │ │ │ +00032320: 6f6e 5472 6565 2d69 6e6c 2e68 3a34 3632 onTree-inl.h:462 │ │ │ │ +00032330: 3c2f 6469 763e 3c2f 6469 763e 0a3c 6469
    . │ │ │ │ +00032360: 3c64 6976 2063 6c61 7373 3d22 7474 6e61 │ │ │ │ +000323c0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +000323d0: 2d2d 2d2d 2de2 8094 204e 6f64 6520 6261 -----... Node ba │ │ │ │ +000323e0: 7365 2063 6c61 7373 202d 2d2d 2d2d 2d2d se class ------- │ │ │ │ +000323f0: 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d 2d2d ---------------- │ │ │ │ +00032400: 2de2 8094 3c2f 6469 763e 3c64 6976 2063 -...
    │ │ │ │ +00032420: 4465 6669 6e69 7469 6f6e 3c2f 623e 2044 Definition D │ │ │ │ +00032430: 6563 6973 696f 6e54 7265 652e 683a 3836 ecisionTree.h:86 │ │ │ │ +00032440: 3c2f 6469 763e 3c2f 6469 763e 0a3c 2f64
    .
    .. │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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::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
    │ │ │ │ +
    Date
    Feb 15, 2011
    │ │ │ │
    Author
    Duy-Nguyen Ta
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +DiscreteBayesNet.cpp File Reference │ │ │ │ │ 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 ********** │ │ │ │ │ Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ + 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_F_a_c_t_o_r_._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/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/DiscreteBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DiscreteMarginals.h File Reference
    │ │ │ │ +
    DiscreteBayesTree.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ 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

    │ │ │ │ -

    A class for computing marginals in a DiscreteFactorGraph.

    │ │ │ │ -
    Author
    Abhijit Kundu
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +

    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

    │ │ │ │ +

    DiscreteBayesTree

    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    June 4, 2012
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -A class for computing marginals in a DiscreteFactorGraph. │ │ │ │ │ +Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ +DiscreteBayesTree │ │ │ │ │ Author │ │ │ │ │ - Abhijit Kundu │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - June 4, 2012 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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_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/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/DecisionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,73 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Assignment.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    DecisionTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Decision Tree for use in DiscreteFactors. │ │ │ │ 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::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

    │ │ │ │ -

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

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Feb 5, 2012
    │ │ │ │ +

    Decision Tree for use in DiscreteFactors.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Can Erdogan
    │ │ │ │ +
    Date
    Jan 30, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -Assignment.h File Reference │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) _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 │ │ │ │ │ +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_:_:_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_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 ********** │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Can Erdogan │ │ │ │ │ Date │ │ │ │ │ - Feb 5, 2012 │ │ │ │ │ + Jan 30, 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_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/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/DecisionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,110 +98,333 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Assignment.h
    │ │ │ │ +
    DecisionTree.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 <functional>
    │ │ │ │ -
    22#include <iostream>
    │ │ │ │ -
    23#include <map>
    │ │ │ │ -
    24#include <sstream>
    │ │ │ │ -
    25#include <utility>
    │ │ │ │ -
    26#include <vector>
    │ │ │ │ -
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ -
    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 }
    │ │ │ │ -
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    110}; // Assignment
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/base/Testable.h>
    │ │ │ │ +
    23#include <gtsam/base/types.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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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#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} // namespace gtsam
    │ │ │ │ -
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    136 template <class ARCHIVE>
    │ │ │ │ +
    137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {}
    │ │ │ │ +
    138 };
    │ │ │ │ +
    │ │ │ │ +
    141 public:
    │ │ │ │ +
    143 using NodePtr = typename Node::Ptr;
    │ │ │ │ +
    144
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    414 const typename DecisionTree<L, Y>::Unary& op) {
    │ │ │ │ +
    415 return f.apply(op);
    │ │ │ │ +
    416 }
    │ │ │ │ +
    │ │ │ │ +
    417
    │ │ │ │ +
    419 template<typename L, typename Y>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    421 const typename DecisionTree<L, Y>::UnaryAssignment& op) {
    │ │ │ │ +
    422 return f.apply(op);
    │ │ │ │ +
    423 }
    │ │ │ │ +
    │ │ │ │ +
    424
    │ │ │ │ +
    426 template<typename L, typename Y>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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; }),
    │ │ │ │ + │ │ │ │ +
    445 [](std::pair<T1, T2> i) { return i.second; }));
    │ │ │ │ +
    446 }
    │ │ │ │ +
    │ │ │ │ +
    447
    │ │ │ │ +
    448} // namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    An assignment from labels to a discrete value index (size_t)
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │ +
    DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
    free versions of apply
    Definition DecisionTree.h:413
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    An assignment from labels to value index (size_t).
    Definition Assignment.h:37
    │ │ │ │ -
    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
    │ │ │ │ +
    Definition DecisionTree-inl.h:52
    │ │ │ │ +
    Definition DecisionTree-inl.h:172
    │ │ │ │ +
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ +
    DecisionTree apply(const Unary &op) const
    apply Unary operation "op" to f
    Definition DecisionTree-inl.h:889
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    DecisionTree combine(const LabelC &labelC, const Binary &op) const
    combine with LabelC for convenience
    Definition DecisionTree.h:368
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual ~DecisionTree()=default
    Make virtual.
    │ │ │ │ +
    static bool DefaultCompare(const Y &a, const Y &b)
    Default method for comparison of two objects of type Y.
    Definition DecisionTree.h:64
    │ │ │ │ +
    typename Node::Ptr NodePtr
    ---------------------— Node base class ------------------------—
    Definition DecisionTree.h:143
    │ │ │ │ +
    std::set< L > labels() const
    Retrieve all unique labels as a set.
    Definition DecisionTree-inl.h:853
    │ │ │ │ +
    bool empty() const
    Check if tree is empty.
    Definition DecisionTree.h:257
    │ │ │ │ +
    void visit(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:736
    │ │ │ │ +
    void visitLeaf(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:773
    │ │ │ │ +
    std::function< Y(const Y &)> Unary
    Handy typedefs for unary and binary function types.
    Definition DecisionTree.h:74
    │ │ │ │ +
    X fold(Func f, X x0) const
    Fold a binary function over the tree, returning accumulator.
    Definition DecisionTree-inl.h:833
    │ │ │ │ +
    NodePtr root_
    A DecisionTree just contains the root. TODO(dellaert): make protected.
    Definition DecisionTree.h:146
    │ │ │ │ +
    void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
    GTSAM-style print.
    Definition DecisionTree-inl.h:872
    │ │ │ │ +
    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
    │ │ │ │ +
    void visitWith(Func f) const
    Visit all leaves in depth-first fashion.
    Definition DecisionTree-inl.h:816
    │ │ │ │ +
    const Y & operator()(const Assignment< L > &x) const
    evaluate
    Definition DecisionTree-inl.h:884
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition DecisionTree.h:399
    │ │ │ │ +
    bool operator==(const DecisionTree &q) const
    equality
    Definition DecisionTree-inl.h:879
    │ │ │ │ +
    std::pair< L, size_t > LabelC
    A label annotated with cardinality.
    Definition DecisionTree.h:79
    │ │ │ │ +
    size_t nrLeaves() const
    Return the number of leaves in the tree.
    Definition DecisionTree-inl.h:823
    │ │ │ │ +
    DecisionTree()
    Default constructor (for serialization)
    Definition DecisionTree-inl.h:462
    │ │ │ │ +
    ---------------------— Node base class ------------------------—
    Definition DecisionTree.h:86
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition DecisionTree.h:135
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,111 +1,417 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Assignment.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) │ │ │ │ │ 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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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#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 return allPossValues; │ │ │ │ │ -109 } │ │ │ │ │ -110}; // Assignment │ │ │ │ │ +108 │ │ │ │ │ +109 // Unique ID for dot files │ │ │ │ │ +110 const void* id() const { return this; } │ │ │ │ │ 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 │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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_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_i_s_c_r_e_t_e │ │ │ │ │ - * _A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ + * _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/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/Cyclic.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3_S2Stereo.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    Cyclic.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    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_S2Stereo &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

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

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

    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_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 │ │ │ │ │ +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_S_t_e_r_e_o &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ +Cyclic group, i.e., the integers modulo N. │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Rot3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    SOn-inl.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Rot3.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ 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)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Template implementations for SO(n)

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

    Rotation, common code between Rotation matrix and Quaternion.

    │ │ │ │ +
    Author
    Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,34 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ -_G_o_ _t_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 | _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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Template implementations for SO(n) │ │ │ │ │ +Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ │ Author │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_-_i_n_l_._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/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/Cal3DS2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Cal3_S2.h File Reference
    │ │ │ │ +
    Cal3DS2.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Cal3_S2
     The most common 5DOF 3D->2D calibration. More...
    class  gtsam::Cal3DS2
     Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimization. More...
     
    struct  gtsam::traits< Cal3_S2 >
    struct  gtsam::traits< Cal3DS2 >
     
    struct  gtsam::traits< const Cal3_S2 >
    struct  gtsam::traits< const Cal3DS2 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

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

    │ │ │ │ -
    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,29 +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 │ │ │ │ │ -Cal3_S2.h File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ -  The most common 5DOF 3D->2D calibration. _M_o_r_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___S_2_ _> │ │ │ │ │ +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___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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +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 │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ydjian @autho Varun Agrawal │ │ │ │ │ + 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___S_2_._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/a00284.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00284 = [ │ │ │ │ │ - ["gtsam::traits< Cal3_S2 >", "a02948.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3_S2 >", "a02952.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3DS2 >", "a02984.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3DS2 >", "a02988.html", 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/Cal3DS2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,142 +98,113 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3_S2.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ -
    24#include <gtsam/geometry/Cal3.h>
    │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ -
    34class GTSAM_EXPORT Cal3_S2 : public Cal3 {
    │ │ │ │ -
    35 public:
    │ │ │ │ -
    36 enum { dimension = 5 };
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/geometry/Cal3DS2_Base.h>
    │ │ │ │ +
    24#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    │ │ │ │ +
    35class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
    │ │ │ │ +
    36 using Base = Cal3DS2_Base;
    │ │ │ │
    37
    │ │ │ │ -
    39 using shared_ptr = boost::shared_ptr<Cal3_S2>;
    │ │ │ │ +
    38 public:
    │ │ │ │ +
    39 enum { dimension = 9 };
    │ │ │ │
    40
    │ │ │ │ +
    42 using shared_ptr = boost::shared_ptr<Cal3DS2>;
    │ │ │ │
    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) {}
    │ │ │ │ -
    │ │ │ │ -
    50
    │ │ │ │ -
    52 Cal3_S2(const Vector5& d) : Cal3(d) {}
    │ │ │ │ +
    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
    │ │ │ │ -
    60 Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
    │ │ │ │ +
    54 ~Cal3DS2() override {}
    │ │ │ │ +
    55
    │ │ │ │ +
    59
    │ │ │ │ +
    60 Cal3DS2(const Vector9& v) : Base(v) {}
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    Common code for all Calibration models.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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}
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ -
    Vector5 vector() const
    vectorized form (column-wise)
    Definition Cal3.h:160
    │ │ │ │ -
    double fy_
    focal length
    Definition Cal3.h:71
    │ │ │ │ -
    double s_
    skew
    Definition Cal3.h:72
    │ │ │ │ -
    double v0_
    principal point
    Definition Cal3.h:73
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ -
    Cal3_S2()=default
    Create a default calibration that leaves coordinates unchanged.
    │ │ │ │ -
    Cal3_S2(double fx, double fy, double s, double u0, double v0)
    constructor from doubles
    Definition Cal3_S2.h:48
    │ │ │ │ -
    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
    │ │ │ │ -
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3_S2.h:118
    │ │ │ │ -
    Cal3_S2(const Vector5 &d)
    constructor from vector
    Definition Cal3_S2.h:52
    │ │ │ │ -
    Vector5 localCoordinates(const Cal3_S2 &T2) const
    Unretraction for the calibration.
    Definition Cal3_S2.h:126
    │ │ │ │ -
    Cal3_S2(double fov, int w, int h)
    Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect.
    Definition Cal3_S2.h:60
    │ │ │ │ -
    Cal3_S2 retract(const Vector &d) const
    Given 5-dim tangent vector, create new calibration.
    Definition Cal3_S2.h:121
    │ │ │ │ +
    Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
    Definition Cal3DS2.h:35
    │ │ │ │ +
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3DS2.h:87
    │ │ │ │ +
    boost::shared_ptr< Base > clone() const override
    Definition Cal3DS2.h:97
    │ │ │ │ +
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3DS2.h:90
    │ │ │ │ +
    Cal3DS2()=default
    Default Constructor with only unit focal length.
    │ │ │ │ +
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,126 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3_S2.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 │ │ │ │ │ -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 │ │ │ │ │ -_3_4class GTSAM_EXPORT _C_a_l_3___S_2 : public _C_a_l_3 { │ │ │ │ │ -35 public: │ │ │ │ │ -36 enum { dimension = 5 }; │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +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; │ │ │ │ │ 37 │ │ │ │ │ -39 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +38 public: │ │ │ │ │ +39 enum { dimension = 9 }; │ │ │ │ │ 40 │ │ │ │ │ +42 using shared_ptr = boost::shared_ptr; │ │ │ │ │ 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) {} │ │ │ │ │ -50 │ │ │ │ │ -_5_2 _C_a_l_3___S_2(const Vector5& d) : _C_a_l_3(d) {} │ │ │ │ │ +_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 │ │ │ │ │ -_6_0 _C_a_l_3___S_2(double fov, int w, int h) : _C_a_l_3(fov, w, h) {} │ │ │ │ │ +54 _~_C_a_l_3_D_S_2() override {} │ │ │ │ │ +55 │ │ │ │ │ +59 │ │ │ │ │ +60 Cal3DS2(const Vector9& v) : Base(v) {} │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -_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 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_:_:_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _C_a_l_3___S_2_._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.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/CameraSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,56 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs
    │ │ │ │ -
    Quaternion.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    CameraSet.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::traits< QUATERNION_TYPE >
    class  gtsam::CameraSet< CAMERA >
     A set of cameras, all with their own calibration. More...
     
    struct  gtsam::traits< CameraSet< CAMERA > >
     
    struct  gtsam::traits< const CameraSet< CAMERA > >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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.

    │ │ │ │ +

    Base class to create smart factors on poses or cameras.

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Feb 19, 2015
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 | _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 │ │ │ │ │ +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 │ │ │ │ │ -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_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_:_:_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_<_ _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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 19, 2015 │ │ │ │ │ * _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_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/a00293.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00293 = [ │ │ │ │ │ - ["gtsam::traits< QUATERNION_TYPE >", "a03208.html", null] │ │ │ │ │ + ["gtsam::CameraSet< CAMERA >", "a03044.html", "a03044"], │ │ │ │ │ + ["gtsam::traits< CameraSet< CAMERA > >", "a03048.html", null], │ │ │ │ │ + ["gtsam::traits< const CameraSet< CAMERA > >", "a03052.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/CameraSet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,200 +98,494 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Quaternion.h
    │ │ │ │ +
    CameraSet.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>
    │ │ │ │ -
    25
    │ │ │ │ -
    26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/base/FastMap.h>
    │ │ │ │ + │ │ │ │ +
    23#include <gtsam/base/Testable.h>
    │ │ │ │ +
    24#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
    │ │ │ │ + │ │ │ │ +
    26#include <gtsam/inference/Key.h>
    │ │ │ │
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ +
    28#include <vector>
    │ │ │ │
    29
    │ │ │ │ -
    30// Define traits
    │ │ │ │ -
    31template<typename _Scalar, int _Options>
    │ │ │ │ -
    │ │ │ │ -
    32struct traits<QUATERNION_TYPE> {
    │ │ │ │ -
    33 typedef QUATERNION_TYPE ManifoldType;
    │ │ │ │ -
    34 typedef QUATERNION_TYPE Q;
    │ │ │ │ -
    35
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    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>>;
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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} // \namespace gtsam
    │ │ │ │ -
    185
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    3*3 matrix representation of SO(3)
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    449 template <class ARCHIVE>
    │ │ │ │ +
    450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    │ │ │ │ +
    451 ar&(*this);
    │ │ │ │ +
    452 }
    │ │ │ │ +
    453
    │ │ │ │ +
    454 public:
    │ │ │ │ + │ │ │ │ +
    456};
    │ │ │ │ +
    │ │ │ │ +
    457
    │ │ │ │ +
    458template <class CAMERA>
    │ │ │ │ +
    459const int CameraSet<CAMERA>::D;
    │ │ │ │ +
    460
    │ │ │ │ +
    461template <class CAMERA>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Calibrated camera for which only pose is unknown.
    │ │ │ │ +
    3D Point
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Group operator syntax flavors.
    Definition Group.h:37
    │ │ │ │ -
    tag to assert a type is a Lie group
    Definition Lie.h:164
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ -
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    void setDiagonalBlock(DenseIndex I, const XprType &xpr)
    Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
    Definition SymmetricBlockMatrix.h:195
    │ │ │ │ +
    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
    │ │ │ │ +
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition SymmetricBlockMatrix.h:114
    │ │ │ │ +
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
    Update an off diagonal block.
    Definition SymmetricBlockMatrix.h:228
    │ │ │ │ +
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual void print(const std::string &s="") const
    print
    Definition CameraSet.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual ~CameraSet()=default
    Destructor.
    │ │ │ │ +
    bool equals(const CameraSet &p, double tol=1e-9) const
    equals
    Definition CameraSet.h:91
    │ │ │ │ +
    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
    │ │ │ │ +
    static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
    Make a vector of re-projection errors.
    Definition CameraSet.h:51
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static const int ZDim
    Measurement dimension.
    Definition CameraSet.h:48
    │ │ │ │ +
    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
    │ │ │ │ +
    static const int D
    Camera dimension.
    Definition CameraSet.h:47
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition CameraSet.h:448
    │ │ │ │ +
    Eigen::Matrix< double, ZDim, D > MatrixZD
    Definitions for blocks of F.
    Definition CameraSet.h:77
    │ │ │ │ +
    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,214 +1,606 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Quaternion.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 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 │ │ │ │ │ -25 │ │ │ │ │ -26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │ +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 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ +28#include │ │ │ │ │ 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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ +_7_4 virtual _~_C_a_m_e_r_a_S_e_t() = default; │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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} // \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) │ │ │ │ │ +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 │ │ │ │ │ +_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_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _Q_u_a_t_e_r_n_i_o_n_._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/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/Cal3_S2Stereo.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    CameraSet.h File Reference
    │ │ │ │ +
    Cal3_S2Stereo.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ 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::Cal3_S2Stereo
     The most common 5DOF 3D->2D calibration, stereo version. More...
     
    struct  gtsam::traits< CameraSet< CAMERA > >
    struct  gtsam::traits< Cal3_S2Stereo >
     
    struct  gtsam::traits< const CameraSet< CAMERA > >
    struct  gtsam::traits< const Cal3_S2Stereo >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Base class to create smart factors on poses or cameras.

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

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

    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -CameraSet.h File Reference │ │ │ │ │ -Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ +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_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_:_:_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_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_<_ _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_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_<_ _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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 19, 2015 │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ + * _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/a00296.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,4 @@ │ │ │ │ │ var a00296 = [ │ │ │ │ │ - ["gtsam::CameraSet< CAMERA >", "a03044.html", "a03044"], │ │ │ │ │ - ["gtsam::traits< CameraSet< CAMERA > >", "a03048.html", null], │ │ │ │ │ - ["gtsam::traits< const CameraSet< CAMERA > >", "a03052.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3_S2Stereo >", "a02960.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3_S2Stereo >", "a02964.html", 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/Cal3_S2Stereo.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,494 +98,163 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    CameraSet.h
    │ │ │ │ +
    Cal3_S2Stereo.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>
    │ │ │ │ - │ │ │ │ -
    23#include <gtsam/base/Testable.h>
    │ │ │ │ -
    24#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
    │ │ │ │ - │ │ │ │ -
    26#include <gtsam/inference/Key.h>
    │ │ │ │ -
    27
    │ │ │ │ -
    28#include <vector>
    │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    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>>;
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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) {}
    │ │ │ │ +
    │ │ │ │
    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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    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>>;
    │ │ │ │ +
    77 Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
    │ │ │ │ +
    78 OptionalJacobian<2, 2> Dp = boost::none) const;
    │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    85 Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
    │ │ │ │ +
    86
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    449 template <class ARCHIVE>
    │ │ │ │ -
    450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    │ │ │ │ -
    451 ar&(*this);
    │ │ │ │ -
    452 }
    │ │ │ │ -
    453
    │ │ │ │ -
    454 public:
    │ │ │ │ - │ │ │ │ -
    456};
    │ │ │ │ -
    │ │ │ │ -
    457
    │ │ │ │ -
    458template <class CAMERA>
    │ │ │ │ -
    459const int CameraSet<CAMERA>::D;
    │ │ │ │ -
    460
    │ │ │ │ -
    461template <class CAMERA>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    Calibrated camera for which only pose is unknown.
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ -
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    void setDiagonalBlock(DenseIndex I, const XprType &xpr)
    Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
    Definition SymmetricBlockMatrix.h:195
    │ │ │ │ -
    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
    │ │ │ │ -
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition SymmetricBlockMatrix.h:114
    │ │ │ │ -
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
    Update an off diagonal block.
    Definition SymmetricBlockMatrix.h:228
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual void print(const std::string &s="") const
    print
    Definition CameraSet.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual ~CameraSet()=default
    Destructor.
    │ │ │ │ -
    bool equals(const CameraSet &p, double tol=1e-9) const
    equals
    Definition CameraSet.h:91
    │ │ │ │ -
    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
    │ │ │ │ -
    static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
    Make a vector of re-projection errors.
    Definition CameraSet.h:51
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static const int ZDim
    Measurement dimension.
    Definition CameraSet.h:48
    │ │ │ │ -
    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
    │ │ │ │ -
    static const int D
    Camera dimension.
    Definition CameraSet.h:47
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition CameraSet.h:448
    │ │ │ │ -
    Eigen::Matrix< double, ZDim, D > MatrixZD
    Definitions for blocks of F.
    Definition CameraSet.h:77
    │ │ │ │ -
    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
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │ +
    Vector6 vector() const
    vectorized form (column-wise)
    Definition Cal3_S2Stereo.h:114
    │ │ │ │ +
    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
    constructor from doubles
    Definition Cal3_S2Stereo.h:47
    │ │ │ │ +
    Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
    Unretraction for the calibration.
    Definition Cal3_S2Stereo.h:137
    │ │ │ │ +
    const Cal3_S2 & calibration() const
    return calibration, same for left and right
    Definition Cal3_S2Stereo.h:105
    │ │ │ │ +
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3_S2Stereo.h:125
    │ │ │ │ +
    Vector3 calibrate(const Vector3 &p) const
    Convert homogeneous image coordinates to intrinsic coordinates.
    Definition Cal3_S2Stereo.h:85
    │ │ │ │ +
    Cal3_S2Stereo retract(const Vector &d) const
    Given 6-dim tangent vector, create new calibration.
    Definition Cal3_S2Stereo.h:131
    │ │ │ │ +
    Matrix3 K() const override
    return calibration matrix K, same for left and right
    Definition Cal3_S2Stereo.h:108
    │ │ │ │ +
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3_S2Stereo.h:128
    │ │ │ │ +
    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
    │ │ │ │ +
    double baseline() const
    return baseline
    Definition Cal3_S2Stereo.h:111
    │ │ │ │ +
    Cal3_S2Stereo(const Vector6 &d)
    constructor from vector
    Definition Cal3_S2Stereo.h:51
    │ │ │ │ +
    Cal3_S2Stereo()=default
    default calibration leaves coordinates unchanged
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,606 +1,204 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CameraSet.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 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 { │ │ │ │ │ -31 │ │ │ │ │ -35template │ │ │ │ │ -_3_6class _C_a_m_e_r_a_S_e_t : public std::vector> { │ │ │ │ │ -37 protected: │ │ │ │ │ -38 using Base = std::vector>; │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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) {} │ │ │ │ │ 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 } │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ -70 public: │ │ │ │ │ -71 using Base::Base; // Inherit the vector constructors │ │ │ │ │ -72 │ │ │ │ │ -_7_4 virtual _~_C_a_m_e_r_a_S_e_t() = default; │ │ │ │ │ -75 │ │ │ │ │ -_7_7 using _M_a_t_r_i_x_Z_D = Eigen::Matrix; │ │ │ │ │ -78 using FBlocks = std::vector>; │ │ │ │ │ +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 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 } │ │ │ │ │ +_8_5 Vector3 _c_a_l_i_b_r_a_t_e(const Vector3& p) const { return Cal3_S2::calibrate(p); } │ │ │ │ │ +86 │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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(_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. │ │ │ │ │ -_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. │ │ │ │ │ -_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 │ │ │ │ │ +_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 │ │ │ │ │ +_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_:_:_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_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_:_:_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_:_:_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___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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ + * _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/a00299.html │ │ │ │ @@ -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/geometry/PinholeSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    SimpleCamera.h File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    PinholeSet.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ 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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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
    │ │ │ │ +

    A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera.

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,30 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_. │ │ │ │ │ -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. │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ -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 > │ │ │ │ │ +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_ _>_ _> │ │ │ │ │   │ │ │ │ │ -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 > │ │ │ │ │ +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_ _>_ _> │ │ │ │ │   │ │ │ │ │ -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 > │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ - Date │ │ │ │ │ - Aug 16, 2009 │ │ │ │ │ +A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00299.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00299 = [ │ │ │ │ │ - ["PinholeCameraCal3_S2", "a00299.html#a3a2e8c622e65623a6853c84073bdb4f2", null] │ │ │ │ │ + ["gtsam::PinholeSet< CAMERA >", "a03136.html", "a03136"], │ │ │ │ │ + ["gtsam::traits< PinholeSet< CAMERA > >", "a03140.html", null], │ │ │ │ │ + ["gtsam::traits< const PinholeSet< CAMERA > >", "a03144.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00299_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SimpleCamera.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,141 +98,117 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SimpleCamera.h
    │ │ │ │ +
    PinholeSet.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <boost/optional.hpp>
    │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    29template<class CAMERA>
    │ │ │ │ +
    │ │ │ │ +
    30class PinholeSet: public CameraSet<CAMERA> {
    │ │ │ │
    31
    │ │ │ │ - │ │ │ │ -
    35 using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
    │ │ │ │ -
    36 using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
    │ │ │ │ -
    37 using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
    │ │ │ │ -
    38 using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
    │ │ │ │ +
    32private:
    │ │ │ │ +
    33 typedef CameraSet<CAMERA> Base;
    │ │ │ │ +
    34 typedef PinholeSet<CAMERA> This;
    │ │ │ │ +
    35
    │ │ │ │ +
    36protected:
    │ │ │ │ +
    37
    │ │ │ │ +
    38public:
    │ │ │ │
    39
    │ │ │ │ -
    40#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
    │ │ │ │ -
    45class GTSAM_EXPORT SimpleCamera : public PinholeCameraCal3_S2 {
    │ │ │ │ +
    │ │ │ │ +
    41 virtual ~PinholeSet() {
    │ │ │ │ +
    42 }
    │ │ │ │ +
    │ │ │ │ +
    43
    │ │ │ │
    46
    │ │ │ │ -
    47 typedef PinholeCamera<Cal3_S2> Base;
    │ │ │ │ -
    48 typedef boost::shared_ptr<SimpleCamera> shared_ptr;
    │ │ │ │ -
    49
    │ │ │ │ -
    50public:
    │ │ │ │ +
    │ │ │ │ +
    48 void print(const std::string& s = "") const override {
    │ │ │ │ +
    49 Base::print(s);
    │ │ │ │ +
    50 }
    │ │ │ │ +
    │ │ │ │
    51
    │ │ │ │ -
    54
    │ │ │ │ -
    56 SimpleCamera() :
    │ │ │ │ -
    57 Base() {
    │ │ │ │ -
    58 }
    │ │ │ │ -
    59
    │ │ │ │ -
    61 explicit SimpleCamera(const Pose3& pose) :
    │ │ │ │ -
    62 Base(pose) {
    │ │ │ │ -
    63 }
    │ │ │ │ -
    64
    │ │ │ │ -
    66 SimpleCamera(const Pose3& pose, const Cal3_S2& K) :
    │ │ │ │ -
    67 Base(pose, K) {
    │ │ │ │ -
    68 }
    │ │ │ │ -
    69
    │ │ │ │ -
    73
    │ │ │ │ -
    81 static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2,
    │ │ │ │ -
    82 double height) {
    │ │ │ │ -
    83 return SimpleCamera(Base::LevelPose(pose2, height), K);
    │ │ │ │ -
    84 }
    │ │ │ │ -
    85
    │ │ │ │ -
    87 static SimpleCamera Level(const Pose2& pose2, double height) {
    │ │ │ │ -
    88 return SimpleCamera::Level(Cal3_S2(), pose2, height);
    │ │ │ │ -
    89 }
    │ │ │ │ -
    90
    │ │ │ │ -
    100 static SimpleCamera Lookat(const Point3& eye, const Point3& target,
    │ │ │ │ -
    101 const Point3& upVector, const Cal3_S2& K = Cal3_S2()) {
    │ │ │ │ -
    102 return SimpleCamera(Base::LookatPose(eye, target, upVector), K);
    │ │ │ │ -
    103 }
    │ │ │ │ -
    104
    │ │ │ │ -
    108
    │ │ │ │ -
    110 explicit SimpleCamera(const Vector &v) :
    │ │ │ │ -
    111 Base(v) {
    │ │ │ │ -
    112 }
    │ │ │ │ -
    113
    │ │ │ │ -
    115 SimpleCamera(const Vector &v, const Vector &K) :
    │ │ │ │ -
    116 Base(v, K) {
    │ │ │ │ -
    117 }
    │ │ │ │ -
    118
    │ │ │ │ -
    120 SimpleCamera::shared_ptr clone() const { return boost::make_shared<SimpleCamera>(*this); }
    │ │ │ │ -
    121
    │ │ │ │ -
    122
    │ │ │ │ -
    126
    │ │ │ │ -
    128 SimpleCamera retract(const Vector& d) const {
    │ │ │ │ -
    129 if ((size_t) d.size() == 6)
    │ │ │ │ -
    130 return SimpleCamera(this->pose().retract(d), calibration());
    │ │ │ │ -
    131 else
    │ │ │ │ -
    132 return SimpleCamera(this->pose().retract(d.head(6)),
    │ │ │ │ -
    133 calibration().retract(d.tail(calibration().dim())));
    │ │ │ │ -
    134 }
    │ │ │ │ -
    135
    │ │ │ │ -
    137
    │ │ │ │ -
    138};
    │ │ │ │ -
    139
    │ │ │ │ -
    141GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
    │ │ │ │ -
    142
    │ │ │ │ -
    143// manifold traits
    │ │ │ │ -
    144template <>
    │ │ │ │ -
    145struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
    │ │ │ │ -
    146
    │ │ │ │ -
    147template <>
    │ │ │ │ -
    148struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
    │ │ │ │ -
    149
    │ │ │ │ -
    150// range traits, used in RangeFactor
    │ │ │ │ -
    151template <typename T>
    │ │ │ │ -
    152struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
    │ │ │ │ -
    153
    │ │ │ │ -
    154#endif
    │ │ │ │ -
    155
    │ │ │ │ -
    156} // namespace gtsam
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ -
    Unified Calibration Model, see Mei07icra for details.
    │ │ │ │ -
    Calibration used by Bundler.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    Calibration of a fisheye camera.
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ -
    Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
    │ │ │ │ +
    │ │ │ │ +
    53 bool equals(const PinholeSet& p, double tol = 1e-9) const {
    │ │ │ │ +
    54 return Base::equals(p, tol); // TODO all flags
    │ │ │ │ +
    55 }
    │ │ │ │ +
    │ │ │ │ +
    56
    │ │ │ │ +
    58
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    61 const typename CAMERA::MeasurementVector& measured,
    │ │ │ │ +
    62 const TriangulationParameters& params) const {
    │ │ │ │ +
    63 return gtsam::triangulateSafe(*this, measured, params);
    │ │ │ │ +
    64 }
    │ │ │ │ +
    │ │ │ │ +
    65
    │ │ │ │ +
    66private:
    │ │ │ │ +
    67
    │ │ │ │ + │ │ │ │ +
    70 template<class ARCHIVE>
    │ │ │ │ +
    71 void serialize(ARCHIVE & ar, const unsigned int version) {
    │ │ │ │ +
    72 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    │ │ │ │ +
    73 }
    │ │ │ │ +
    74};
    │ │ │ │ +
    │ │ │ │ +
    75
    │ │ │ │ +
    76template<class CAMERA>
    │ │ │ │ +
    │ │ │ │ +
    77struct traits<PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
    │ │ │ │ +
    78};
    │ │ │ │ +
    │ │ │ │ +
    79
    │ │ │ │ +
    80template<class CAMERA>
    │ │ │ │ +
    │ │ │ │ +
    81struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
    │ │ │ │ +
    82};
    │ │ │ │ +
    │ │ │ │ +
    83
    │ │ │ │ +
    84} // \ namespace gtsam
    │ │ │ │ +
    Base class to create smart factors on poses or cameras.
    │ │ │ │ +
    Functions for triangulation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2
    Convenient aliases for Pinhole camera classes with different calibrations.
    Definition SimpleCamera.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    PinholeSet: triangulates point and keeps an estimate of it around.
    Definition PinholeSet.h:30
    │ │ │ │ +
    TriangulationResult triangulateSafe(const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params) const
    triangulateSafe
    Definition PinholeSet.h:60
    │ │ │ │ +
    bool equals(const PinholeSet &p, double tol=1e-9) const
    equals
    Definition PinholeSet.h:53
    │ │ │ │ +
    virtual ~PinholeSet()
    Virtual destructor.
    Definition PinholeSet.h:41
    │ │ │ │ +
    void print(const std::string &s="") const override
    print
    Definition PinholeSet.h:48
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition PinholeSet.h:69
    │ │ │ │ +
    Definition triangulation.h:556
    │ │ │ │ +
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,160 +1,134 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SimpleCamera.h │ │ │ │ │ +PinholeSet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_D_S_2_._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_F_i_s_h_e_y_e_._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 <_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> │ │ │ │ │ -29 │ │ │ │ │ -30namespace _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_m_e_r_a_S_e_t_._h> │ │ │ │ │ +21#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> │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +29template │ │ │ │ │ +_3_0class _P_i_n_h_o_l_e_S_e_t: public _C_a_m_e_r_a_S_e_t { │ │ │ │ │ 31 │ │ │ │ │ -_3_4 using _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_>; │ │ │ │ │ -35 using PinholeCameraCal3Bundler = _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_>; │ │ │ │ │ -36 using PinholeCameraCal3DS2 = _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_>; │ │ │ │ │ -37 using PinholeCameraCal3Unified = _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_>; │ │ │ │ │ -38 using PinholeCameraCal3Fisheye = _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_>; │ │ │ │ │ +32private: │ │ │ │ │ +33 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> Base; │ │ │ │ │ +34 typedef _P_i_n_h_o_l_e_S_e_t_<_C_A_M_E_R_A_> This; │ │ │ │ │ +35 │ │ │ │ │ +36protected: │ │ │ │ │ +37 │ │ │ │ │ +38public: │ │ │ │ │ 39 │ │ │ │ │ -40#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -45class GTSAM_EXPORT SimpleCamera : public _P_i_n_h_o_l_e_C_a_m_e_r_a_C_a_l_3___S_2 { │ │ │ │ │ +_4_1 virtual _~_P_i_n_h_o_l_e_S_e_t() { │ │ │ │ │ +42 } │ │ │ │ │ +43 │ │ │ │ │ 46 │ │ │ │ │ -47 typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3___S_2_> Base; │ │ │ │ │ -48 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -49 │ │ │ │ │ -50public: │ │ │ │ │ +_4_8 void _p_r_i_n_t(const std::string& s = "") const override { │ │ │ │ │ +49 Base::print(s); │ │ │ │ │ +50 } │ │ │ │ │ 51 │ │ │ │ │ -54 │ │ │ │ │ -56 SimpleCamera() : │ │ │ │ │ -57 Base() { │ │ │ │ │ -58 } │ │ │ │ │ -59 │ │ │ │ │ -61 explicit SimpleCamera(const _P_o_s_e_3& pose) : │ │ │ │ │ -62 Base(pose) { │ │ │ │ │ -63 } │ │ │ │ │ -64 │ │ │ │ │ -66 SimpleCamera(const _P_o_s_e_3& pose, const _C_a_l_3___S_2& K) : │ │ │ │ │ -67 Base(pose, K) { │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -73 │ │ │ │ │ -81 static SimpleCamera Level(const _C_a_l_3___S_2 &K, const _P_o_s_e_2& pose2, │ │ │ │ │ -82 double height) { │ │ │ │ │ -83 return SimpleCamera(Base::LevelPose(pose2, height), K); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -87 static SimpleCamera Level(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ -88 return SimpleCamera::Level(_C_a_l_3___S_2(), pose2, height); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -100 static SimpleCamera Lookat(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ -101 const _P_o_i_n_t_3& upVector, const _C_a_l_3___S_2& K = _C_a_l_3___S_2()) { │ │ │ │ │ -102 return SimpleCamera(Base::LookatPose(eye, target, upVector), K); │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -108 │ │ │ │ │ -110 explicit SimpleCamera(const Vector &v) : │ │ │ │ │ -111 Base(v) { │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -115 SimpleCamera(const Vector &v, const Vector &K) : │ │ │ │ │ -116 Base(v, K) { │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -120 SimpleCamera::shared_ptr clone() const { return boost:: │ │ │ │ │ -make_shared(*this); } │ │ │ │ │ -121 │ │ │ │ │ -122 │ │ │ │ │ -126 │ │ │ │ │ -128 SimpleCamera retract(const Vector& d) const { │ │ │ │ │ -129 if ((size_t) d.size() == 6) │ │ │ │ │ -130 return SimpleCamera(this->pose().retract(d), calibration()); │ │ │ │ │ -131 else │ │ │ │ │ -132 return SimpleCamera(this->pose().retract(d.head(6)), │ │ │ │ │ -133 calibration().retract(d.tail(calibration().dim()))); │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -137 │ │ │ │ │ -138}; │ │ │ │ │ -139 │ │ │ │ │ -141GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); │ │ │ │ │ -142 │ │ │ │ │ -143// manifold traits │ │ │ │ │ -144template <> │ │ │ │ │ -145struct traits : public internal::Manifold {}; │ │ │ │ │ -146 │ │ │ │ │ -147template <> │ │ │ │ │ -148struct traits : public internal::Manifold │ │ │ │ │ -{}; │ │ │ │ │ -149 │ │ │ │ │ -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_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_F_i_s_h_e_y_e_._h │ │ │ │ │ -Calibration of a fisheye camera. │ │ │ │ │ -_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. │ │ │ │ │ +_5_3 bool _e_q_u_a_l_s(const _P_i_n_h_o_l_e_S_e_t& p, double tol = 1e-9) const { │ │ │ │ │ +54 return Base::equals(p, tol); // TODO all flags │ │ │ │ │ +55 } │ │ │ │ │ +56 │ │ │ │ │ +58 │ │ │ │ │ +_6_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( │ │ │ │ │ +61 const typename CAMERA::MeasurementVector& measured, │ │ │ │ │ +62 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) const { │ │ │ │ │ +63 return _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(*this, measured, params); │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +66private: │ │ │ │ │ +67 │ │ │ │ │ +_6_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; │ │ │ │ │ +70 template │ │ │ │ │ +71 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +72 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +73 } │ │ │ │ │ +74}; │ │ │ │ │ +75 │ │ │ │ │ +76template │ │ │ │ │ +_7_7struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_S_e_t > : public _T_e_s_t_a_b_l_e > { │ │ │ │ │ +78}; │ │ │ │ │ +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. │ │ │ │ │ _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 │ │ │ │ │ -_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_:_:_C_a_l_3___S_2 │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ -_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_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_:_:_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_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_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_S_e_t │ │ │ │ │ +PinholeSet: triangulates point and keeps an estimate of it around. │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ +TriangulationResult triangulateSafe(const typename CAMERA::MeasurementVector │ │ │ │ │ +&measured, const TriangulationParameters ¶ms) const │ │ │ │ │ +triangulateSafe │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const PinholeSet &p, double tol=1e-9) const │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_~_P_i_n_h_o_l_e_S_e_t │ │ │ │ │ +virtual ~PinholeSet() │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn PinholeSet.h:69 │ │ │ │ │ +_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_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_p_l_e_C_a_m_e_r_a_._h │ │ │ │ │ + * _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/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/BearingRange.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Cal3_S2Stereo.h File Reference
    │ │ │ │ +
    BearingRange.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Bearing-Range product. │ │ │ │ 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::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< Cal3_S2Stereo >
    struct  gtsam::traits< BearingRange< A1, A2 > >
     
    struct  gtsam::traits< const Cal3_S2Stereo >
    struct  gtsam::HasBearing< A1, A2, RT >
     
    struct  gtsam::HasRange< A1, A2, RT >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

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

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

    Bearing-Range product.

    │ │ │ │ +
    Date
    July, 2015
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -Cal3_S2Stereo.h File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ - 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_:_:_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_<_ _C_a_l_3___S_2_S_t_e_r_e_o_ _> │ │ │ │ │ +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_:_:_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_:_:_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ + Date │ │ │ │ │ + July, 2015 │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00302.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,6 @@ │ │ │ │ │ var a00302 = [ │ │ │ │ │ - ["gtsam::traits< Cal3_S2Stereo >", "a02960.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3_S2Stereo >", "a02964.html", null] │ │ │ │ │ + ["gtsam::BearingRange< A1, A2, B, R >", "a02924.html", "a02924"], │ │ │ │ │ + ["gtsam::traits< BearingRange< A1, A2 > >", "a02928.html", null], │ │ │ │ │ + ["gtsam::HasBearing< A1, A2, RT >", "a02932.html", null], │ │ │ │ │ + ["gtsam::HasRange< A1, A2, RT >", "a02936.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/BearingRange.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,163 +98,228 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3_S2Stereo.h
    │ │ │ │ +
    BearingRange.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
    │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ -
    │ │ │ │ -
    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); }
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    22#include <gtsam/base/Testable.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
    │ │ │ │ +
    31// For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
    │ │ │ │ +
    32// At time of writing only Pose2 and Pose3 specialize this functor.
    │ │ │ │ +
    33template <typename A1, typename A2>
    │ │ │ │ +
    34struct Bearing;
    │ │ │ │ +
    35
    │ │ │ │ +
    36// Forward declaration of Range functor which should be of A1*A2 -> return_type
    │ │ │ │ +
    37// For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h will return double
    │ │ │ │ +
    38// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
    │ │ │ │ +
    39template <typename A1, typename A2>
    │ │ │ │ +
    40struct Range;
    │ │ │ │ +
    41
    │ │ │ │ +
    48template <typename A1, typename A2,
    │ │ │ │ +
    49 typename B = typename Bearing<A1, A2>::result_type,
    │ │ │ │ +
    50 typename R = typename Range<A1, A2>::result_type>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    52private:
    │ │ │ │ +
    53 B bearing_;
    │ │ │ │ +
    54 R range_;
    │ │ │ │ +
    55
    │ │ │ │ +
    56public:
    │ │ │ │ +
    57 enum { dimB = traits<B>::dimension };
    │ │ │ │ +
    58 enum { dimR = traits<R>::dimension };
    │ │ │ │ +
    59 enum { dimension = dimB + dimR };
    │ │ │ │ +
    60
    │ │ │ │ +
    63
    │ │ │ │ +
    64 BearingRange() {}
    │ │ │ │ +
    65 BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
    │ │ │ │ +
    66
    │ │ │ │ +
    70
    │ │ │ │ +
    72 const B& bearing() const { return bearing_; }
    │ │ │ │ +
    73
    │ │ │ │ +
    75 const R& range() const { return range_; }
    │ │ │ │ +
    76
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    79 const A1& a1, const A2& a2,
    │ │ │ │ +
    80 OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
    │ │ │ │ +
    81 OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
    │ │ │ │ +
    82 typename MakeJacobian<B, A1>::type HB1;
    │ │ │ │ +
    83 typename MakeJacobian<B, A2>::type HB2;
    │ │ │ │ +
    84 typename MakeJacobian<R, A1>::type HR1;
    │ │ │ │ +
    85 typename MakeJacobian<R, A2>::type HR2;
    │ │ │ │
    86
    │ │ │ │ +
    87 B b = Bearing<A1, A2>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
    │ │ │ │ +
    88 R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
    │ │ │ │
    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;
    │ │ │ │ +
    90 if (H1) *H1 << HB1, HR1;
    │ │ │ │ +
    91 if (H2) *H2 << HB2, HR2;
    │ │ │ │ +
    92 return BearingRange(b, r);
    │ │ │ │ +
    93 }
    │ │ │ │ +
    │ │ │ │ +
    94
    │ │ │ │ +
    │ │ │ │ +
    96 static B MeasureBearing(const A1& a1, const A2& a2) {
    │ │ │ │ +
    97 return Bearing<A1, A2>()(a1, a2);
    │ │ │ │ +
    98 }
    │ │ │ │ +
    │ │ │ │
    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
    │ │ │ │ -
    123
    │ │ │ │ -
    125 inline size_t dim() const override { return Dim(); }
    │ │ │ │ -
    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));
    │ │ │ │ +
    │ │ │ │ +
    101 static R MeasureRange(const A1& a1, const A2& a2) {
    │ │ │ │ +
    102 return Range<A1, A2>()(a1, a2);
    │ │ │ │ +
    103 }
    │ │ │ │ +
    │ │ │ │ +
    104
    │ │ │ │ +
    108
    │ │ │ │ +
    109 void print(const std::string& str = "") const {
    │ │ │ │ +
    110 std::cout << str;
    │ │ │ │ +
    111 traits<B>::Print(bearing_, "bearing ");
    │ │ │ │ +
    112 traits<R>::Print(range_, "range ");
    │ │ │ │ +
    113 }
    │ │ │ │ +
    114 bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
    │ │ │ │ +
    115 return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
    │ │ │ │ +
    116 traits<R>::Equals(range_, m2.range_, tol);
    │ │ │ │ +
    117 }
    │ │ │ │ +
    118
    │ │ │ │ +
    122
    │ │ │ │ +
    123 inline static size_t Dim() { return dimension; }
    │ │ │ │ +
    124 inline size_t dim() const { return dimension; }
    │ │ │ │ +
    125
    │ │ │ │ +
    126 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
    │ │ │ │ +
    127 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
    │ │ │ │ +
    128
    │ │ │ │ +
    │ │ │ │ +
    130 BearingRange retract(const TangentVector& xi) const {
    │ │ │ │ +
    131 B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
    │ │ │ │ +
    132 R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
    │ │ │ │ +
    133 return BearingRange(m1, m2);
    │ │ │ │
    134 }
    │ │ │ │
    │ │ │ │
    135
    │ │ │ │
    │ │ │ │ -
    137 Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
    │ │ │ │ -
    138 return T2.vector() - vector();
    │ │ │ │ -
    139 }
    │ │ │ │ +
    137 TangentVector localCoordinates(const BearingRange& other) const {
    │ │ │ │ +
    138 typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
    │ │ │ │ +
    139 typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
    │ │ │ │ +
    140 TangentVector v;
    │ │ │ │ +
    141 v << v1, v2;
    │ │ │ │ +
    142 return v;
    │ │ │ │ +
    143 }
    │ │ │ │
    │ │ │ │ -
    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};
    │ │ │ │ -
    │ │ │ │ +
    148
    │ │ │ │ +
    149private:
    │ │ │ │ +
    151 template <class ARCHIVE>
    │ │ │ │ +
    152 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    │ │ │ │ +
    153 ar& boost::serialization::make_nvp("bearing", bearing_);
    │ │ │ │ +
    154 ar& boost::serialization::make_nvp("range", range_);
    │ │ │ │ +
    155 }
    │ │ │ │
    156
    │ │ │ │ -
    157// Define GTSAM traits
    │ │ │ │ -
    158template <>
    │ │ │ │ -
    159struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
    │ │ │ │ +
    157 friend class boost::serialization::access;
    │ │ │ │ +
    158
    │ │ │ │
    160
    │ │ │ │ -
    161template <>
    │ │ │ │ -
    │ │ │ │ -
    162struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
    │ │ │ │ -
    163};
    │ │ │ │ -
    │ │ │ │ -
    164
    │ │ │ │ -
    165} // \ namespace gtsam
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    167};
    │ │ │ │ +
    │ │ │ │ +
    168
    │ │ │ │ +
    169// Declare this to be both Testable and a Manifold
    │ │ │ │ +
    170template <typename A1, typename A2>
    │ │ │ │ +
    │ │ │ │ +
    171struct traits<BearingRange<A1, A2> >
    │ │ │ │ +
    172 : Testable<BearingRange<A1, A2> >,
    │ │ │ │ +
    173 internal::ManifoldTraits<BearingRange<A1, A2> > {};
    │ │ │ │ +
    │ │ │ │ +
    174
    │ │ │ │ +
    175// Helper class for to implement Range traits for classes with a bearing method
    │ │ │ │ +
    176// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
    │ │ │ │ +
    177// template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
    │ │ │ │ +
    178// where the third argument is used to indicate the return type
    │ │ │ │ +
    179template <class A1, typename A2, class RT>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    181 typedef RT result_type;
    │ │ │ │ +
    182 RT operator()(
    │ │ │ │ +
    183 const A1& a1, const A2& a2,
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    186 return a1.bearing(a2, H1, H2);
    │ │ │ │ +
    187 }
    │ │ │ │ +
    188};
    │ │ │ │ +
    │ │ │ │ +
    189
    │ │ │ │ +
    190// Similar helper class for to implement Range traits for classes with a range method
    │ │ │ │ +
    191// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
    │ │ │ │ +
    192// template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
    │ │ │ │ +
    193template <class A1, typename A2, class RT>
    │ │ │ │ +
    │ │ │ │ +
    194struct HasRange {
    │ │ │ │ +
    195 typedef RT result_type;
    │ │ │ │ +
    196 RT operator()(
    │ │ │ │ +
    197 const A1& a1, const A2& a2,
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    200 return a1.range(a2, H1, H2);
    │ │ │ │ +
    201 }
    │ │ │ │ +
    202};
    │ │ │ │ +
    │ │ │ │ +
    203
    │ │ │ │ +
    204} // namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM manifolds.
    Definition Manifold.h:95
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │ -
    Vector6 vector() const
    vectorized form (column-wise)
    Definition Cal3_S2Stereo.h:114
    │ │ │ │ -
    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
    constructor from doubles
    Definition Cal3_S2Stereo.h:47
    │ │ │ │ -
    Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
    Unretraction for the calibration.
    Definition Cal3_S2Stereo.h:137
    │ │ │ │ -
    const Cal3_S2 & calibration() const
    return calibration, same for left and right
    Definition Cal3_S2Stereo.h:105
    │ │ │ │ -
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3_S2Stereo.h:125
    │ │ │ │ -
    Vector3 calibrate(const Vector3 &p) const
    Convert homogeneous image coordinates to intrinsic coordinates.
    Definition Cal3_S2Stereo.h:85
    │ │ │ │ -
    Cal3_S2Stereo retract(const Vector &d) const
    Given 6-dim tangent vector, create new calibration.
    Definition Cal3_S2Stereo.h:131
    │ │ │ │ -
    Matrix3 K() const override
    return calibration matrix K, same for left and right
    Definition Cal3_S2Stereo.h:108
    │ │ │ │ -
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3_S2Stereo.h:128
    │ │ │ │ -
    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
    │ │ │ │ -
    double baseline() const
    return baseline
    Definition Cal3_S2Stereo.h:111
    │ │ │ │ -
    Cal3_S2Stereo(const Vector6 &d)
    constructor from vector
    Definition Cal3_S2Stereo.h:51
    │ │ │ │ -
    Cal3_S2Stereo()=default
    default calibration leaves coordinates unchanged
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Definition BearingRange.h:34
    │ │ │ │ +
    Definition BearingRange.h:40
    │ │ │ │ +
    Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
    Definition BearingRange.h:51
    │ │ │ │ +
    static R MeasureRange(const A1 &a1, const A2 &a2)
    Predict range.
    Definition BearingRange.h:101
    │ │ │ │ +
    static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< dimension, traits< A2 >::dimension > H2=boost::none)
    Prediction function that stacks measurements.
    Definition BearingRange.h:78
    │ │ │ │ +
    BearingRange retract(const TangentVector &xi) const
    Retract delta to manifold.
    Definition BearingRange.h:130
    │ │ │ │ +
    const B & bearing() const
    Return bearing measurement.
    Definition BearingRange.h:72
    │ │ │ │ +
    TangentVector localCoordinates(const BearingRange &other) const
    Compute the coordinates in the tangent space.
    Definition BearingRange.h:137
    │ │ │ │ +
    const R & range() const
    Return range measurement.
    Definition BearingRange.h:75
    │ │ │ │ +
    static B MeasureBearing(const A1 &a1, const A2 &a2)
    Predict bearing.
    Definition BearingRange.h:96
    │ │ │ │ +
    Definition BearingRange.h:180
    │ │ │ │ +
    Definition BearingRange.h:194
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,204 +1,268 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3_S2Stereo.h │ │ │ │ │ +BearingRange.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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) {} │ │ │ │ │ -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 │ │ │ │ │ -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); } │ │ │ │ │ +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_e_s_t_a_b_l_e_._h> │ │ │ │ │ +23#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> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Forward declaration of Bearing functor which should be of A1*A2 - │ │ │ │ │ +> return_type │ │ │ │ │ +31// For example Bearing(pose,point), defined in Pose3.h will │ │ │ │ │ +return Unit3 │ │ │ │ │ +32// At time of writing only Pose2 and Pose3 specialize this functor. │ │ │ │ │ +33template │ │ │ │ │ +_3_4struct _B_e_a_r_i_n_g; │ │ │ │ │ +35 │ │ │ │ │ +36// Forward declaration of Range functor which should be of A1*A2 - │ │ │ │ │ +> return_type │ │ │ │ │ +37// For example Range(T1,T2), defined in Pose2.h will return │ │ │ │ │ +double │ │ │ │ │ +38// At time of writing Pose2, Pose3, and several Camera variants specialize │ │ │ │ │ +this for several types │ │ │ │ │ +39template │ │ │ │ │ +_4_0struct _R_a_n_g_e; │ │ │ │ │ +41 │ │ │ │ │ +48template _:_:_r_e_s_u_l_t___t_y_p_e, │ │ │ │ │ +50 typename R = typename _R_a_n_g_e_<_A_1_,_ _A_2_>_:_:_r_e_s_u_l_t___t_y_p_e> │ │ │ │ │ +_5_1struct _B_e_a_r_i_n_g_R_a_n_g_e { │ │ │ │ │ +52private: │ │ │ │ │ +53 B bearing_; │ │ │ │ │ +54 R range_; │ │ │ │ │ +55 │ │ │ │ │ +56public: │ │ │ │ │ +57 enum { dimB = _t_r_a_i_t_s_<_B_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +58 enum { dimR = _t_r_a_i_t_s_<_R_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +59 enum { dimension = dimB + dimR }; │ │ │ │ │ +60 │ │ │ │ │ +63 │ │ │ │ │ +64 _B_e_a_r_i_n_g_R_a_n_g_e() {} │ │ │ │ │ +65 _B_e_a_r_i_n_g_R_a_n_g_e(const B& b, const R& r) : bearing_(b), range_(r) {} │ │ │ │ │ +66 │ │ │ │ │ +70 │ │ │ │ │ +_7_2 const B& _b_e_a_r_i_n_g() const { return bearing_; } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 const R& _r_a_n_g_e() const { return range_; } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 static _B_e_a_r_i_n_g_R_a_n_g_e _M_e_a_s_u_r_e( │ │ │ │ │ +79 const A1& a1, const A2& a2, │ │ │ │ │ +80 _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> H1 = boost::none, │ │ │ │ │ +81 _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> H2 = boost::none) { │ │ │ │ │ +82 typename MakeJacobian::type HB1; │ │ │ │ │ +83 typename MakeJacobian::type HB2; │ │ │ │ │ +84 typename MakeJacobian::type HR1; │ │ │ │ │ +85 typename MakeJacobian::type HR2; │ │ │ │ │ 86 │ │ │ │ │ +87 B b = _B_e_a_r_i_n_g_<_A_1_,_ _A_2_>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0); │ │ │ │ │ +88 R r = _R_a_n_g_e_<_A_1_,_ _A_2_>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0); │ │ │ │ │ 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; │ │ │ │ │ +90 if (H1) *H1 << HB1, HR1; │ │ │ │ │ +91 if (H2) *H2 << HB2, HR2; │ │ │ │ │ +92 return _B_e_a_r_i_n_g_R_a_n_g_e(b, r); │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 static B _M_e_a_s_u_r_e_B_e_a_r_i_n_g(const A1& a1, const A2& a2) { │ │ │ │ │ +97 return _B_e_a_r_i_n_g_<_A_1_,_ _A_2_>()(a1, a2); │ │ │ │ │ +98 } │ │ │ │ │ 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 │ │ │ │ │ -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 │ │ │ │ │ -_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)); │ │ │ │ │ +_1_0_1 static R _M_e_a_s_u_r_e_R_a_n_g_e(const A1& a1, const A2& a2) { │ │ │ │ │ +102 return _R_a_n_g_e_<_A_1_,_ _A_2_>()(a1, a2); │ │ │ │ │ +103 } │ │ │ │ │ +104 │ │ │ │ │ +108 │ │ │ │ │ +109 void print(const std::string& str = "") const { │ │ │ │ │ +110 std::cout << str; │ │ │ │ │ +111 _t_r_a_i_t_s_<_B_>_:_:_P_r_i_n_t(bearing_, "bearing "); │ │ │ │ │ +112 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(range_, "range "); │ │ │ │ │ +113 } │ │ │ │ │ +114 bool equals(const BearingRange& m2, double tol = 1e-8) const { │ │ │ │ │ +115 return traits::Equals(bearing_, m2.bearing_, tol) && │ │ │ │ │ +116 traits::Equals(range_, m2.range_, tol); │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +122 │ │ │ │ │ +123 inline static size_t Dim() { return dimension; } │ │ │ │ │ +124 inline size_t dim() const { return dimension; } │ │ │ │ │ +125 │ │ │ │ │ +126 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +127 typedef OptionalJacobian ChartJacobian; │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 _B_e_a_r_i_n_g_R_a_n_g_e _r_e_t_r_a_c_t(const TangentVector& xi) const { │ │ │ │ │ +131 B m1 = _t_r_a_i_t_s_<_B_>_:_:_R_e_t_r_a_c_t(bearing_, xi.template head()); │ │ │ │ │ +132 R m2 = _t_r_a_i_t_s_<_R_>_:_:_R_e_t_r_a_c_t(range_, xi.template tail()); │ │ │ │ │ +133 return _B_e_a_r_i_n_g_R_a_n_g_e(m1, m2); │ │ │ │ │ 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 │ │ │ │ │ +_1_3_7 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _B_e_a_r_i_n_g_R_a_n_g_e& other) const { │ │ │ │ │ +138 typename _t_r_a_i_t_s_<_B_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v1 = _t_r_a_i_t_s_<_B_>_:_:_L_o_c_a_l(bearing_, │ │ │ │ │ +other.bearing_); │ │ │ │ │ +139 typename _t_r_a_i_t_s_<_R_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v2 = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(range_, │ │ │ │ │ +other.range_); │ │ │ │ │ +140 TangentVector v; │ │ │ │ │ +141 v << v1, v2; │ │ │ │ │ +142 return v; │ │ │ │ │ +143 } │ │ │ │ │ 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}; │ │ │ │ │ +148 │ │ │ │ │ +149private: │ │ │ │ │ +151 template │ │ │ │ │ +152 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +153 ar& boost::serialization::make_nvp("bearing", bearing_); │ │ │ │ │ +154 ar& boost::serialization::make_nvp("range", range_); │ │ │ │ │ +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 {}; │ │ │ │ │ +157 friend class boost::serialization::access; │ │ │ │ │ +158 │ │ │ │ │ 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. │ │ │ │ │ +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 _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) │ │ │ │ │ +167}; │ │ │ │ │ +168 │ │ │ │ │ +169// Declare this to be both Testable and a Manifold │ │ │ │ │ +170template │ │ │ │ │ +_1_7_1struct _t_r_a_i_t_s<_B_e_a_r_i_n_g_R_a_n_g_e > │ │ │ │ │ +172 : _T_e_s_t_a_b_l_e >, │ │ │ │ │ +173 _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s > {}; │ │ │ │ │ +174 │ │ │ │ │ +175// Helper class for to implement Range traits for classes with a bearing │ │ │ │ │ +method │ │ │ │ │ +176// For example, to specialize Bearing to Pose3 and Point3, using Pose3:: │ │ │ │ │ +bearing, it suffices to say │ │ │ │ │ +177// template <> struct Bearing : HasBearing {}; │ │ │ │ │ +178// where the third argument is used to indicate the return type │ │ │ │ │ +179template │ │ │ │ │ +_1_8_0struct _H_a_s_B_e_a_r_i_n_g { │ │ │ │ │ +181 typedef RT result_type; │ │ │ │ │ +182 RT operator()( │ │ │ │ │ +183 const A1& a1, const A2& a2, │ │ │ │ │ +184 _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_1_>_:_:_d_i_m_e_n_s_i_o_n> H1=boost:: │ │ │ │ │ +none, │ │ │ │ │ +185 _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) { │ │ │ │ │ +186 return a1.bearing(a2, H1, H2); │ │ │ │ │ +187 } │ │ │ │ │ +188}; │ │ │ │ │ +189 │ │ │ │ │ +190// Similar helper class for to implement Range traits for classes with a │ │ │ │ │ +range method │ │ │ │ │ +191// For classes with overloaded range methods, such as PinholeCamera, this │ │ │ │ │ +can even be templated: │ │ │ │ │ +192// template struct Range : │ │ │ │ │ +HasRange {}; │ │ │ │ │ +193template │ │ │ │ │ +_1_9_4struct _H_a_s_R_a_n_g_e { │ │ │ │ │ +195 typedef RT result_type; │ │ │ │ │ +196 RT operator()( │ │ │ │ │ +197 const A1& a1, const A2& a2, │ │ │ │ │ +198 _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_1_>_:_:_d_i_m_e_n_s_i_o_n> H1=boost:: │ │ │ │ │ +none, │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -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_:_:_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_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_:_:_B_e_a_r_i_n_g_R_a_n_g_e │ │ │ │ │ +Bearing-Range product for a particular A1,A2 combination will use the functors │ │ │ │ │ +above to create a simi... │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e_R_a_n_g_e │ │ │ │ │ +static R MeasureRange(const A1 &a1, const A2 &a2) │ │ │ │ │ +Predict range. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e │ │ │ │ │ +static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< │ │ │ │ │ +dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< │ │ │ │ │ +dimension, traits< A2 >::dimension > H2=boost::none) │ │ │ │ │ +Prediction function that stacks measurements. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ +BearingRange retract(const TangentVector &xi) const │ │ │ │ │ +Retract delta to manifold. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_b_e_a_r_i_n_g │ │ │ │ │ +const B & bearing() const │ │ │ │ │ +Return bearing measurement. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const BearingRange &other) const │ │ │ │ │ +Compute the coordinates in the tangent space. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_r_a_n_g_e │ │ │ │ │ +const R & range() const │ │ │ │ │ +Return range measurement. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e_B_e_a_r_i_n_g │ │ │ │ │ +static B MeasureBearing(const A1 &a1, const A2 &a2) │ │ │ │ │ +Predict bearing. │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:96 │ │ │ │ │ +_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ + * _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/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/Rot3M.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Cal3Unified.h File Reference
    │ │ │ │ +
    Rot3M.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Rotation (internal: 3*3 matrix representation*) │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Unified Calibration Model, see Mei07icra for details.

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

    Rotation (internal: 3*3 matrix representation*)

    │ │ │ │ +
    Author
    Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ - Date │ │ │ │ │ - Mar 8, 2014 │ │ │ │ │ +Rotation (internal: 3*3 matrix representation*) │ │ │ │ │ Author │ │ │ │ │ - Jing Dong │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/OrientedPlane3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,400 +98,178 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SOn.h
    │ │ │ │ +
    OrientedPlane3.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -Go to the documentation of this file.
    1/* ----------------------------------------------------------------------------
    │ │ │ │ +
    1/* ----------------------------------------------------------------------------
    │ │ │ │
    2
    │ │ │ │ -
    3 * GTSAM Copyright 2010-2019, 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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ +
    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#include <gtsam/base/Lie.h>
    │ │ │ │ -
    22#include <gtsam/base/Manifold.h>
    │ │ │ │ - │ │ │ │ -
    24#include <gtsam/dllexport.h>
    │ │ │ │ -
    25#include <Eigen/Core>
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/geometry/Unit3.h>
    │ │ │ │ + │ │ │ │ +
    25#include <string>
    │ │ │ │
    26
    │ │ │ │ -
    27#include <boost/serialization/nvp.hpp>
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │
    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}
    │ │ │ │ -
    │ │ │ │ -
    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>;
    │ │ │ │ +
    │ │ │ │ +
    36class GTSAM_EXPORT OrientedPlane3 {
    │ │ │ │ +
    37private:
    │ │ │ │ +
    38 Unit3 n_;
    │ │ │ │ +
    39 double d_;
    │ │ │ │ +
    40
    │ │ │ │ +
    41public:
    │ │ │ │ +
    42 enum {
    │ │ │ │ +
    43 dimension = 3
    │ │ │ │ +
    44 };
    │ │ │ │ +
    45
    │ │ │ │ +
    48
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    51 n_(), d_(0.0) {
    │ │ │ │ +
    52 }
    │ │ │ │ +
    │ │ │ │ +
    53
    │ │ │ │ +
    │ │ │ │ +
    55 OrientedPlane3(const Unit3& n, double d) :
    │ │ │ │ +
    56 n_(n), d_(d) {
    │ │ │ │ +
    57 }
    │ │ │ │ +
    │ │ │ │
    58
    │ │ │ │ - │ │ │ │ -
    60
    │ │ │ │ -
    61 protected:
    │ │ │ │ -
    62 MatrixNN matrix_;
    │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    73 public:
    │ │ │ │ -
    76
    │ │ │ │ -
    78 template <int N_ = N, typename = IsFixed<N_>>
    │ │ │ │ -
    79 SO() : matrix_(MatrixNN::Identity()) {}
    │ │ │ │ +
    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 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()) {}
    │ │ │ │ +
    82
    │ │ │ │ +
    89 OrientedPlane3 transform(const Pose3& xr,
    │ │ │ │ +
    90 OptionalJacobian<3, 3> Hp = boost::none,
    │ │ │ │ +
    91 OptionalJacobian<3, 6> Hr = boost::none) const;
    │ │ │ │
    92
    │ │ │ │ -
    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);
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    144template<> struct traits<const OrientedPlane3> : public internal::Manifold<
    │ │ │ │ +
    145OrientedPlane3> {
    │ │ │ │ +
    146};
    │ │ │ │
    │ │ │ │ +
    147
    │ │ │ │ +
    148} // namespace gtsam
    │ │ │ │
    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;
    │ │ │ │ -
    165
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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"
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    make_shared trampoline function to ensure proper alignment
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ -
    Template implementations for SO(n)
    │ │ │ │ -
    constexpr int DimensionSO(int N)
    Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
    Definition SOn.h:39
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ -
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ -
    static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
    Named constructor from Eigen Matrix.
    Definition SOn.h:95
    │ │ │ │ -
    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
    │ │ │ │ -
    static Matrix VectorizedGenerators()
    Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
    Definition SOn.h:300
    │ │ │ │ -
    SO inverse() const
    inverse of a rotation = transpose
    Definition SOn.h:193
    │ │ │ │ -
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ -
    static SO ChordalMean(const std::vector< SO > &rotations)
    Named constructor that finds chordal mean , currently only defined for SO3.
    │ │ │ │ -
    SO operator*(const SO &other) const
    Multiplication.
    Definition SOn.h:175
    │ │ │ │ -
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ -
    MatrixNN matrix_
    Rotation matrix.
    Definition SOn.h:62
    │ │ │ │ -
    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
    │ │ │ │ -
    static SO AxisAngle(const Vector3 &axis, double theta)
    Constructor from axis and angle. Only defined for SO3.
    │ │ │ │ -
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ -
    static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
    In-place version of Hat (see details there), implements recursion.
    │ │ │ │ -
    static SO Identity()
    SO<N> identity for N >= 2.
    Definition SOn.h:182
    │ │ │ │ -
    static int Dim()
    Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
    Definition SOn.h:203
    │ │ │ │ -
    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
    │ │ │ │ -
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ -
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ -
    SO(const SO< M > &R)
    Construct dynamic SO(n) from Fixed SO<M>
    Definition SOn.h:111
    │ │ │ │ -
    SO(size_t n=0)
    Construct SO<N> identity for N == Eigen::Dynamic.
    Definition SOn.h:83
    │ │ │ │ -
    SO()
    Construct SO<N> identity for N >= 2.
    Definition SOn.h:79
    │ │ │ │ -
    static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
    Named constructor from lower dimensional matrix.
    Definition SOn.h:101
    │ │ │ │ -
    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
    │ │ │ │ -
    static SO Identity(size_t n=0)
    SO<N> identity for N == Eigen::Dynamic.
    Definition SOn.h:188
    │ │ │ │ -
    const MatrixNN & matrix() const
    Return matrix.
    Definition SOn.h:155
    │ │ │ │ -
    static SO Random(std::mt19937 &rng)
    Random SO(N) element (no big claims about uniformity)
    Definition SOn.h:145
    │ │ │ │ -
    static SO ClosestTo(const MatrixNN &M)
    Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
    │ │ │ │ -
    SO(const Eigen::AngleAxisd &angleAxis)
    Constructor from AngleAxisd.
    Definition SOn.h:115
    │ │ │ │ -
    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
    │ │ │ │ -
    SO(const Eigen::MatrixBase< Derived > &R)
    Constructor from Eigen Matrix, dynamic version.
    Definition SOn.h:91
    │ │ │ │ -
    Definition SOn.h:240
    │ │ │ │ -
    static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
    Inverse of Retract.
    Definition SOn-inl.h:50
    │ │ │ │ -
    static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
    Retract uses Cayley map.
    Definition SOn-inl.h:40
    │ │ │ │ +
    Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
    Definition OrientedPlane3.h:36
    │ │ │ │ +
    OrientedPlane3(double a, double b, double c, double d)
    Construct from four numbers of plane coeffcients (a, b, c, d)
    Definition OrientedPlane3.h:64
    │ │ │ │ +
    Vector4 planeCoefficients() const
    Returns the plane coefficients.
    Definition OrientedPlane3.h:122
    │ │ │ │ +
    OrientedPlane3(const Unit3 &n, double d)
    Construct from a Unit3 and a distance.
    Definition OrientedPlane3.h:55
    │ │ │ │ +
    size_t dim() const
    Dimensionality of tangent space = 3 DOF.
    Definition OrientedPlane3.h:110
    │ │ │ │ +
    bool equals(const OrientedPlane3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition OrientedPlane3.h:77
    │ │ │ │ +
    OrientedPlane3(const Vector4 &vec)
    Construct from a vector of plane coefficients.
    Definition OrientedPlane3.h:60
    │ │ │ │ +
    static size_t Dim()
    Dimensionality of tangent space = 3 DOF.
    Definition OrientedPlane3.h:105
    │ │ │ │ +
    OrientedPlane3()
    Default constructor.
    Definition OrientedPlane3.h:50
    │ │ │ │ +
    double distance(OptionalJacobian< 1, 3 > H=boost::none) const
    Return the perpendicular distance to the origin.
    Definition OrientedPlane3.h:134
    │ │ │ │ +
    Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
    Return the normal.
    Definition OrientedPlane3.h:128
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Vector.
    Definition Unit3.cpp:151
    │ │ │ │ +
    bool equals(const Unit3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition Unit3.h:115
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,507 +1,201 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +OrientedPlane3.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, 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 │ │ │ │ │ -19#pragma once │ │ │ │ │ +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#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 │ │ │ │ │ +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 │ │ │ │ │ -27#include │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ 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} │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -73 public: │ │ │ │ │ -76 │ │ │ │ │ -78 template > │ │ │ │ │ -_7_9 _S_O() : _m_a_t_r_i_x__(MatrixNN::_I_d_e_n_t_i_t_y()) {} │ │ │ │ │ +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 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()) {} │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_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_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 │ │ │ │ │ -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 void print(const std::string& s = std::string()) const; │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_._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/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/CalibratedCamera.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,36 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Cal3Bundler.h File Reference
    │ │ │ │ +
    CalibratedCamera.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Calibration used by Bundler. │ │ │ │ +

    Calibrated camera for which only pose is unknown. │ │ │ │ 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 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Calibration used by Bundler.

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

    Calibrated camera for which only pose is unknown.

    │ │ │ │ +
    Date
    Aug 17, 2009
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_:_:_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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ Date │ │ │ │ │ - Sep 25, 2010 │ │ │ │ │ + Aug 17, 2009 │ │ │ │ │ Author │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + 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_._h │ │ │ │ │ + * _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/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/Cal3Bundler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,45 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Cyclic.h File Reference
    │ │ │ │ +
    Cal3Bundler.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Cyclic< N >
     Cyclic group of order N. More...
    class  gtsam::Cal3Bundler
     Calibration used by Bundler. More...
     
    struct  gtsam::traits< Cyclic< N > >
     Define cyclic group to be a model of the Additive Group concept. More...
    struct  gtsam::traits< Cal3Bundler >
     
    struct  gtsam::traits< const Cal3Bundler >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

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

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

    Calibration used by Bundler.

    │ │ │ │ +
    Date
    Sep 25, 2010
    │ │ │ │ +
    Author
    Yong Dian Jian
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -Cyclic.h File Reference │ │ │ │ │ -Cyclic group, i.e., the integers modulo N. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_C_y_c_l_i_c_<_ _N_ _> │ │ │ │ │ -  _C_y_c_l_i_c group of order N. _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_:_:_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_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_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Cyclic group, i.e., the integers modulo N. │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ + Date │ │ │ │ │ + Sep 25, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_y_c_l_i_c_._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/a00317.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00317 = [ │ │ │ │ │ - ["gtsam::Cyclic< N >", "a03060.html", "a03060"], │ │ │ │ │ - ["gtsam::traits< Cyclic< N > >", "a03064.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3Bundler >", "a02972.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3Bundler >", "a02976.html", 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/Cal3Bundler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,114 +98,162 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cyclic.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
    │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Concept check class for variable types with Group properties.
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/geometry/Cal3.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
    │ │ │ │ +
    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 }
    │ │ │ │ +
    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
    │ │ │ │ +
    Common code for all Calibration models.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Cyclic group of order N.
    Definition Cyclic.h:30
    │ │ │ │ -
    bool equals(const Cyclic &other, double tol=1e-9) const
    equals with an tolerance, prints out message if unequal
    Definition Cyclic.h:64
    │ │ │ │ -
    void print(const std::string &s="") const
    print with optional string
    Definition Cyclic.h:60
    │ │ │ │ -
    Cyclic()
    Default constructor yields identity.
    Definition Cyclic.h:39
    │ │ │ │ -
    Cyclic(size_t i)
    Constructor.
    Definition Cyclic.h:34
    │ │ │ │ -
    Cyclic operator-(const Cyclic &h) const
    Subtraction modulo N.
    Definition Cyclic.h:52
    │ │ │ │ -
    Cyclic operator+(const Cyclic &h) const
    Addition modulo N.
    Definition Cyclic.h:48
    │ │ │ │ -
    Cyclic operator-() const
    Inverse.
    Definition Cyclic.h:56
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ +
    Calibration used by Bundler.
    Definition Cal3Bundler.h:32
    │ │ │ │ +
    double k1() const
    distorsion parameter k1
    Definition Cal3Bundler.h:87
    │ │ │ │ +
    double k2() const
    distorsion parameter k2
    Definition Cal3Bundler.h:90
    │ │ │ │ +
    Cal3Bundler()=default
    Default constructor.
    │ │ │ │ +
    Cal3Bundler retract(const Vector &d) const
    Update calibration with tangent space delta.
    Definition Cal3Bundler.h:153
    │ │ │ │ +
    double py() const
    image center in y
    Definition Cal3Bundler.h:96
    │ │ │ │ +
    Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
    Constructor.
    Definition Cal3Bundler.h:62
    │ │ │ │ +
    Vector3 localCoordinates(const Cal3Bundler &T2) const
    Calculate local coordinates to another calibration.
    Definition Cal3Bundler.h:158
    │ │ │ │ +
    double px() const
    image center in x
    Definition Cal3Bundler.h:93
    │ │ │ │ +
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3Bundler.h:147
    │ │ │ │ +
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3Bundler.h:150
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,118 +1,196 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cyclic.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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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 │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ +_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_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_:_:_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) │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _C_y_c_l_i_c_._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/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/PinholePose.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3DS2.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    PinholePose.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Pinhole camera with known calibration. │ │ │ │ +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 Cal3DS2 &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    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,24 +1,37 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ 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) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 28, 2010 │ │ │ │ │ +Pinhole camera with known calibration. │ │ │ │ │ Author │ │ │ │ │ - ydjian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 20, 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2_._c_p_p │ │ │ │ │ + * _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/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/SO3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,117 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    Cal3Bundler.cpp File Reference
    │ │ │ │ +
    SO3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    3*3 matrix representation of SO(3) │ │ │ │ +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

    │ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Bundler &cal)
     
    │ │ │ │ +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

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

    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 SO3R,
    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,23 +1,61 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSOO33 = _S_O< 3 > │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_B_u_n_d_l_e_r &cal) │ │ │ │ │ +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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Sep 25, 2010 │ │ │ │ │ +3*3 matrix representation of SO(3) │ │ │ │ │ Author │ │ │ │ │ - ydjian │ │ │ │ │ + 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_3_B_u_n_d_l_e_r_._c_p_p │ │ │ │ │ + * _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/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/Cal3Fisheye.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Pose2.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    Cal3Fisheye.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    2D Pose │ │ │ │ +

    Calibration of a fisheye camera. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::Cal3Fisheye
     Calibration of a fisheye camera. More...
     
    struct  gtsam::traits< Cal3Fisheye >
     
    struct  gtsam::traits< const Cal3Fisheye >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ Namespaces

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

    │ │ │ │ -Functions

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

    Detailed Description

    │ │ │ │ -

    2D Pose

    │ │ │ │ +

    Calibration of a fisheye camera.

    │ │ │ │ +
    Date
    Apr 8, 2020
    │ │ │ │ +
    Author
    ghaggin
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_<_ _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_ _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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_o_s_e_2 &pose) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Pose │ │ │ │ │ +Calibration of a fisheye camera. │ │ │ │ │ + Date │ │ │ │ │ + Apr 8, 2020 │ │ │ │ │ + Author │ │ │ │ │ + ghaggin │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_2_._c_p_p │ │ │ │ │ + * _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/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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    SphericalCamera.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Pose2.cpp 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::SphericalCamera
     A spherical camera class that has a Pose3 and measures bearing vectors. More...
     
    struct  gtsam::traits< SphericalCamera >
     
    struct  gtsam::traits< const SphericalCamera >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

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

    Detailed Description

    │ │ │ │ -

    Calibrated camera with spherical projection.

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

    2D Pose

    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 │ │ │ │ │ -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_:_:_E_m_p_t_y_C_a_l │ │ │ │ │ -  Empty calibration. _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_:_:_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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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_._._. │ │ │ │ │ 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) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera with spherical projection. │ │ │ │ │ - Date │ │ │ │ │ - Aug 26, 2021 │ │ │ │ │ - Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ +2D Pose │ │ │ │ │ * _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_._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/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/Cyclic.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PinholeCamera.h File Reference
    │ │ │ │ +
    Cyclic.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Cyclic group implementation. │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Base class for all pinhole cameras.

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

    Cyclic group implementation.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ +Cyclic group implementation. │ │ │ │ │ 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_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/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/Cal3Bundler.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    SOn.cpp File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3Bundler.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Definitions of dynamic specializations of SO(n) │ │ │ │ -More...

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

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

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

    Detailed Description

    │ │ │ │ -

    Definitions of dynamic specializations of SO(n)

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    Date
    March 2019
    │ │ │ │ +
    Date
    Sep 25, 2010
    │ │ │ │ +
    Author
    ydjian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,23 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_B_u_n_d_l_e_r &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Definitions of dynamic specializations of SO(n) │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Sep 25, 2010 │ │ │ │ │ + Author │ │ │ │ │ + ydjian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_._c_p_p │ │ │ │ │ + * _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/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/StereoCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,75 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    Pose3.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    StereoCamera.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    3D Pose │ │ │ │ +

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Pose3
     A 3D pose (R,t) : (Rot3,Point3) More...
    class  gtsam::StereoCheiralityException
     
    struct  gtsam::Pose3::ChartAtOrigin
    class  gtsam::StereoCamera
     A stereo camera class, parameterize by left camera pose and stereo calibration. More...
     
    struct  gtsam::traits< Pose3 >
    struct  gtsam::traits< StereoCamera >
     
    struct  gtsam::traits< const Pose3 >
     
    struct  gtsam::Bearing< Pose3, Point3 >
     
    struct  gtsam::Bearing< Pose3, Pose3 >
     
    struct  gtsam::Range< Pose3, T >
    struct  gtsam::traits< const StereoCamera >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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< Pose3gtsam::Pose3Vector
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Functions

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

    Detailed Description

    │ │ │ │ -

    3D Pose

    │ │ │ │ +

    A Stereo Camera based on two Simple Cameras.

    │ │ │ │ +

    A Rectified Stereo Camera.

    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +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 │ │ │ │ │ -Pose3.h File Reference │ │ │ │ │ -3D Pose _M_o_r_e_._._. │ │ │ │ │ +_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_:_:_P_o_s_e_3 │ │ │ │ │ -  A 3D pose (R,t) : (_R_o_t_3,Point3) _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_:_:_P_o_s_e_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_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_<_ _P_o_s_e_3_ _> │ │ │ │ │ +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_ _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_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -3D Pose │ │ │ │ │ +A Stereo Camera based on two Simple Cameras. │ │ │ │ │ +A Rectified Stereo Camera. │ │ │ │ │ + Author │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_3_._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/a00341.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,9 +1,5 @@ │ │ │ │ │ var a00341 = [ │ │ │ │ │ - ["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 >", "a00341.html#aefb606eb24889c0f418acf1515a2c723", 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/a00341_source.html │ │ │ │ @@ -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/StereoCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,334 +98,222 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Pose3.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
    │ │ │ │ -
    17// \callgraph
    │ │ │ │
    18#pragma once
    │ │ │ │
    19
    │ │ │ │ -
    20#include <gtsam/config.h>
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <gtsam/geometry/Rot3.h>
    │ │ │ │ -
    25#include <gtsam/base/Lie.h>
    │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    29class Pose2;
    │ │ │ │ -
    30// forward declare
    │ │ │ │ -
    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_;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    │ │ │ │ +
    26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
    │ │ │ │ +
    27public:
    │ │ │ │ + │ │ │ │ +
    29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
    │ │ │ │ +
    30
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ +
    56 typedef StereoPoint2Vector MeasurementVector;
    │ │ │ │ +
    57
    │ │ │ │ +
    58private:
    │ │ │ │ +
    59 Pose3 leftCamPose_;
    │ │ │ │ +
    60 Cal3_S2Stereo::shared_ptr K_;
    │ │ │ │
    61
    │ │ │ │ -
    │ │ │ │ -
    63 Pose3(const Rot3& R, const Point3& t) :
    │ │ │ │ -
    64 R_(R), t_(t) {
    │ │ │ │ -
    65 }
    │ │ │ │ -
    │ │ │ │ -
    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)) {
    │ │ │ │ +
    62public:
    │ │ │ │ +
    63
    │ │ │ │ +
    64 enum {
    │ │ │ │ +
    65 dimension = 6
    │ │ │ │ +
    66 };
    │ │ │ │ +
    67
    │ │ │ │ +
    70
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    73 K_(new Cal3_S2Stereo()) {
    │ │ │ │
    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);
    │ │ │ │ +
    77 StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
    │ │ │ │ +
    78
    │ │ │ │ +
    │ │ │ │ +
    80 const Cal3_S2Stereo& calibration() const {
    │ │ │ │ +
    81 return *K_;
    │ │ │ │ +
    82 }
    │ │ │ │ +
    │ │ │ │ +
    83
    │ │ │ │
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    117
    │ │ │ │ -
    132 Pose3 interpolateRt(const Pose3& T, double t) const;
    │ │ │ │ -
    133
    │ │ │ │ +
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    139 static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
    │ │ │ │ +
    139 StereoPoint2 project(const Point3& point) const;
    │ │ │ │
    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);
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    171 boost::none) const;
    │ │ │ │ +
    172
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    175 return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
    │ │ │ │ +
    176 }
    │ │ │ │ +
    │ │ │ │ +
    177
    │ │ │ │ +
    179
    │ │ │ │ +
    180private:
    │ │ │ │
    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);
    │ │ │ │ +
    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 // 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);
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    189};
    │ │ │ │ +
    │ │ │ │ +
    190
    │ │ │ │ +
    191template<>
    │ │ │ │ +
    │ │ │ │ +
    192struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
    │ │ │ │ +
    193};
    │ │ │ │ +
    │ │ │ │ +
    194
    │ │ │ │ +
    195template<>
    │ │ │ │ +
    │ │ │ │ +
    196struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
    │ │ │ │ +
    197};
    │ │ │ │ +
    │ │ │ │ +
    198}
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration + Stereo baseline.
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    A 2D stereo point (uL,uR,v)
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Matrix wedge< Pose3 >(const Vector &xi)
    wedge for Pose3:
    Definition Pose3.h:417
    │ │ │ │ +
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Definition BearingRange.h:34
    │ │ │ │ -
    Definition BearingRange.h:40
    │ │ │ │ -
    Definition BearingRange.h:180
    │ │ │ │ -
    Definition BearingRange.h:194
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    static Pose3 Identity()
    identity for group operation
    Definition Pose3.h:106
    │ │ │ │ -
    Pose3(const Pose3 &pose)
    Copy constructor.
    Definition Pose3.h:58
    │ │ │ │ -
    Pose3()
    Default constructor is origin.
    Definition Pose3.h:55
    │ │ │ │ -
    static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
    wedge for Pose3:
    Definition Pose3.h:233
    │ │ │ │ -
    Pose3(const Rot3 &R, const Point3 &t)
    Construct from R,t.
    Definition Pose3.h:63
    │ │ │ │ -
    Pose3(const Matrix &T)
    Constructor from 4*4 matrix.
    Definition Pose3.h:71
    │ │ │ │ -
    double z() const
    get z
    Definition Pose3.h:302
    │ │ │ │ -
    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
    │ │ │ │ -
    Pose3 operator*(const Pose3 &T) const
    compose syntactic sugar
    Definition Pose3.h:114
    │ │ │ │ -
    Rot3 Rotation
    Pose Concept requirements.
    Definition Pose3.h:41
    │ │ │ │ -
    double y() const
    get y
    Definition Pose3.h:297
    │ │ │ │ -
    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
    │ │ │ │ -
    Point3 operator*(const Point3 &point) const
    syntactic sugar for transformFrom
    Definition Pose3.h:260
    │ │ │ │ -
    double x() const
    get x
    Definition Pose3.h:292
    │ │ │ │ -
    Definition Pose3.h:207
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    bool equals(const Pose3 &pose, double tol=1e-9) const
    assert equality up to a tolerance
    Definition Pose3.cpp:157
    │ │ │ │ +
    void print(const std::string &s="") const
    print with optional string
    Definition Pose3.cpp:152
    │ │ │ │ +
    Definition StereoCamera.h:26
    │ │ │ │ +
    A stereo camera class, parameterize by left camera pose and stereo calibration.
    Definition StereoCamera.h:47
    │ │ │ │ +
    StereoCamera()
    Default constructor allocates a calibration!
    Definition StereoCamera.h:72
    │ │ │ │ +
    static size_t Dim()
    Dimensionality of the tangent space.
    Definition StereoCamera.h:110
    │ │ │ │ +
    void print(const std::string &s="") const
    print
    Definition StereoCamera.h:89
    │ │ │ │ +
    Vector6 localCoordinates(const StereoCamera &t2) const
    Local coordinates of manifold neighborhood around current value.
    Definition StereoCamera.h:120
    │ │ │ │ +
    bool equals(const StereoCamera &camera, double tol=1e-9) const
    equals
    Definition StereoCamera.h:95
    │ │ │ │ +
    size_t dim() const
    Dimensionality of the tangent space.
    Definition StereoCamera.h:105
    │ │ │ │ +
    StereoCamera retract(const Vector &v) const
    Updates a with tangent space delta.
    Definition StereoCamera.h:115
    │ │ │ │ +
    StereoPoint2 Measurement
    Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
    Definition StereoCamera.h:55
    │ │ │ │ +
    const Cal3_S2Stereo & calibration() const
    Return shared pointer to calibration.
    Definition StereoCamera.h:80
    │ │ │ │ +
    double baseline() const
    baseline
    Definition StereoCamera.h:134
    │ │ │ │ +
    const Pose3 & pose() const
    pose
    Definition StereoCamera.h:129
    │ │ │ │ +
    Vector defaultErrorWhenTriangulatingBehindCamera() const
    for Nonlinear Triangulation
    Definition StereoCamera.h:174
    │ │ │ │ +
    A 2D stereo point, v will be same for rectified images.
    Definition StereoPoint2.h:32
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,402 +1,265 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Pose3.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 │ │ │ │ │ -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 │ │ │ │ │ -_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_; │ │ │ │ │ +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) {} │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_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 │ │ │ │ │ -_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 │ │ │ │ │ -_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)) { │ │ │ │ │ +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 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); │ │ │ │ │ +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 │ │ │ │ │ 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 │ │ │ │ │ -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 } │ │ │ │ │ -117 │ │ │ │ │ -132 _P_o_s_e_3 interpolateRt(const _P_o_s_e_3& T, double t) const; │ │ │ │ │ -133 │ │ │ │ │ +_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 } │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 // 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); │ │ │ │ │ -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(); │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ +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) │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _P_o_s_e_3_._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/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/SO4.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,95 +98,67 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    Point3.h File Reference
    │ │ │ │ +
    SO4.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    3D Point │ │ │ │ +

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::Range< Point3, Point3 >
    struct  gtsam::traits< SO4 >
     
    struct  gtsam::traits< const SO4 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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 >
     
    │ │ │ │ +using gtsam::SO4 = SO< 4 >
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ 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.
     
    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

    │ │ │ │ -

    3D Point

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

    4*4 matrix representation of SO(4)

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    Date
    March 2019
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,72 +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 │ │ │ │ │ -Point3.h File Reference │ │ │ │ │ -3D Point _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_i_n_t_3_,_ _P_o_i_n_t_3_ _> │ │ │ │ │ +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_o_n_s_t_ _S_O_4_ _> │ │ │ │ │   │ │ │ │ │ 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 > │ │ │ │ │ +using  ggttssaamm::::SSOO44 = _S_O< 4 > │ │ │ │ │   │ │ │ │ │ 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. │ │ │ │ │ + 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 ********** │ │ │ │ │ -3D Point │ │ │ │ │ +4*4 matrix representation of SO(4) │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_3_._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/a00344.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,7 @@ │ │ │ │ │ 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] │ │ │ │ │ + ["gtsam::traits< SO4 >", "a03296.html", null], │ │ │ │ │ + ["gtsam::traits< const SO4 >", "a03300.html", null], │ │ │ │ │ + ["serialize", "a00344.html#a76855a1214bf05a8704ee3deaece6339", null], │ │ │ │ │ + ["stiefel", "a00344.html#a3e57e4771f13855a495ec2c6454c9121", null], │ │ │ │ │ + ["topLeft", "a00344.html#a8d21728e3da4cf1a96e1b6f87a97da47", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00344_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,141 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Point3.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
    │ │ │ │ -
    20// \callgraph
    │ │ │ │ +
    20#pragma once
    │ │ │ │
    21
    │ │ │ │ -
    22#pragma once
    │ │ │ │ +
    22#include <gtsam/geometry/SOn.h>
    │ │ │ │
    23
    │ │ │ │ -
    24#include <gtsam/config.h>
    │ │ │ │ -
    25#include <gtsam/base/VectorSpace.h>
    │ │ │ │ -
    26#include <gtsam/base/Vector.h>
    │ │ │ │ -
    27#include <gtsam/dllexport.h>
    │ │ │ │ - │ │ │ │ -
    29#include <boost/serialization/nvp.hpp>
    │ │ │ │ -
    30#include <numeric>
    │ │ │ │ +
    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
    │ │ │ │ -
    36typedef Vector3 Point3;
    │ │ │ │ -
    37typedef std::vector<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
    │ │ │ │ +
    34using SO4 = SO<4>;
    │ │ │ │ +
    35
    │ │ │ │ +
    36// /// Random SO(4) element (no big claims about uniformity)
    │ │ │ │ +
    37// static SO4 Random(std::mt19937 &rng);
    │ │ │ │
    38
    │ │ │ │ -
    39// Convenience typedef
    │ │ │ │ -
    40using Point3Pair = std::pair<Point3, Point3>;
    │ │ │ │ -
    41GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
    │ │ │ │ -
    42
    │ │ │ │ -
    43using Point3Pairs = std::vector<Point3Pair>;
    │ │ │ │ -
    44
    │ │ │ │ -
    46GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
    │ │ │ │ -
    47 OptionalJacobian<1, 3> H1 = boost::none,
    │ │ │ │ -
    48 OptionalJacobian<1, 3> H2 = boost::none);
    │ │ │ │ +
    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
    │ │ │ │ -
    51GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
    │ │ │ │ -
    52
    │ │ │ │ -
    54GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
    │ │ │ │ -
    55
    │ │ │ │ -
    57GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
    │ │ │ │ -
    58 OptionalJacobian<3, 3> H_p = boost::none,
    │ │ │ │ -
    59 OptionalJacobian<3, 3> H_q = boost::none);
    │ │ │ │ -
    60
    │ │ │ │ -
    62GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
    │ │ │ │ -
    63 OptionalJacobian<1, 3> H_p = boost::none,
    │ │ │ │ -
    64 OptionalJacobian<1, 3> H_q = boost::none);
    │ │ │ │ +
    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
    │ │ │ │ -
    67template <class CONTAINER>
    │ │ │ │ -
    │ │ │ │ -
    68Point3 mean(const CONTAINER& points) {
    │ │ │ │ -
    69 if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
    │ │ │ │ -
    70 Point3 sum(0, 0, 0);
    │ │ │ │ -
    71 sum = std::accumulate(points.begin(), points.end(), sum);
    │ │ │ │ -
    72 return sum / points.size();
    │ │ │ │ -
    73}
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    76GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
    │ │ │ │ -
    77
    │ │ │ │ -
    78template <typename A1, typename A2>
    │ │ │ │ -
    79struct Range;
    │ │ │ │ +
    79GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
    │ │ │ │
    80
    │ │ │ │ -
    81template <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    83 typedef double result_type;
    │ │ │ │ -
    84 double operator()(const Point3& p, const Point3& q,
    │ │ │ │ -
    85 OptionalJacobian<1, 3> H1 = boost::none,
    │ │ │ │ -
    86 OptionalJacobian<1, 3> H2 = boost::none) {
    │ │ │ │ -
    87 return distance3(p, q, H1, H2);
    │ │ │ │ -
    88 }
    │ │ │ │ -
    89};
    │ │ │ │ -
    │ │ │ │ -
    90
    │ │ │ │ -
    91} // namespace gtsam
    │ │ │ │ -
    92
    │ │ │ │ -
    serialization for Vectors
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Concept check class for variable types with Group properties.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    N*N matrix representation of SO(N).
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 mean(const CONTAINER &points)
    mean
    Definition Point3.h:68
    │ │ │ │ -
    Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
    cross product
    Definition Point3.cpp:64
    │ │ │ │ -
    Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
    Calculate the two means of a set of Point2 pairs.
    Definition Point2.cpp:116
    │ │ │ │ -
    double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
    distance between two points
    Definition Point3.cpp:27
    │ │ │ │ -
    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
    │ │ │ │ -
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ -
    double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
    Distance of the point from the origin, with Jacobian.
    Definition Point3.cpp:41
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Definition BearingRange.h:40
    │ │ │ │ +
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ +
    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_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
    Project to top-left 3*3 matrix.
    Definition SO4.cpp:206
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ +
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ +
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ +
    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,140 +1,178 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Point3.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 │ │ │ │ │ -20// \callgraph │ │ │ │ │ +20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#pragma once │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ 23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -27#include │ │ │ │ │ -28#include <_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> │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ +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 │ │ │ │ │ -_3_6typedef Vector3 _P_o_i_n_t_3; │ │ │ │ │ -37typedef std::vector > Point3Vector; │ │ │ │ │ +34using SO4 = SO<4>; │ │ │ │ │ +35 │ │ │ │ │ +36// /// Random SO(4) element (no big claims about uniformity) │ │ │ │ │ +37// static SO4 Random(std::mt19937 &rng); │ │ │ │ │ 38 │ │ │ │ │ -39// Convenience typedef │ │ │ │ │ -40using Point3Pair = std::pair; │ │ │ │ │ -41GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam:: │ │ │ │ │ -Point3Pair &p); │ │ │ │ │ -42 │ │ │ │ │ -43using Point3Pairs = std::vector; │ │ │ │ │ -44 │ │ │ │ │ -46GTSAM_EXPORT double _d_i_s_t_a_n_c_e_3(const _P_o_i_n_t_3& p1, const _P_o_i_n_t_3& q, │ │ │ │ │ -47 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1 = boost::none, │ │ │ │ │ -48 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2 = boost::none); │ │ │ │ │ +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 │ │ │ │ │ -51GTSAM_EXPORT double _n_o_r_m_3(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); │ │ │ │ │ -52 │ │ │ │ │ -54GTSAM_EXPORT _P_o_i_n_t_3 _n_o_r_m_a_l_i_z_e(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); │ │ │ │ │ -55 │ │ │ │ │ -57GTSAM_EXPORT _P_o_i_n_t_3 _c_r_o_s_s(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ -58 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H_p = boost::none, │ │ │ │ │ -59 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H_q = boost::none); │ │ │ │ │ -60 │ │ │ │ │ -62GTSAM_EXPORT double _d_o_t(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ -63 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H_p = boost::none, │ │ │ │ │ -64 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H_q = boost::none); │ │ │ │ │ +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 │ │ │ │ │ -67template │ │ │ │ │ -_6_8_P_o_i_n_t_3 _m_e_a_n(const CONTAINER& points) { │ │ │ │ │ -69 if (points.size() == 0) throw std::invalid_argument("Point3::mean input │ │ │ │ │ -container is empty"); │ │ │ │ │ -70 _P_o_i_n_t_3 sum(0, 0, 0); │ │ │ │ │ -71 sum = std::accumulate(points.begin(), points.end(), sum); │ │ │ │ │ -72 return sum / points.size(); │ │ │ │ │ -73} │ │ │ │ │ +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 │ │ │ │ │ -76GTSAM_EXPORT Point3Pair _m_e_a_n_s(const std::vector &abPointPairs); │ │ │ │ │ -77 │ │ │ │ │ -78template │ │ │ │ │ -79struct Range; │ │ │ │ │ +79GTSAM_EXPORT Matrix43 _s_t_i_e_f_e_l(const SO4 &Q, OptionalJacobian<12, 6> H = │ │ │ │ │ +boost::none); │ │ │ │ │ 80 │ │ │ │ │ -81template <> │ │ │ │ │ -_8_2struct _R_a_n_g_e<_P_o_i_n_t_3, _P_o_i_n_t_3> { │ │ │ │ │ -83 typedef double result_type; │ │ │ │ │ -84 double operator()(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ -85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1 = boost::none, │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +_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). │ │ │ │ │ _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_:_:_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_:_:_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_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_:_:_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_:_:_n_o_r_m_3 │ │ │ │ │ -double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H) │ │ │ │ │ -Distance of the point from the origin, with Jacobian. │ │ │ │ │ -DDeeffiinniittiioonn Point3.cpp:41 │ │ │ │ │ -_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_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_:_:_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_:_:_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 │ │ │ │ │ - * _P_o_i_n_t_3_._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/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/Similarity2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,53 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Rot3.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    Similarity2.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::Similarity2
     2D similarity transform More...
     
    struct  gtsam::Similarity2::ChartAtOrigin
     Chart at the origin. More...
     
    struct  gtsam::traits< Similarity2 >
     
    struct  gtsam::traits< const Similarity2 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Rotation, common code between Rotation matrix and Quaternion.

    │ │ │ │ -
    Author
    Alireza Fathi
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +

    Implementation of Similarity2 transform.

    │ │ │ │ +
    Author
    John Lambert, Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -Rot3.cpp File Reference │ │ │ │ │ -Rotation, common code between Rotation matrix and Quaternion. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Similarity2.h File Reference │ │ │ │ │ +Implementation of Similarity2 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_i_m_i_l_a_r_i_t_y_2 │ │ │ │ │ +  2D similarity transform _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_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_i_m_i_l_a_r_i_t_y_2_ _> │ │ │ │ │ +  │ │ │ │ │ +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_2_ _> │ │ │ │ │ +  │ │ │ │ │ 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) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ │ +Implementation of Similarity2 transform. │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + John Lambert, Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_._c_p_p │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_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/a00347.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00347 = [ │ │ │ │ │ - ["RQ", "a00347.html#a5172b5b6d51bd5348c7e551e1376f60a", null] │ │ │ │ │ + ["gtsam::Similarity2", "a03248.html", "a03248"], │ │ │ │ │ + ["gtsam::Similarity2::ChartAtOrigin", "a03252.html", null], │ │ │ │ │ + ["gtsam::traits< Similarity2 >", "a03256.html", null], │ │ │ │ │ + ["gtsam::traits< const Similarity2 >", "a03260.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00353_source.html │ │ │ │ @@ -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/Unit3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,177 +98,245 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3.h
    │ │ │ │ +
    Unit3.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
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    12/*
    │ │ │ │ +
    13 * @file Unit3.h
    │ │ │ │ +
    14 * @date Feb 02, 2011
    │ │ │ │ +
    15 * @author Can Erdogan
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    26#include <gtsam/base/Vector.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
    │ │ │ │ +
    36#ifdef GTSAM_USE_TBB
    │ │ │ │ +
    37#include <mutex> // std::mutex
    │ │ │ │ +
    38#endif
    │ │ │ │ +
    39
    │ │ │ │ +
    40namespace gtsam {
    │ │ │ │ +
    41
    │ │ │ │ +
    │ │ │ │ +
    43class GTSAM_EXPORT Unit3 {
    │ │ │ │ +
    44
    │ │ │ │ +
    45private:
    │ │ │ │ +
    46
    │ │ │ │ +
    47 Vector3 p_;
    │ │ │ │ +
    48 mutable boost::optional<Matrix32> B_;
    │ │ │ │ +
    49 mutable boost::optional<Matrix62> H_B_;
    │ │ │ │ +
    50
    │ │ │ │ +
    51#ifdef GTSAM_USE_TBB
    │ │ │ │ +
    52 mutable std::mutex B_mutex_;
    │ │ │ │ +
    53#endif
    │ │ │ │ +
    54
    │ │ │ │ +
    55public:
    │ │ │ │
    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}
    │ │ │ │ -
    │ │ │ │ +
    57 enum {
    │ │ │ │ +
    58 dimension = 2
    │ │ │ │ +
    59 };
    │ │ │ │ +
    60
    │ │ │ │
    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;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    66 p_(1.0, 0.0, 0.0) {
    │ │ │ │ +
    67 }
    │ │ │ │ +
    │ │ │ │ +
    68
    │ │ │ │ +
    70 explicit Unit3(const Vector3& p);
    │ │ │ │ +
    71
    │ │ │ │ +
    73 Unit3(double x, double y, double z);
    │ │ │ │
    74
    │ │ │ │ -
    75 public:
    │ │ │ │ -
    76 enum { dimension = 5 };
    │ │ │ │ -
    78 using shared_ptr = boost::shared_ptr<Cal3>;
    │ │ │ │ -
    79
    │ │ │ │ -
    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
    │ │ │ │ +
    77 explicit Unit3(const Point2& p, double f);
    │ │ │ │ +
    78
    │ │ │ │ +
    │ │ │ │ +
    80 Unit3(const Unit3& u) {
    │ │ │ │ +
    81 p_ = u.p_;
    │ │ │ │ +
    82 }
    │ │ │ │ +
    │ │ │ │ +
    83
    │ │ │ │ +
    │ │ │ │ +
    85 Unit3& operator=(const Unit3 & u) {
    │ │ │ │ +
    86 p_ = u.p_;
    │ │ │ │ +
    87 B_ = u.B_;
    │ │ │ │ +
    88 H_B_ = u.H_B_;
    │ │ │ │ +
    89 return *this;
    │ │ │ │ +
    90 }
    │ │ │ │ +
    │ │ │ │ +
    91
    │ │ │ │ +
    93 static Unit3 FromPoint3(const Point3& point, //
    │ │ │ │ +
    94 OptionalJacobian<2, 3> H = boost::none);
    │ │ │ │ +
    95
    │ │ │ │ +
    102 static Unit3 Random(std::mt19937 & rng);
    │ │ │ │ +
    103
    │ │ │ │ +
    105
    │ │ │ │
    108
    │ │ │ │ -
    118 Cal3(const std::string& path);
    │ │ │ │ +
    109 friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
    │ │ │ │ +
    110
    │ │ │ │ +
    112 void print(const std::string& s = std::string()) const;
    │ │ │ │ +
    113
    │ │ │ │ +
    │ │ │ │ +
    115 bool equals(const Unit3& s, double tol = 1e-9) const {
    │ │ │ │ +
    116 return equal_with_abs_tol(p_, s.p_, tol);
    │ │ │ │ +
    117 }
    │ │ │ │ +
    │ │ │ │
    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;
    │ │ │ │ +
    122
    │ │ │ │ +
    129 const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
    │ │ │ │
    130
    │ │ │ │ -
    132 bool equals(const Cal3& K, double tol = 10e-9) const;
    │ │ │ │ +
    132 Matrix3 skew() 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_; }
    │ │ │ │ +
    135 Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
    │ │ │ │ +
    136
    │ │ │ │ +
    138 Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
    │ │ │ │ +
    139
    │ │ │ │ +
    │ │ │ │ +
    141 friend Point3 operator*(double s, const Unit3& d) {
    │ │ │ │ +
    142 return Point3(s * d.p_);
    │ │ │ │ +
    143 }
    │ │ │ │ +
    │ │ │ │ +
    144
    │ │ │ │ +
    146 double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
    │ │ │ │ +
    147 OptionalJacobian<1,2> H2 = boost::none) const;
    │ │ │ │ +
    148
    │ │ │ │ +
    151 Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
    │ │ │ │
    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;
    │ │ │ │ +
    155 Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
    │ │ │ │ +
    156 OptionalJacobian<2, 2> H_q = boost::none) const;
    │ │ │ │ +
    157
    │ │ │ │ +
    159 double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
    │ │ │ │ +
    160
    │ │ │ │ +
    │ │ │ │ +
    162 Unit3 cross(const Unit3& q) const {
    │ │ │ │ +
    163 return Unit3(p_.cross(q.p_));
    │ │ │ │
    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 }
    │ │ │ │ +
    167 Point3 cross(const Point3& q) const {
    │ │ │ │ +
    168 return point3().cross(q);
    │ │ │ │ +
    169 }
    │ │ │ │
    │ │ │ │ +
    170
    │ │ │ │
    172
    │ │ │ │ -
    173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
    │ │ │ │ -
    175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); }
    │ │ │ │ -
    176#endif
    │ │ │ │ -
    177
    │ │ │ │ -
    179 Matrix3 inverse() const;
    │ │ │ │ +
    175
    │ │ │ │ +
    │ │ │ │ +
    177 inline static size_t Dim() {
    │ │ │ │ +
    178 return 2;
    │ │ │ │ +
    179 }
    │ │ │ │ +
    │ │ │ │
    180
    │ │ │ │ -
    182 inline virtual size_t dim() const { return Dim(); }
    │ │ │ │ -
    183
    │ │ │ │ -
    185 inline static size_t Dim() { return dimension; }
    │ │ │ │ -
    186
    │ │ │ │ +
    │ │ │ │ +
    182 inline size_t dim() const {
    │ │ │ │ +
    183 return 2;
    │ │ │ │ +
    184 }
    │ │ │ │ +
    │ │ │ │ +
    185
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    188 RENORM
    │ │ │ │ +
    189 };
    │ │ │ │ +
    │ │ │ │
    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
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    192 Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
    │ │ │ │ +
    193
    │ │ │ │ +
    195 Vector2 localCoordinates(const Unit3& s) const;
    │ │ │ │ +
    196
    │ │ │ │ +
    198
    │ │ │ │ +
    199private:
    │ │ │ │ +
    200
    │ │ │ │ +
    203
    │ │ │ │ +
    204 friend class boost::serialization::access;
    │ │ │ │ +
    205 template<class ARCHIVE>
    │ │ │ │ +
    206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    207 ar & BOOST_SERIALIZATION_NVP(p_);
    │ │ │ │ +
    208 }
    │ │ │ │ +
    209
    │ │ │ │ +
    211
    │ │ │ │ +
    212public:
    │ │ │ │ + │ │ │ │ +
    214};
    │ │ │ │ +
    │ │ │ │ +
    215
    │ │ │ │ +
    216// Define GTSAM traits
    │ │ │ │ +
    │ │ │ │ +
    217template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
    │ │ │ │ +
    218};
    │ │ │ │ +
    │ │ │ │ +
    219
    │ │ │ │ +
    │ │ │ │ +
    220template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
    │ │ │ │ +
    221};
    │ │ │ │ +
    │ │ │ │ +
    222
    │ │ │ │ +
    223} // namespace gtsam
    │ │ │ │ +
    224
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    serialization for Vectors
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ -
    Cal3(double fx, double fy, double s, double u0, double v0)
    constructor from doubles
    Definition Cal3.h:87
    │ │ │ │ -
    virtual Matrix3 K() const
    return calibration matrix K
    Definition Cal3.h:167
    │ │ │ │ -
    Cal3(const Vector5 &d)
    constructor from vector
    Definition Cal3.h:91
    │ │ │ │ -
    virtual ~Cal3()
    Virtual destructor.
    Definition Cal3.h:103
    │ │ │ │ -
    Vector5 vector() const
    vectorized form (column-wise)
    Definition Cal3.h:160
    │ │ │ │ -
    Cal3()=default
    Create a default calibration that leaves coordinates unchanged.
    │ │ │ │ -
    double px() const
    image center in x
    Definition Cal3.h:151
    │ │ │ │ -
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3.h:185
    │ │ │ │ -
    Point2 principalPoint() const
    return the principal point
    Definition Cal3.h:157
    │ │ │ │ -
    double fx() const
    focal length x
    Definition Cal3.h:139
    │ │ │ │ -
    double py() const
    image center in y
    Definition Cal3.h:154
    │ │ │ │ -
    double skew() const
    skew
    Definition Cal3.h:148
    │ │ │ │ -
    virtual size_t dim() const
    return DOF, dimensionality of tangent space
    Definition Cal3.h:182
    │ │ │ │ -
    double aspectRatio() const
    aspect ratio
    Definition Cal3.h:145
    │ │ │ │ -
    double fy() const
    focal length y
    Definition Cal3.h:142
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Unit3(const Unit3 &u)
    Copy constructor.
    Definition Unit3.h:80
    │ │ │ │ +
    Unit3()
    Default constructor.
    Definition Unit3.h:65
    │ │ │ │ +
    friend Point3 operator*(double s, const Unit3 &d)
    Return scaled direction as Point3.
    Definition Unit3.h:141
    │ │ │ │ +
    static size_t Dim()
    Dimensionality of tangent space = 2 DOF.
    Definition Unit3.h:177
    │ │ │ │ +
    size_t dim() const
    Dimensionality of tangent space = 2 DOF.
    Definition Unit3.h:182
    │ │ │ │ +
    bool equals(const Unit3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition Unit3.h:115
    │ │ │ │ +
    CoordinatesMode
    Definition Unit3.h:186
    │ │ │ │ +
    @ EXPMAP
    Use the exponential map to retract.
    Definition Unit3.h:187
    │ │ │ │ +
    Unit3 cross(const Unit3 &q) const
    Cross-product between two Unit3s.
    Definition Unit3.h:162
    │ │ │ │ +
    Point3 cross(const Point3 &q) const
    Cross-product w Point3.
    Definition Unit3.h:167
    │ │ │ │ +
    Unit3 & operator=(const Unit3 &u)
    Copy assignment.
    Definition Unit3.h:85
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,224 +1,285 @@ │ │ │ │ │ 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_. │ │ │ │ │ +Unit3.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; │ │ │ │ │ -53 │ │ │ │ │ -54 // Compute uncalibrate Jacobians │ │ │ │ │ -55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file Unit3.h │ │ │ │ │ +14 * @date Feb 02, 2011 │ │ │ │ │ +15 * @author Can Erdogan │ │ │ │ │ +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 <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +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_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +27#include <_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> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35 │ │ │ │ │ +36#ifdef GTSAM_USE_TBB │ │ │ │ │ +37#include // std::mutex │ │ │ │ │ +38#endif │ │ │ │ │ +39 │ │ │ │ │ +40namespace _g_t_s_a_m { │ │ │ │ │ +41 │ │ │ │ │ +_4_3class GTSAM_EXPORT _U_n_i_t_3 { │ │ │ │ │ +44 │ │ │ │ │ +45private: │ │ │ │ │ +46 │ │ │ │ │ +47 Vector3 p_; │ │ │ │ │ +48 mutable boost::optional B_; │ │ │ │ │ +49 mutable boost::optional H_B_; │ │ │ │ │ +50 │ │ │ │ │ +51#ifdef GTSAM_USE_TBB │ │ │ │ │ +52 mutable std::mutex B_mutex_; │ │ │ │ │ +53#endif │ │ │ │ │ +54 │ │ │ │ │ +55public: │ │ │ │ │ 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} │ │ │ │ │ +57 enum { │ │ │ │ │ +58 dimension = 2 │ │ │ │ │ +59 }; │ │ │ │ │ +60 │ │ │ │ │ 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; │ │ │ │ │ +_6_5 _U_n_i_t_3() : │ │ │ │ │ +66 p_(1.0, 0.0, 0.0) { │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +70 explicit _U_n_i_t_3(const Vector3& p); │ │ │ │ │ +71 │ │ │ │ │ +73 _U_n_i_t_3(double x, double y, double z); │ │ │ │ │ 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 _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 │ │ │ │ │ +77 explicit _U_n_i_t_3(const _P_o_i_n_t_2& p, double f); │ │ │ │ │ +78 │ │ │ │ │ +_8_0 _U_n_i_t_3(const _U_n_i_t_3& u) { │ │ │ │ │ +81 p_ = u.p_; │ │ │ │ │ +82 } │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _U_n_i_t_3& _o_p_e_r_a_t_o_r_=(const _U_n_i_t_3 & u) { │ │ │ │ │ +86 p_ = u.p_; │ │ │ │ │ +87 B_ = u.B_; │ │ │ │ │ +88 H_B_ = u.H_B_; │ │ │ │ │ +89 return *this; │ │ │ │ │ +90 } │ │ │ │ │ +91 │ │ │ │ │ +93 static _U_n_i_t_3 FromPoint3(const _P_o_i_n_t_3& point, // │ │ │ │ │ +94 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> H = boost::none); │ │ │ │ │ +95 │ │ │ │ │ +102 static _U_n_i_t_3 Random(std::mt19937 & rng); │ │ │ │ │ +103 │ │ │ │ │ +105 │ │ │ │ │ 108 │ │ │ │ │ -118 _C_a_l_3(const std::string& path); │ │ │ │ │ +109 friend std::ostream& operator<<(std::ostream& os, const _U_n_i_t_3& pair); │ │ │ │ │ +110 │ │ │ │ │ +112 void _p_r_i_n_t(const std::string& s = std::string()) const; │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 bool _e_q_u_a_l_s(const _U_n_i_t_3& s, double tol = 1e-9) const { │ │ │ │ │ +116 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(p_, s.p_, tol); │ │ │ │ │ +117 } │ │ │ │ │ 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; │ │ │ │ │ +122 │ │ │ │ │ +129 const Matrix32& basis(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _2_> H = boost::none) const; │ │ │ │ │ 130 │ │ │ │ │ -132 bool _e_q_u_a_l_s(const _C_a_l_3& K, double tol = 10e-9) const; │ │ │ │ │ +132 Matrix3 skew() 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_; } │ │ │ │ │ +135 _P_o_i_n_t_3 point3(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> H = boost::none) const; │ │ │ │ │ +136 │ │ │ │ │ +138 Vector3 unitVector(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> H = boost::none) const; │ │ │ │ │ +139 │ │ │ │ │ +_1_4_1 friend _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(double s, const _U_n_i_t_3& d) { │ │ │ │ │ +142 return _P_o_i_n_t_3(s * d.p_); │ │ │ │ │ +143 } │ │ │ │ │ +144 │ │ │ │ │ +146 double _d_o_t(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H1 = boost::none, // │ │ │ │ │ +147 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H2 = boost::none) const; │ │ │ │ │ +148 │ │ │ │ │ +151 Vector2 error(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_q = boost::none) │ │ │ │ │ +const; │ │ │ │ │ 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; │ │ │ │ │ +155 Vector2 errorVector(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_p = boost:: │ │ │ │ │ +none, // │ │ │ │ │ +156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_q = boost::none) const; │ │ │ │ │ +157 │ │ │ │ │ +159 double distance(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H = boost::none) │ │ │ │ │ +const; │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 _U_n_i_t_3 _c_r_o_s_s(const _U_n_i_t_3& q) const { │ │ │ │ │ +163 return _U_n_i_t_3(p_.cross(q.p_)); │ │ │ │ │ 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 } │ │ │ │ │ +_1_6_7 _P_o_i_n_t_3 _c_r_o_s_s(const _P_o_i_n_t_3& q) const { │ │ │ │ │ +168 return point3().cross(q); │ │ │ │ │ +169 } │ │ │ │ │ +170 │ │ │ │ │ 172 │ │ │ │ │ -173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); } │ │ │ │ │ -176#endif │ │ │ │ │ -177 │ │ │ │ │ -179 Matrix3 inverse() const; │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 inline static size_t _D_i_m() { │ │ │ │ │ +178 return 2; │ │ │ │ │ +179 } │ │ │ │ │ 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 │ │ │ │ │ +_1_8_2 inline size_t _d_i_m() const { │ │ │ │ │ +183 return 2; │ │ │ │ │ +184 } │ │ │ │ │ +185 │ │ │ │ │ +_1_8_6 enum _C_o_o_r_d_i_n_a_t_e_s_M_o_d_e { │ │ │ │ │ +_1_8_7 _E_X_P_M_A_P, │ │ │ │ │ +188 RENORM │ │ │ │ │ +_1_8_9 }; │ │ │ │ │ 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 │ │ │ │ │ +192 _U_n_i_t_3 retract(const Vector2& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_2_> H = boost::none) │ │ │ │ │ +const; │ │ │ │ │ +193 │ │ │ │ │ +195 Vector2 localCoordinates(const _U_n_i_t_3& s) const; │ │ │ │ │ +196 │ │ │ │ │ +198 │ │ │ │ │ +199private: │ │ │ │ │ +200 │ │ │ │ │ +203 │ │ │ │ │ +_2_0_4 friend class boost::serialization::access; │ │ │ │ │ +205 template │ │ │ │ │ +206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +207 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ +208 } │ │ │ │ │ +209 │ │ │ │ │ +211 │ │ │ │ │ +212public: │ │ │ │ │ +213 _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 │ │ │ │ │ +214}; │ │ │ │ │ +215 │ │ │ │ │ +216// Define GTSAM traits │ │ │ │ │ +_2_1_7template<> struct _t_r_a_i_t_s<_U_n_i_t_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ +_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_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 │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ _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_:_:_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_:_:_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_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_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_:_:_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_:_:_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_3 │ │ │ │ │ +Unit3(const Unit3 &u) │ │ │ │ │ +Copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_U_n_i_t_3 │ │ │ │ │ +Unit3() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +friend Point3 operator*(double s, const Unit3 &d) │ │ │ │ │ +Return scaled direction as Point3. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_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 = 2 DOF. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality of tangent space = 2 DOF. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:182 │ │ │ │ │ +_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_:_:_U_n_i_t_3_:_:_C_o_o_r_d_i_n_a_t_e_s_M_o_d_e │ │ │ │ │ +CoordinatesMode │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_E_X_P_M_A_P │ │ │ │ │ +@ EXPMAP │ │ │ │ │ +Use the exponential map to retract. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_c_r_o_s_s │ │ │ │ │ +Unit3 cross(const Unit3 &q) const │ │ │ │ │ +Cross-product between two Unit3s. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_c_r_o_s_s │ │ │ │ │ +Point3 cross(const Point3 &q) const │ │ │ │ │ +Cross-product w Point3. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +Unit3 & operator=(const Unit3 &u) │ │ │ │ │ +Copy assignment. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:85 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._h │ │ │ │ │ + * UUnniitt33..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/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/triangulation.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,192 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Cal3Fisheye.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    triangulation.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Calibration of a fisheye camera. │ │ │ │ +

    Functions for triangulation. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Cal3Fisheye
     Calibration of a fisheye camera. More...
    class  gtsam::TriangulationUnderconstrainedException
     Exception thrown by triangulateDLT when SVD returns rank < 3. More...
     
    struct  gtsam::traits< Cal3Fisheye >
    class  gtsam::TriangulationCheiralityException
     Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
     
    struct  gtsam::traits< const Cal3Fisheye >
    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, Valuesgtsam::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, Valuesgtsam::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

    │ │ │ │ -

    Calibration of a fisheye camera.

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

    Functions for triangulation.

    │ │ │ │ +
    Date
    July 31, 2013
    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │ +
    │ │ │ │ +Akshay Krishnan
    │ │ │ │ +
    Date
    July 31, 2013
    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Akshay Krishnan
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -Cal3Fisheye.h File Reference │ │ │ │ │ -Calibration of a fisheye camera. _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_:_:_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_:_:_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_:_:_t_r_a_i_t_s_<_ _C_a_l_3_F_i_s_h_e_y_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_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_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_._._. │ │ │ │ │   │ │ │ │ │ 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 ********** │ │ │ │ │ -Calibration of a fisheye camera. │ │ │ │ │ +Functions for triangulation. │ │ │ │ │ + Date │ │ │ │ │ + July 31, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Chris Beall │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - Apr 8, 2020 │ │ │ │ │ + July 31, 2013 │ │ │ │ │ Author │ │ │ │ │ - ghaggin │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Chris Beall │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00359.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,30 @@ │ │ │ │ │ var a00359 = [ │ │ │ │ │ - ["gtsam::traits< Cal3Fisheye >", "a03000.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3Fisheye >", "a03004.html", null] │ │ │ │ │ + ["gtsam::TriangulationUnderconstrainedException", "a03364.html", null], │ │ │ │ │ + ["gtsam::TriangulationCheiralityException", "a03368.html", null], │ │ │ │ │ + ["gtsam::TriangulationParameters", "a03372.html", "a03372"], │ │ │ │ │ + ["gtsam::TriangulationResult", "a03376.html", "a03376"], │ │ │ │ │ + ["calibrateMeasurements", "a00359.html#aebe53b43e5151ce9c355b4aa4ba57c13", null], │ │ │ │ │ + ["calibrateMeasurements", "a00359.html#a48ad2c1085fcae3881fa956f4cd81890", null], │ │ │ │ │ + ["calibrateMeasurementsShared", "a00359.html#a0a386184e9ed9cc4ec844d48d91ba759", null], │ │ │ │ │ + ["createPinholeCalibration", "a00359.html#a3d070e707c4b2d69d0f024a30501f06c", null], │ │ │ │ │ + ["optimize", "a00359.html#a4b0057879b0c5f323185452b0d1a4232", null], │ │ │ │ │ + ["triangulateDLT", "a00359.html#aed2918ac0049316bf09f96879efca3b6", null], │ │ │ │ │ + ["triangulateDLT", "a00359.html#a523a0528f883f1c569a8a4c9a0e514a0", null], │ │ │ │ │ + ["triangulateHomogeneousDLT", "a00359.html#af378e231b519e8bb1f09f4aa13a38dca", null], │ │ │ │ │ + ["triangulateHomogeneousDLT", "a00359.html#a8d16887f8890aaf7ceb5e3cbb181a191", null], │ │ │ │ │ + ["triangulateLOST", "a00359.html#a0f9e5111ffc481b34f0123418b0ecd13", null], │ │ │ │ │ + ["triangulateNonlinear", "a00359.html#a13ac0858b6f6600f5a4242aeb797692f", null], │ │ │ │ │ + ["triangulateNonlinear", "a00359.html#a9dab6b5829b51511735b6b841bb36a36", null], │ │ │ │ │ + ["triangulatePoint3", "a00359.html#a58f4009e3a8872d0ac6780d654cccc2e", null], │ │ │ │ │ + ["triangulatePoint3", "a00359.html#aac6df5bbfb7131cbb05e2a9691e59ba3", null], │ │ │ │ │ + ["triangulatePoint3", "a00359.html#ab79854d3cb3bcc4f562fc88a0731a447", null], │ │ │ │ │ + ["triangulateSafe", "a00359.html#a8d52d7d8a5e5c8d7318ce95ee57e9cfa", null], │ │ │ │ │ + ["triangulationGraph", "a00359.html#a2b96bc32f3c5c436db3fe60e5139319e", null], │ │ │ │ │ + ["triangulationGraph", "a00359.html#ad167fac94fa72c0d3b8db36e5fa2becb", null], │ │ │ │ │ + ["undistortMeasurementInternal", "a00359.html#a46a14c34d729a626825e4eff8a7de8dd", null], │ │ │ │ │ + ["undistortMeasurements", "a00359.html#af01b2988c1bb89268ad572ea7d7ba293", null], │ │ │ │ │ + ["undistortMeasurements", "a00359.html#a31e52d463db397f0dda1a13352ab217c", null], │ │ │ │ │ + ["undistortMeasurements", "a00359.html#a1a0e4ac1b773cc295daae7849c833876", null], │ │ │ │ │ + ["undistortMeasurements", "a00359.html#a7f3d15de99fffcc537089fa1440d8ca2", null], │ │ │ │ │ + ["undistortMeasurements", "a00359.html#a6a1849035b3acd163d6de715e7683ebf", 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/triangulation.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,177 +98,675 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3Fisheye.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/Cal3.h>
    │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    26
    │ │ │ │ -
    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) {}
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    34#include <gtsam/slam/TriangulationFactor.h>
    │ │ │ │ +
    35
    │ │ │ │ +
    36namespace gtsam {
    │ │ │ │ +
    37
    │ │ │ │ +
    │ │ │ │ +
    39class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
    │ │ │ │ +
    40public:
    │ │ │ │ + │ │ │ │ +
    42 std::runtime_error("Triangulation Underconstrained Exception.") {
    │ │ │ │ +
    43 }
    │ │ │ │ +
    44};
    │ │ │ │ +
    │ │ │ │ +
    45
    │ │ │ │ +
    │ │ │ │ +
    47class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
    │ │ │ │ +
    48public:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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_; }
    │ │ │ │ +
    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
    │ │ │ │ -
    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 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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    Common code for all Calibration models.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    561
    │ │ │ │ + │ │ │ │ +
    567
    │ │ │ │ + │ │ │ │ +
    574
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    722 && maxReprojError > params.dynamicOutlierRejectionThreshold)
    │ │ │ │ +
    723 return TriangulationResult::Outlier();
    │ │ │ │ +
    724
    │ │ │ │ +
    725 // all good!
    │ │ │ │ +
    726 return TriangulationResult(point);
    │ │ │ │ + │ │ │ │ +
    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();
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
    │ │ │ │ +
    Base class to create smart factors on poses or cameras.
    │ │ │ │ +
    Calibration used by Bundler.
    │ │ │ │ +
    Calibration of a fisheye camera.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    2D Pose
    │ │ │ │ +
    Calibrated camera with spherical projection.
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    Unified Calibration Model, see Mei07icra for details.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
    Create a pinhole calibration from a different Cal3 object, removing distortion.
    Definition triangulation.h:253
    │ │ │ │ +
    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
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    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
    │ │ │ │ +
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ +
    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
    │ │ │ │ +
    double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
    distance between two points
    Definition Point3.cpp:27
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ -
    Calibration of a fisheye camera.
    Definition Cal3Fisheye.h:51
    │ │ │ │ -
    double k4() const
    Second tangential distortion coefficient.
    Definition Cal3Fisheye.h:105
    │ │ │ │ -
    double k1() const
    First distortion coefficient.
    Definition Cal3Fisheye.h:96
    │ │ │ │ -
    double k3() const
    First tangential distortion coefficient.
    Definition Cal3Fisheye.h:102
    │ │ │ │ -
    Vector localCoordinates(const Cal3Fisheye &T2) const
    Given a different calibration, calculate update to obtain it.
    Definition Cal3Fisheye.h:168
    │ │ │ │ -
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3Fisheye.h:160
    │ │ │ │ -
    double k2() const
    Second distortion coefficient.
    Definition Cal3Fisheye.h:99
    │ │ │ │ -
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3Fisheye.h:157
    │ │ │ │ -
    Vector9 vector() const
    Return all parameters as a vector.
    Definition Cal3Fisheye.cpp:28
    │ │ │ │ -
    virtual boost::shared_ptr< Cal3Fisheye > clone() const
    Definition Cal3Fisheye.h:177
    │ │ │ │ -
    Vector4 k() const
    return distortion parameter vector
    Definition Cal3Fisheye.h:108
    │ │ │ │ -
    Cal3Fisheye()=default
    Default Constructor with only unit focal length.
    │ │ │ │ -
    Cal3Fisheye retract(const Vector &d) const
    Given delta vector, update calibration.
    Definition Cal3Fisheye.h:163
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    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
    │ │ │ │ +
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │ +
    Exception thrown by triangulateDLT when SVD returns rank < 3.
    Definition triangulation.h:39
    │ │ │ │ +
    Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
    Definition triangulation.h:47
    │ │ │ │ +
    Definition triangulation.h:556
    │ │ │ │ +
    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
    │ │ │ │ +
    double dynamicOutlierRejectionThreshold
    If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
    Definition triangulation.h:573
    │ │ │ │ +
    double rankTolerance
    threshold to decide whether triangulation is result.degenerate
    Definition triangulation.h:558
    │ │ │ │ +
    double landmarkDistanceThreshold
    if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
    Definition triangulation.h:566
    │ │ │ │ +
    bool enableEPI
    if set to true, will refine triangulation using LM
    Definition triangulation.h:560
    │ │ │ │ +
    SharedNoiseModel noiseModel
    used in the nonlinear triangulation
    Definition triangulation.h:575
    │ │ │ │ +
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │ +
    TriangulationResult()
    Default constructor, only for serialization.
    Definition triangulation.h:638
    │ │ │ │ +
    TriangulationResult(const Point3 &p)
    Constructor.
    Definition triangulation.h:643
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition triangulation.h:671
    │ │ │ │ +
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ +
    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
    │ │ │ │ +
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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
    │ │ │ │ +
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition TriangulationFactor.h:33
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ + │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,224 +1,815 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Fisheye.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_/_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 { │ │ │ │ │ -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) {} │ │ │ │ │ +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 │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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// 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. │ │ │ │ │ +_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ +_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +_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. │ │ │ │ │ +_P_o_s_e_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ +_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ +Calibrated camera with spherical projection. │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _C_a_l_3_F_i_s_h_e_y_e_._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/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/Rot2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,89 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    SO3.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    Rot2.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    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

    │ │ │ │ -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)

    │ │ │ │ +

    2D rotation

    │ │ │ │ +
    Date
    Dec 9, 2009
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Duy Nguyen Ta
    │ │ │ │ -
    Date
    December 2014
    │ │ │ │ -

    Function Documentation

    │ │ │ │ - │ │ │ │ -

    ◆ compose()

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
    const SO3R,
    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.

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +John Lambert │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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) │ │ │ │ │ +2D rotation │ │ │ │ │ + Date │ │ │ │ │ + Dec 9, 2009 │ │ │ │ │ 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. │ │ │ │ │ + John Lambert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_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/a00365.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00365 = [ │ │ │ │ │ - ["compose", "a00365.html#a72fc8b10f24d82aa8a4c99f99403fe5e", null], │ │ │ │ │ - ["Dcompose", "a00365.html#a7c7e26b6d07ec0f16363c196247b294d", null] │ │ │ │ │ + ["gtsam::Rot2::ChartAtOrigin", "a03216.html", null], │ │ │ │ │ + ["gtsam::traits< Rot2 >", "a03220.html", null], │ │ │ │ │ + ["gtsam::traits< const Rot2 >", "a03224.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/Pose2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,74 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    BearingRange.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    Pose2.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Bearing-Range product. │ │ │ │ +

    2D Pose │ │ │ │ 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...
    class  gtsam::Pose2
     A 2D pose (Point2,Rot2) More...
     
    struct  gtsam::traits< BearingRange< A1, A2 > >
    struct  gtsam::Pose2::ChartAtOrigin
     
    struct  gtsam::HasBearing< A1, A2, RT >
    struct  gtsam::traits< Pose2 >
     
    struct  gtsam::HasRange< A1, A2, RT >
    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

    │ │ │ │ -

    Bearing-Range product.

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

    2D Pose

    │ │ │ │ +
    Author
    : Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +: Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +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 │ │ │ │ │ -BearingRange.h File Reference │ │ │ │ │ -Bearing-Range product. _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 │ │ │ │ │ -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_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +  A 2D pose (Point2,_R_o_t_2) _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_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │   │ │ │ │ │ -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_:_:_t_r_a_i_t_s_<_ _P_o_s_e_2_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_H_a_s_R_a_n_g_e_<_ _A_1_,_ _A_2_,_ _R_T_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -Bearing-Range product. │ │ │ │ │ - Date │ │ │ │ │ - July, 2015 │ │ │ │ │ +2D Pose │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + : Frank Dellaert │ │ │ │ │ + : Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00368.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,8 @@ │ │ │ │ │ var a00368 = [ │ │ │ │ │ - ["gtsam::BearingRange< A1, A2, B, R >", "a02924.html", "a02924"], │ │ │ │ │ - ["gtsam::traits< BearingRange< A1, A2 > >", "a02928.html", null], │ │ │ │ │ - ["gtsam::HasBearing< A1, A2, RT >", "a02932.html", null], │ │ │ │ │ - ["gtsam::HasRange< A1, A2, RT >", "a02936.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 >", "a00368.html#a4b78f4db0e05f3ca838ee4da6187710e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00368_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,228 +98,337 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BearingRange.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/Testable.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
    │ │ │ │ -
    31// For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
    │ │ │ │ -
    32// At time of writing only Pose2 and Pose3 specialize this functor.
    │ │ │ │ -
    33template <typename A1, typename A2>
    │ │ │ │ -
    34struct Bearing;
    │ │ │ │ -
    35
    │ │ │ │ -
    36// Forward declaration of Range functor which should be of A1*A2 -> return_type
    │ │ │ │ -
    37// For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h will return double
    │ │ │ │ -
    38// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
    │ │ │ │ -
    39template <typename A1, typename A2>
    │ │ │ │ -
    40struct Range;
    │ │ │ │ -
    41
    │ │ │ │ -
    48template <typename A1, typename A2,
    │ │ │ │ -
    49 typename B = typename Bearing<A1, A2>::result_type,
    │ │ │ │ -
    50 typename R = typename Range<A1, A2>::result_type>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    52private:
    │ │ │ │ -
    53 B bearing_;
    │ │ │ │ -
    54 R range_;
    │ │ │ │ -
    55
    │ │ │ │ -
    56public:
    │ │ │ │ -
    57 enum { dimB = traits<B>::dimension };
    │ │ │ │ -
    58 enum { dimR = traits<R>::dimension };
    │ │ │ │ -
    59 enum { dimension = dimB + dimR };
    │ │ │ │ -
    60
    │ │ │ │ -
    63
    │ │ │ │ -
    64 BearingRange() {}
    │ │ │ │ -
    65 BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
    │ │ │ │ -
    66
    │ │ │ │ -
    70
    │ │ │ │ -
    72 const B& bearing() const { return bearing_; }
    │ │ │ │ -
    73
    │ │ │ │ -
    75 const R& range() const { return range_; }
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    26#include <gtsam/base/Lie.h>
    │ │ │ │ +
    27#include <gtsam/dllexport.h>
    │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    79 const A1& a1, const A2& a2,
    │ │ │ │ -
    80 OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
    │ │ │ │ -
    81 OptionalJacobian<dimension, traits<A2>::dimension> H2 = boost::none) {
    │ │ │ │ -
    82 typename MakeJacobian<B, A1>::type HB1;
    │ │ │ │ -
    83 typename MakeJacobian<B, A2>::type HB2;
    │ │ │ │ -
    84 typename MakeJacobian<R, A1>::type HR1;
    │ │ │ │ -
    85 typename MakeJacobian<R, A2>::type HR2;
    │ │ │ │ -
    86
    │ │ │ │ -
    87 B b = Bearing<A1, A2>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
    │ │ │ │ -
    88 R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
    │ │ │ │ +
    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
    │ │ │ │ -
    90 if (H1) *H1 << HB1, HR1;
    │ │ │ │ -
    91 if (H2) *H2 << HB2, HR2;
    │ │ │ │ -
    92 return BearingRange(b, r);
    │ │ │ │ +
    │ │ │ │ +
    91 Pose2(const Vector& v) : Pose2() {
    │ │ │ │ +
    92 *this = Expmap(v);
    │ │ │ │
    93 }
    │ │ │ │
    │ │ │ │
    94
    │ │ │ │ -
    │ │ │ │ -
    96 static B MeasureBearing(const A1& a1, const A2& a2) {
    │ │ │ │ -
    97 return Bearing<A1, A2>()(a1, a2);
    │ │ │ │ -
    98 }
    │ │ │ │ -
    │ │ │ │ -
    99
    │ │ │ │ -
    │ │ │ │ -
    101 static R MeasureRange(const A1& a1, const A2& a2) {
    │ │ │ │ -
    102 return Range<A1, A2>()(a1, a2);
    │ │ │ │ -
    103 }
    │ │ │ │ -
    │ │ │ │ -
    104
    │ │ │ │ -
    108
    │ │ │ │ -
    109 void print(const std::string& str = "") const {
    │ │ │ │ -
    110 std::cout << str;
    │ │ │ │ -
    111 traits<B>::Print(bearing_, "bearing ");
    │ │ │ │ -
    112 traits<R>::Print(range_, "range ");
    │ │ │ │ -
    113 }
    │ │ │ │ -
    114 bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
    │ │ │ │ -
    115 return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
    │ │ │ │ -
    116 traits<R>::Equals(range_, m2.range_, tol);
    │ │ │ │ -
    117 }
    │ │ │ │ -
    118
    │ │ │ │ -
    122
    │ │ │ │ -
    123 inline static size_t Dim() { return dimension; }
    │ │ │ │ -
    124 inline size_t dim() const { return dimension; }
    │ │ │ │ -
    125
    │ │ │ │ -
    126 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
    │ │ │ │ -
    127 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
    │ │ │ │ -
    128
    │ │ │ │ -
    │ │ │ │ -
    130 BearingRange retract(const TangentVector& xi) const {
    │ │ │ │ -
    131 B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
    │ │ │ │ -
    132 R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
    │ │ │ │ -
    133 return BearingRange(m1, m2);
    │ │ │ │ -
    134 }
    │ │ │ │ +
    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;
    │ │ │ │ +
    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 TangentVector localCoordinates(const BearingRange& other) const {
    │ │ │ │ -
    138 typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
    │ │ │ │ -
    139 typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
    │ │ │ │ -
    140 TangentVector v;
    │ │ │ │ -
    141 v << v1, v2;
    │ │ │ │ -
    142 return v;
    │ │ │ │ -
    143 }
    │ │ │ │ -
    │ │ │ │ -
    144
    │ │ │ │ -
    148
    │ │ │ │ -
    149private:
    │ │ │ │ -
    151 template <class ARCHIVE>
    │ │ │ │ -
    152 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    │ │ │ │ -
    153 ar& boost::serialization::make_nvp("bearing", bearing_);
    │ │ │ │ -
    154 ar& boost::serialization::make_nvp("range", range_);
    │ │ │ │ -
    155 }
    │ │ │ │ -
    156
    │ │ │ │ -
    157 friend class boost::serialization::access;
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    167};
    │ │ │ │ -
    │ │ │ │ -
    168
    │ │ │ │ -
    169// Declare this to be both Testable and a Manifold
    │ │ │ │ -
    170template <typename A1, typename A2>
    │ │ │ │ -
    │ │ │ │ -
    171struct traits<BearingRange<A1, A2> >
    │ │ │ │ -
    172 : Testable<BearingRange<A1, A2> >,
    │ │ │ │ -
    173 internal::ManifoldTraits<BearingRange<A1, A2> > {};
    │ │ │ │ -
    │ │ │ │ -
    174
    │ │ │ │ -
    175// Helper class for to implement Range traits for classes with a bearing method
    │ │ │ │ -
    176// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
    │ │ │ │ -
    177// template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
    │ │ │ │ -
    178// where the third argument is used to indicate the return type
    │ │ │ │ -
    179template <class A1, typename A2, class RT>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    181 typedef RT result_type;
    │ │ │ │ -
    182 RT operator()(
    │ │ │ │ -
    183 const A1& a1, const A2& a2,
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    186 return a1.bearing(a2, H1, H2);
    │ │ │ │ -
    187 }
    │ │ │ │ -
    188};
    │ │ │ │ -
    │ │ │ │ -
    189
    │ │ │ │ -
    190// Similar helper class for to implement Range traits for classes with a range method
    │ │ │ │ -
    191// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
    │ │ │ │ -
    192// template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
    │ │ │ │ -
    193template <class A1, typename A2, class RT>
    │ │ │ │ -
    │ │ │ │ -
    194struct HasRange {
    │ │ │ │ -
    195 typedef RT result_type;
    │ │ │ │ -
    196 RT operator()(
    │ │ │ │ -
    197 const A1& a1, const A2& a2,
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    200 return a1.range(a2, H1, H2);
    │ │ │ │ -
    201 }
    │ │ │ │ -
    202};
    │ │ │ │ -
    │ │ │ │ -
    203
    │ │ │ │ -
    204} // namespace gtsam
    │ │ │ │ -
    Special class for optional Jacobian arguments.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    Matrix wedge< Pose2 >(const Vector &xi)
    specialization for pose2 wedge function (generic template in Lie.h)
    Definition Pose2.h:341
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM manifolds.
    Definition Manifold.h:95
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    Definition BearingRange.h:34
    │ │ │ │
    Definition BearingRange.h:40
    │ │ │ │ -
    Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
    Definition BearingRange.h:51
    │ │ │ │ -
    static R MeasureRange(const A1 &a1, const A2 &a2)
    Predict range.
    Definition BearingRange.h:101
    │ │ │ │ -
    static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< dimension, traits< A2 >::dimension > H2=boost::none)
    Prediction function that stacks measurements.
    Definition BearingRange.h:78
    │ │ │ │ -
    BearingRange retract(const TangentVector &xi) const
    Retract delta to manifold.
    Definition BearingRange.h:130
    │ │ │ │ -
    const B & bearing() const
    Return bearing measurement.
    Definition BearingRange.h:72
    │ │ │ │ -
    TangentVector localCoordinates(const BearingRange &other) const
    Compute the coordinates in the tangent space.
    Definition BearingRange.h:137
    │ │ │ │ -
    const R & range() const
    Return range measurement.
    Definition BearingRange.h:75
    │ │ │ │ -
    static B MeasureBearing(const A1 &a1, const A2 &a2)
    Predict bearing.
    Definition BearingRange.h:96
    │ │ │ │
    Definition BearingRange.h:180
    │ │ │ │
    Definition BearingRange.h:194
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    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
    │ │ │ │ +
    Pose2 operator*(const Pose2 &p2) const
    compose syntactic sugar
    Definition Pose2.h:128
    │ │ │ │ +
    GTSAM_EXPORT void print(const std::string &s="") const
    print with optional string
    Definition Pose2.cpp:50
    │ │ │ │ +
    const Rot2 & rotation() const
    rotation
    Definition Pose2.h:261
    │ │ │ │ +
    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
    │ │ │ │ +
    static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
    Derivative of Logmap.
    Definition Pose2.cpp:179
    │ │ │ │ +
    double y() const
    get y
    Definition Pose2.h:246
    │ │ │ │ +
    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_EXPORT Pose2 inverse() const
    inverse
    Definition Pose2.cpp:201
    │ │ │ │ +
    GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
    Output stream operator.
    Definition Pose2.cpp:55
    │ │ │ │ +
    Point2 operator*(const Point2 &point) const
    syntactic sugar for transformFrom
    Definition Pose2.h:234
    │ │ │ │ +
    Pose2(double x, double y, double theta)
    construct from (x,y,theta)
    Definition Pose2.h:68
    │ │ │ │ +
    const Point2 & t() const
    translation
    Definition Pose2.h:252
    │ │ │ │ +
    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
    │ │ │ │ +
    Pose2(double theta, const Point2 &t)
    construct from rotation and translation
    Definition Pose2.h:73
    │ │ │ │ +
    double x() const
    get x
    Definition Pose2.h:243
    │ │ │ │ +
    const Rot2 & r() const
    rotation
    Definition Pose2.h:255
    │ │ │ │ +
    Pose2(const Vector &v)
    Construct from canonical coordinates (Lie algebra)
    Definition Pose2.h:91
    │ │ │ │ +
    Vector3 Adjoint(const Vector3 &xi) const
    Apply AdjointMap to twist xi.
    Definition Pose2.h:149
    │ │ │ │ +
    static Matrix3 wedge(double vx, double vy, double w)
    wedge for SE(2):
    Definition Pose2.h:183
    │ │ │ │ +
    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
    │ │ │ │ +
    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_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
    │ │ │ │ +
    double theta() const
    get theta
    Definition Pose2.h:249
    │ │ │ │ +
    static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
    Derivative of Expmap.
    Definition Pose2.cpp:148
    │ │ │ │ +
    Rot2 Rotation
    Pose Concept requirements.
    Definition Pose2.h:41
    │ │ │ │ +
    Pose2()
    default constructor = origin
    Definition Pose2.h:55
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static Pose2 Identity()
    identity for group operation
    Definition Pose2.h:122
    │ │ │ │ +
    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
    │ │ │ │ +
    Pose2(const Rot2 &r, const Point2 &t)
    construct from r,t
    Definition Pose2.h:78
    │ │ │ │ +
    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
    │ │ │ │ +
    Pose2(const Pose2 &pose)
    copy constructor
    Definition Pose2.h:60
    │ │ │ │ +
    const Point2 & translation() const
    translation
    Definition Pose2.h:258
    │ │ │ │ +
    Pose2(const Matrix &T)
    Constructor from 3*3 matrix.
    Definition Pose2.h:81
    │ │ │ │ +
    Definition Pose2.h:198
    │ │ │ │ +
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ +
    double theta() const
    return angle (RADIANS)
    Definition Rot2.h:187
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,268 +1,472 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BearingRange.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_e_s_t_a_b_l_e_._h> │ │ │ │ │ -23#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> │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Forward declaration of Bearing functor which should be of A1*A2 - │ │ │ │ │ -> return_type │ │ │ │ │ -31// For example Bearing(pose,point), defined in Pose3.h will │ │ │ │ │ -return Unit3 │ │ │ │ │ -32// At time of writing only Pose2 and Pose3 specialize this functor. │ │ │ │ │ -33template │ │ │ │ │ -_3_4struct _B_e_a_r_i_n_g; │ │ │ │ │ -35 │ │ │ │ │ -36// Forward declaration of Range functor which should be of A1*A2 - │ │ │ │ │ -> return_type │ │ │ │ │ -37// For example Range(T1,T2), defined in Pose2.h will return │ │ │ │ │ -double │ │ │ │ │ -38// At time of writing Pose2, Pose3, and several Camera variants specialize │ │ │ │ │ -this for several types │ │ │ │ │ -39template │ │ │ │ │ -_4_0struct _R_a_n_g_e; │ │ │ │ │ -41 │ │ │ │ │ -48template _:_:_r_e_s_u_l_t___t_y_p_e, │ │ │ │ │ -50 typename R = typename _R_a_n_g_e_<_A_1_,_ _A_2_>_:_:_r_e_s_u_l_t___t_y_p_e> │ │ │ │ │ -_5_1struct _B_e_a_r_i_n_g_R_a_n_g_e { │ │ │ │ │ -52private: │ │ │ │ │ -53 B bearing_; │ │ │ │ │ -54 R range_; │ │ │ │ │ -55 │ │ │ │ │ -56public: │ │ │ │ │ -57 enum { dimB = _t_r_a_i_t_s_<_B_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ -58 enum { dimR = _t_r_a_i_t_s_<_R_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ -59 enum { dimension = dimB + dimR }; │ │ │ │ │ -60 │ │ │ │ │ -63 │ │ │ │ │ -64 _B_e_a_r_i_n_g_R_a_n_g_e() {} │ │ │ │ │ -65 _B_e_a_r_i_n_g_R_a_n_g_e(const B& b, const R& r) : bearing_(b), range_(r) {} │ │ │ │ │ -66 │ │ │ │ │ -70 │ │ │ │ │ -_7_2 const B& _b_e_a_r_i_n_g() const { return bearing_; } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 const R& _r_a_n_g_e() const { return range_; } │ │ │ │ │ +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 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_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 │ │ │ │ │ +_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 static _B_e_a_r_i_n_g_R_a_n_g_e _M_e_a_s_u_r_e( │ │ │ │ │ -79 const A1& a1, const A2& a2, │ │ │ │ │ -80 _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> H1 = boost::none, │ │ │ │ │ -81 _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> H2 = boost::none) { │ │ │ │ │ -82 typename MakeJacobian::type HB1; │ │ │ │ │ -83 typename MakeJacobian::type HB2; │ │ │ │ │ -84 typename MakeJacobian::type HR1; │ │ │ │ │ -85 typename MakeJacobian::type HR2; │ │ │ │ │ -86 │ │ │ │ │ -87 B b = _B_e_a_r_i_n_g_<_A_1_,_ _A_2_>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0); │ │ │ │ │ -88 R r = _R_a_n_g_e_<_A_1_,_ _A_2_>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0); │ │ │ │ │ +_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 │ │ │ │ │ -90 if (H1) *H1 << HB1, HR1; │ │ │ │ │ -91 if (H2) *H2 << HB2, HR2; │ │ │ │ │ -92 return _B_e_a_r_i_n_g_R_a_n_g_e(b, r); │ │ │ │ │ +_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 │ │ │ │ │ -_9_6 static B _M_e_a_s_u_r_e_B_e_a_r_i_n_g(const A1& a1, const A2& a2) { │ │ │ │ │ -97 return _B_e_a_r_i_n_g_<_A_1_,_ _A_2_>()(a1, a2); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 static R _M_e_a_s_u_r_e_R_a_n_g_e(const A1& a1, const A2& a2) { │ │ │ │ │ -102 return _R_a_n_g_e_<_A_1_,_ _A_2_>()(a1, a2); │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -108 │ │ │ │ │ -109 void print(const std::string& str = "") const { │ │ │ │ │ -110 std::cout << str; │ │ │ │ │ -111 _t_r_a_i_t_s_<_B_>_:_:_P_r_i_n_t(bearing_, "bearing "); │ │ │ │ │ -112 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(range_, "range "); │ │ │ │ │ -113 } │ │ │ │ │ -114 bool equals(const BearingRange& m2, double tol = 1e-8) const { │ │ │ │ │ -115 return traits::Equals(bearing_, m2.bearing_, tol) && │ │ │ │ │ -116 traits::Equals(range_, m2.range_, tol); │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -122 │ │ │ │ │ -123 inline static size_t Dim() { return dimension; } │ │ │ │ │ -124 inline size_t dim() const { return dimension; } │ │ │ │ │ -125 │ │ │ │ │ -126 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -127 typedef OptionalJacobian ChartJacobian; │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 _B_e_a_r_i_n_g_R_a_n_g_e _r_e_t_r_a_c_t(const TangentVector& xi) const { │ │ │ │ │ -131 B m1 = _t_r_a_i_t_s_<_B_>_:_:_R_e_t_r_a_c_t(bearing_, xi.template head()); │ │ │ │ │ -132 R m2 = _t_r_a_i_t_s_<_R_>_:_:_R_e_t_r_a_c_t(range_, xi.template tail()); │ │ │ │ │ -133 return _B_e_a_r_i_n_g_R_a_n_g_e(m1, m2); │ │ │ │ │ -134 } │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -_1_3_7 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _B_e_a_r_i_n_g_R_a_n_g_e& other) const { │ │ │ │ │ -138 typename _t_r_a_i_t_s_<_B_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v1 = _t_r_a_i_t_s_<_B_>_:_:_L_o_c_a_l(bearing_, │ │ │ │ │ -other.bearing_); │ │ │ │ │ -139 typename _t_r_a_i_t_s_<_R_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v2 = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(range_, │ │ │ │ │ -other.range_); │ │ │ │ │ -140 TangentVector v; │ │ │ │ │ -141 v << v1, v2; │ │ │ │ │ -142 return v; │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -148 │ │ │ │ │ -149private: │ │ │ │ │ -151 template │ │ │ │ │ -152 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -153 ar& boost::serialization::make_nvp("bearing", bearing_); │ │ │ │ │ -154 ar& boost::serialization::make_nvp("range", range_); │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -157 friend class boost::serialization::access; │ │ │ │ │ -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 _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) │ │ │ │ │ -167}; │ │ │ │ │ -168 │ │ │ │ │ -169// Declare this to be both Testable and a Manifold │ │ │ │ │ -170template │ │ │ │ │ -_1_7_1struct _t_r_a_i_t_s<_B_e_a_r_i_n_g_R_a_n_g_e > │ │ │ │ │ -172 : _T_e_s_t_a_b_l_e >, │ │ │ │ │ -173 _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s > {}; │ │ │ │ │ -174 │ │ │ │ │ -175// Helper class for to implement Range traits for classes with a bearing │ │ │ │ │ -method │ │ │ │ │ -176// For example, to specialize Bearing to Pose3 and Point3, using Pose3:: │ │ │ │ │ -bearing, it suffices to say │ │ │ │ │ -177// template <> struct Bearing : HasBearing {}; │ │ │ │ │ -178// where the third argument is used to indicate the return type │ │ │ │ │ -179template │ │ │ │ │ -_1_8_0struct _H_a_s_B_e_a_r_i_n_g { │ │ │ │ │ -181 typedef RT result_type; │ │ │ │ │ -182 RT operator()( │ │ │ │ │ -183 const A1& a1, const A2& a2, │ │ │ │ │ -184 _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_1_>_:_:_d_i_m_e_n_s_i_o_n> H1=boost:: │ │ │ │ │ -none, │ │ │ │ │ -185 _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) { │ │ │ │ │ -186 return a1.bearing(a2, H1, H2); │ │ │ │ │ -187 } │ │ │ │ │ -188}; │ │ │ │ │ -189 │ │ │ │ │ -190// Similar helper class for to implement Range traits for classes with a │ │ │ │ │ -range method │ │ │ │ │ -191// For classes with overloaded range methods, such as PinholeCamera, this │ │ │ │ │ -can even be templated: │ │ │ │ │ -192// template struct Range : │ │ │ │ │ -HasRange {}; │ │ │ │ │ -193template │ │ │ │ │ -_1_9_4struct _H_a_s_R_a_n_g_e { │ │ │ │ │ -195 typedef RT result_type; │ │ │ │ │ -196 RT operator()( │ │ │ │ │ -197 const A1& a1, const A2& a2, │ │ │ │ │ -198 _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_1_>_:_:_d_i_m_e_n_s_i_o_n> H1=boost:: │ │ │ │ │ -none, │ │ │ │ │ -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. │ │ │ │ │ -_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) │ │ │ │ │ +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:317 │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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 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_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_:_:_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_:_:_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_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_:_:_B_e_a_r_i_n_g_R_a_n_g_e │ │ │ │ │ -Bearing-Range product for a particular A1,A2 combination will use the functors │ │ │ │ │ -above to create a simi... │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e_R_a_n_g_e │ │ │ │ │ -static R MeasureRange(const A1 &a1, const A2 &a2) │ │ │ │ │ -Predict range. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e │ │ │ │ │ -static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< │ │ │ │ │ -dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< │ │ │ │ │ -dimension, traits< A2 >::dimension > H2=boost::none) │ │ │ │ │ -Prediction function that stacks measurements. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ -BearingRange retract(const TangentVector &xi) const │ │ │ │ │ -Retract delta to manifold. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_b_e_a_r_i_n_g │ │ │ │ │ -const B & bearing() const │ │ │ │ │ -Return bearing measurement. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const BearingRange &other) const │ │ │ │ │ -Compute the coordinates in the tangent space. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_r_a_n_g_e │ │ │ │ │ -const R & range() const │ │ │ │ │ -Return range measurement. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_:_:_M_e_a_s_u_r_e_B_e_a_r_i_n_g │ │ │ │ │ -static B MeasureBearing(const A1 &a1, const A2 &a2) │ │ │ │ │ -Predict bearing. │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:96 │ │ │ │ │ _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 │ │ │ │ │ - * _B_e_a_r_i_n_g_R_a_n_g_e_._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/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/Point3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,99 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    Cal3.cpp File Reference
    │ │ │ │ +
    Point3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    3D Point │ │ │ │ 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

    │ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3 &cal)
     
    │ │ │ │ +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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Common code for all calibration models.

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

    3D Point

    │ │ │ │ +
    Author
    Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +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 │ │ │ │ │ -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 | _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_ _> │ │ │ │ │ +  │ │ │ │ │ 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 │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3 &cal) │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Common code for all calibration models. │ │ │ │ │ +3D Point │ │ │ │ │ Author │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._c_p_p │ │ │ │ │ + * _P_o_i_n_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/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/Rot3Q.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,42 +93,27 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3Fisheye.cpp File Reference
    │ │ │ │ +
    Rot3Q.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

    │ │ │ │ -Namespaces

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

    │ │ │ │ -Functions

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

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

    │ │ │ │

    Detailed Description

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

    Rotation (internal: quaternion representation*)

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,15 @@ │ │ │ │ │ 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 │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ +Rot3Q.cpp File Reference │ │ │ │ │ +Rotation (internal: quaternion representation*) _M_o_r_e_._._. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Apr 8, 2020 │ │ │ │ │ +Rotation (internal: quaternion representation*) │ │ │ │ │ Author │ │ │ │ │ - ghaggin │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Cal3_S2Stereo.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    StereoCamera.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3_S2Stereo.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ 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 Cal3_S2Stereo &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A Stereo Camera based on two Simple Cameras.

    │ │ │ │ -

    A Rectified Stereo Camera.

    │ │ │ │ +

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

    │ │ │ │
    Author
    Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +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 ********** │ │ │ │ │ -A Stereo Camera based on two Simple Cameras. │ │ │ │ │ -A Rectified Stereo Camera. │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ Author │ │ │ │ │ Chris Beall │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Cal3DS2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,84 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    Rot3.h File Reference
    │ │ │ │ +
    Cal3DS2.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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

    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'.
     
    │ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2 &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    3D rotation represented as a rotation matrix or quaternion

    │ │ │ │ -
    Author
    Alireza Fathi
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ +
    Date
    Feb 28, 2010
    │ │ │ │ +
    Author
    ydjian
    │ │ │ │
    │ │ │ │ Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,56 +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 | _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_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3DS2.cpp File Reference │ │ │ │ │ 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'. │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2 &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2010 │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Luca Carlone │ │ │ │ │ + ydjian │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_._h │ │ │ │ │ + * _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/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/Cal3_S2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    PinholeSet.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3_S2.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    The most common 5DOF 3D->2D calibration. │ │ │ │ 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

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

    Detailed Description

    │ │ │ │ -

    A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera.

    │ │ │ │ +

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

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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___S_2 &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_S_e_t_._h │ │ │ │ │ + * _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/a00389_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Unit3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,245 +98,162 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Unit3.h
    │ │ │ │ +
    SOn-inl.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-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/*
    │ │ │ │ -
    13 * @file Unit3.h
    │ │ │ │ -
    14 * @date Feb 02, 2011
    │ │ │ │ -
    15 * @author Can Erdogan
    │ │ │ │ -
    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
    │ │ │ │ +
    12#pragma once
    │ │ │ │ +
    13
    │ │ │ │ +
    21#include <gtsam/base/Matrix.h>
    │ │ │ │
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/base/Manifold.h>
    │ │ │ │ -
    26#include <gtsam/base/Vector.h>
    │ │ │ │ - │ │ │ │ -
    28#include <gtsam/base/Matrix.h>
    │ │ │ │ -
    29#include <gtsam/dllexport.h>
    │ │ │ │ -
    30
    │ │ │ │ -
    31#include <boost/optional.hpp>
    │ │ │ │ +
    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#include <random>
    │ │ │ │ -
    34#include <string>
    │ │ │ │ -
    35
    │ │ │ │ -
    36#ifdef GTSAM_USE_TBB
    │ │ │ │ -
    37#include <mutex> // std::mutex
    │ │ │ │ -
    38#endif
    │ │ │ │ -
    39
    │ │ │ │ -
    40namespace gtsam {
    │ │ │ │ -
    41
    │ │ │ │ -
    │ │ │ │ -
    43class GTSAM_EXPORT Unit3 {
    │ │ │ │ -
    44
    │ │ │ │ -
    45private:
    │ │ │ │ -
    46
    │ │ │ │ -
    47 Vector3 p_;
    │ │ │ │ -
    48 mutable boost::optional<Matrix32> B_;
    │ │ │ │ -
    49 mutable boost::optional<Matrix62> H_B_;
    │ │ │ │ -
    50
    │ │ │ │ -
    51#ifdef GTSAM_USE_TBB
    │ │ │ │ -
    52 mutable std::mutex B_mutex_;
    │ │ │ │ -
    53#endif
    │ │ │ │ -
    54
    │ │ │ │ -
    55public:
    │ │ │ │ -
    56
    │ │ │ │ -
    57 enum {
    │ │ │ │ -
    58 dimension = 2
    │ │ │ │ -
    59 };
    │ │ │ │ -
    60
    │ │ │ │ -
    63
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    66 p_(1.0, 0.0, 0.0) {
    │ │ │ │ -
    67 }
    │ │ │ │ -
    │ │ │ │ -
    68
    │ │ │ │ -
    70 explicit Unit3(const Vector3& p);
    │ │ │ │ -
    71
    │ │ │ │ -
    73 Unit3(double x, double y, double z);
    │ │ │ │ -
    74
    │ │ │ │ -
    77 explicit Unit3(const Point2& p, double f);
    │ │ │ │ -
    78
    │ │ │ │ -
    │ │ │ │ -
    80 Unit3(const Unit3& u) {
    │ │ │ │ -
    81 p_ = u.p_;
    │ │ │ │ -
    82 }
    │ │ │ │ -
    │ │ │ │ -
    83
    │ │ │ │ -
    │ │ │ │ -
    85 Unit3& operator=(const Unit3 & u) {
    │ │ │ │ -
    86 p_ = u.p_;
    │ │ │ │ -
    87 B_ = u.B_;
    │ │ │ │ -
    88 H_B_ = u.H_B_;
    │ │ │ │ -
    89 return *this;
    │ │ │ │ -
    90 }
    │ │ │ │ -
    │ │ │ │ -
    91
    │ │ │ │ -
    93 static Unit3 FromPoint3(const Point3& point, //
    │ │ │ │ -
    94 OptionalJacobian<2, 3> H = boost::none);
    │ │ │ │ -
    95
    │ │ │ │ -
    102 static Unit3 Random(std::mt19937 & rng);
    │ │ │ │ -
    103
    │ │ │ │ -
    105
    │ │ │ │ -
    108
    │ │ │ │ -
    109 friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
    │ │ │ │ -
    110
    │ │ │ │ -
    112 void print(const std::string& s = std::string()) const;
    │ │ │ │ -
    113
    │ │ │ │ -
    │ │ │ │ -
    115 bool equals(const Unit3& s, double tol = 1e-9) const {
    │ │ │ │ -
    116 return equal_with_abs_tol(p_, s.p_, tol);
    │ │ │ │ -
    117 }
    │ │ │ │ -
    │ │ │ │ -
    119
    │ │ │ │ -
    122
    │ │ │ │ -
    129 const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
    │ │ │ │ -
    130
    │ │ │ │ -
    132 Matrix3 skew() const;
    │ │ │ │ -
    133
    │ │ │ │ -
    135 Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
    │ │ │ │ -
    136
    │ │ │ │ -
    138 Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
    │ │ │ │ -
    139
    │ │ │ │ -
    │ │ │ │ -
    141 friend Point3 operator*(double s, const Unit3& d) {
    │ │ │ │ -
    142 return Point3(s * d.p_);
    │ │ │ │ -
    143 }
    │ │ │ │ -
    │ │ │ │ -
    144
    │ │ │ │ -
    146 double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
    │ │ │ │ -
    147 OptionalJacobian<1,2> H2 = boost::none) const;
    │ │ │ │ -
    148
    │ │ │ │ -
    151 Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
    │ │ │ │ -
    152
    │ │ │ │ -
    155 Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
    │ │ │ │ -
    156 OptionalJacobian<2, 2> H_q = boost::none) const;
    │ │ │ │ -
    157
    │ │ │ │ -
    159 double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
    │ │ │ │ -
    160
    │ │ │ │ -
    │ │ │ │ -
    162 Unit3 cross(const Unit3& q) const {
    │ │ │ │ -
    163 return Unit3(p_.cross(q.p_));
    │ │ │ │ -
    164 }
    │ │ │ │ -
    │ │ │ │ -
    165
    │ │ │ │ -
    │ │ │ │ -
    167 Point3 cross(const Point3& q) const {
    │ │ │ │ -
    168 return point3().cross(q);
    │ │ │ │ -
    169 }
    │ │ │ │ -
    │ │ │ │ -
    170
    │ │ │ │ -
    172
    │ │ │ │ -
    175
    │ │ │ │ -
    │ │ │ │ -
    177 inline static size_t Dim() {
    │ │ │ │ -
    178 return 2;
    │ │ │ │ -
    179 }
    │ │ │ │ -
    │ │ │ │ -
    180
    │ │ │ │ -
    │ │ │ │ -
    182 inline size_t dim() const {
    │ │ │ │ -
    183 return 2;
    │ │ │ │ -
    184 }
    │ │ │ │ -
    │ │ │ │ -
    185
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    188 RENORM
    │ │ │ │ -
    189 };
    │ │ │ │ -
    │ │ │ │ -
    190
    │ │ │ │ -
    192 Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
    │ │ │ │ -
    193
    │ │ │ │ -
    195 Vector2 localCoordinates(const Unit3& s) const;
    │ │ │ │ -
    196
    │ │ │ │ -
    198
    │ │ │ │ -
    199private:
    │ │ │ │ -
    200
    │ │ │ │ -
    203
    │ │ │ │ -
    204 friend class boost::serialization::access;
    │ │ │ │ -
    205 template<class ARCHIVE>
    │ │ │ │ -
    206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    207 ar & BOOST_SERIALIZATION_NVP(p_);
    │ │ │ │ -
    208 }
    │ │ │ │ -
    209
    │ │ │ │ -
    211
    │ │ │ │ -
    212public:
    │ │ │ │ - │ │ │ │ -
    214};
    │ │ │ │ -
    │ │ │ │ -
    215
    │ │ │ │ -
    216// Define GTSAM traits
    │ │ │ │ -
    │ │ │ │ -
    217template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
    │ │ │ │ -
    218};
    │ │ │ │ -
    │ │ │ │ -
    219
    │ │ │ │ -
    │ │ │ │ -
    220template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
    │ │ │ │ -
    221};
    │ │ │ │ -
    │ │ │ │ -
    222
    │ │ │ │ -
    223} // namespace gtsam
    │ │ │ │ -
    224
    │ │ │ │ -
    serialization for Vectors
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    Unit3(const Unit3 &u)
    Copy constructor.
    Definition Unit3.h:80
    │ │ │ │ -
    Unit3()
    Default constructor.
    Definition Unit3.h:65
    │ │ │ │ -
    friend Point3 operator*(double s, const Unit3 &d)
    Return scaled direction as Point3.
    Definition Unit3.h:141
    │ │ │ │ -
    static size_t Dim()
    Dimensionality of tangent space = 2 DOF.
    Definition Unit3.h:177
    │ │ │ │ -
    size_t dim() const
    Dimensionality of tangent space = 2 DOF.
    Definition Unit3.h:182
    │ │ │ │ -
    bool equals(const Unit3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition Unit3.h:115
    │ │ │ │ -
    CoordinatesMode
    Definition Unit3.h:186
    │ │ │ │ -
    @ EXPMAP
    Use the exponential map to retract.
    Definition Unit3.h:187
    │ │ │ │ -
    Unit3 cross(const Unit3 &q) const
    Cross-product between two Unit3s.
    Definition Unit3.h:162
    │ │ │ │ -
    Point3 cross(const Point3 &q) const
    Cross-product w Point3.
    Definition Unit3.h:167
    │ │ │ │ -
    Unit3 & operator=(const Unit3 &u)
    Copy assignment.
    Definition Unit3.h:85
    │ │ │ │ +
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    static Matrix VectorizedGenerators()
    Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
    Definition SOn.h:300
    │ │ │ │ +
    SO inverse() const
    inverse of a rotation = transpose
    Definition SOn.h:193
    │ │ │ │ +
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ +
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ +
    MatrixNN matrix_
    Rotation matrix.
    Definition SOn.h:62
    │ │ │ │ +
    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
    │ │ │ │ +
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ +
    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
    │ │ │ │ +
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ +
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ +
    SO()
    Construct SO<N> identity for N >= 2.
    Definition SOn.h:79
    │ │ │ │ +
    static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
    Inverse of Retract.
    Definition SOn-inl.h:50
    │ │ │ │ +
    static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
    Retract uses Cayley map.
    Definition SOn-inl.h:40
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,285 +1,194 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Unit3.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 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * 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 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file Unit3.h │ │ │ │ │ -14 * @date Feb 02, 2011 │ │ │ │ │ -15 * @author Can Erdogan │ │ │ │ │ -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 │ │ │ │ │ +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 <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -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_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -27#include <_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> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31#include │ │ │ │ │ +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#include │ │ │ │ │ -34#include │ │ │ │ │ -35 │ │ │ │ │ -36#ifdef GTSAM_USE_TBB │ │ │ │ │ -37#include // std::mutex │ │ │ │ │ -38#endif │ │ │ │ │ -39 │ │ │ │ │ -40namespace _g_t_s_a_m { │ │ │ │ │ -41 │ │ │ │ │ -_4_3class GTSAM_EXPORT _U_n_i_t_3 { │ │ │ │ │ -44 │ │ │ │ │ -45private: │ │ │ │ │ -46 │ │ │ │ │ -47 Vector3 p_; │ │ │ │ │ -48 mutable boost::optional B_; │ │ │ │ │ -49 mutable boost::optional H_B_; │ │ │ │ │ -50 │ │ │ │ │ -51#ifdef GTSAM_USE_TBB │ │ │ │ │ -52 mutable std::mutex B_mutex_; │ │ │ │ │ -53#endif │ │ │ │ │ -54 │ │ │ │ │ -55public: │ │ │ │ │ -56 │ │ │ │ │ -57 enum { │ │ │ │ │ -58 dimension = 2 │ │ │ │ │ -59 }; │ │ │ │ │ -60 │ │ │ │ │ -63 │ │ │ │ │ -_6_5 _U_n_i_t_3() : │ │ │ │ │ -66 p_(1.0, 0.0, 0.0) { │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -70 explicit _U_n_i_t_3(const Vector3& p); │ │ │ │ │ -71 │ │ │ │ │ -73 _U_n_i_t_3(double x, double y, double z); │ │ │ │ │ -74 │ │ │ │ │ -77 explicit _U_n_i_t_3(const _P_o_i_n_t_2& p, double f); │ │ │ │ │ -78 │ │ │ │ │ -_8_0 _U_n_i_t_3(const _U_n_i_t_3& u) { │ │ │ │ │ -81 p_ = u.p_; │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -_8_5 _U_n_i_t_3& _o_p_e_r_a_t_o_r_=(const _U_n_i_t_3 & u) { │ │ │ │ │ -86 p_ = u.p_; │ │ │ │ │ -87 B_ = u.B_; │ │ │ │ │ -88 H_B_ = u.H_B_; │ │ │ │ │ -89 return *this; │ │ │ │ │ -90 } │ │ │ │ │ -91 │ │ │ │ │ -93 static _U_n_i_t_3 FromPoint3(const _P_o_i_n_t_3& point, // │ │ │ │ │ -94 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> H = boost::none); │ │ │ │ │ -95 │ │ │ │ │ -102 static _U_n_i_t_3 Random(std::mt19937 & rng); │ │ │ │ │ -103 │ │ │ │ │ -105 │ │ │ │ │ -108 │ │ │ │ │ -109 friend std::ostream& operator<<(std::ostream& os, const _U_n_i_t_3& pair); │ │ │ │ │ -110 │ │ │ │ │ -112 void _p_r_i_n_t(const std::string& s = std::string()) const; │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 bool _e_q_u_a_l_s(const _U_n_i_t_3& s, double tol = 1e-9) const { │ │ │ │ │ -116 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(p_, s.p_, tol); │ │ │ │ │ -117 } │ │ │ │ │ -119 │ │ │ │ │ -122 │ │ │ │ │ -129 const Matrix32& basis(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _2_> H = boost::none) const; │ │ │ │ │ -130 │ │ │ │ │ -132 Matrix3 skew() const; │ │ │ │ │ -133 │ │ │ │ │ -135 _P_o_i_n_t_3 point3(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> H = boost::none) const; │ │ │ │ │ -136 │ │ │ │ │ -138 Vector3 unitVector(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> H = boost::none) const; │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 friend _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(double s, const _U_n_i_t_3& d) { │ │ │ │ │ -142 return _P_o_i_n_t_3(s * d.p_); │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -146 double _d_o_t(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H1 = boost::none, // │ │ │ │ │ -147 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H2 = boost::none) const; │ │ │ │ │ -148 │ │ │ │ │ -151 Vector2 error(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_q = boost::none) │ │ │ │ │ -const; │ │ │ │ │ -152 │ │ │ │ │ -155 Vector2 errorVector(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_p = boost:: │ │ │ │ │ -none, // │ │ │ │ │ -156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H_q = boost::none) const; │ │ │ │ │ -157 │ │ │ │ │ -159 double distance(const _U_n_i_t_3& q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H = boost::none) │ │ │ │ │ -const; │ │ │ │ │ -160 │ │ │ │ │ -_1_6_2 _U_n_i_t_3 _c_r_o_s_s(const _U_n_i_t_3& q) const { │ │ │ │ │ -163 return _U_n_i_t_3(p_.cross(q.p_)); │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -_1_6_7 _P_o_i_n_t_3 _c_r_o_s_s(const _P_o_i_n_t_3& q) const { │ │ │ │ │ -168 return point3().cross(q); │ │ │ │ │ -169 } │ │ │ │ │ -170 │ │ │ │ │ -172 │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 inline static size_t _D_i_m() { │ │ │ │ │ -178 return 2; │ │ │ │ │ -179 } │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 inline size_t _d_i_m() const { │ │ │ │ │ -183 return 2; │ │ │ │ │ -184 } │ │ │ │ │ -185 │ │ │ │ │ -_1_8_6 enum _C_o_o_r_d_i_n_a_t_e_s_M_o_d_e { │ │ │ │ │ -_1_8_7 _E_X_P_M_A_P, │ │ │ │ │ -188 RENORM │ │ │ │ │ -_1_8_9 }; │ │ │ │ │ -190 │ │ │ │ │ -192 _U_n_i_t_3 retract(const Vector2& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_2_> H = boost::none) │ │ │ │ │ -const; │ │ │ │ │ -193 │ │ │ │ │ -195 Vector2 localCoordinates(const _U_n_i_t_3& s) const; │ │ │ │ │ -196 │ │ │ │ │ -198 │ │ │ │ │ -199private: │ │ │ │ │ -200 │ │ │ │ │ -203 │ │ │ │ │ -_2_0_4 friend class boost::serialization::access; │ │ │ │ │ -205 template │ │ │ │ │ -206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -207 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ -208 } │ │ │ │ │ -209 │ │ │ │ │ -211 │ │ │ │ │ -212public: │ │ │ │ │ -213 _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 │ │ │ │ │ -214}; │ │ │ │ │ -215 │ │ │ │ │ -216// Define GTSAM traits │ │ │ │ │ -_2_1_7template<> struct _t_r_a_i_t_s<_U_n_i_t_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ -_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_:_:_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_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_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_:_:_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_:_:_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_3 │ │ │ │ │ -Unit3(const Unit3 &u) │ │ │ │ │ -Copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_U_n_i_t_3 │ │ │ │ │ -Unit3() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -friend Point3 operator*(double s, const Unit3 &d) │ │ │ │ │ -Return scaled direction as Point3. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Dimensionality of tangent space = 2 DOF. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality of tangent space = 2 DOF. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:182 │ │ │ │ │ -_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_:_:_U_n_i_t_3_:_:_C_o_o_r_d_i_n_a_t_e_s_M_o_d_e │ │ │ │ │ -CoordinatesMode │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_E_X_P_M_A_P │ │ │ │ │ -@ EXPMAP │ │ │ │ │ -Use the exponential map to retract. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_c_r_o_s_s │ │ │ │ │ -Unit3 cross(const Unit3 &q) const │ │ │ │ │ -Cross-product between two Unit3s. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_c_r_o_s_s │ │ │ │ │ -Point3 cross(const Point3 &q) const │ │ │ │ │ -Cross-product w Point3. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -Unit3 & operator=(const Unit3 &u) │ │ │ │ │ -Copy assignment. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:85 │ │ │ │ │ +_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 │ │ │ │ │ - * UUnniitt33..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/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/Pose3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,71 +98,72 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    Pose2.h File Reference
    │ │ │ │ +
    Pose3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    2D Pose │ │ │ │ +

    3D Pose │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Pose2
     A 2D pose (Point2,Rot2) More...
    class  gtsam::Pose3
     A 3D pose (R,t) : (Rot3,Point3) More...
     
    struct  gtsam::Pose2::ChartAtOrigin
    struct  gtsam::Pose3::ChartAtOrigin
     
    struct  gtsam::traits< Pose2 >
    struct  gtsam::traits< Pose3 >
     
    struct  gtsam::traits< const Pose2 >
    struct  gtsam::traits< const Pose3 >
     
    struct  gtsam::Bearing< Pose2, T >
    struct  gtsam::Bearing< Pose3, Point3 >
     
    struct  gtsam::Range< Pose2, T >
    struct  gtsam::Bearing< Pose3, Pose3 >
     
    struct  gtsam::Range< Pose3, 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 >
     
    │ │ │ │ +using gtsam::Pose3Pair = std::pair< Pose3, Pose3 >
     
    │ │ │ │ +using gtsam::Pose3Pairs = std::vector< std::pair< Pose3, Pose3 > >
     
    │ │ │ │ +typedef std::vector< Pose3gtsam::Pose3Vector
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ Functions

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

    Detailed Description

    │ │ │ │ -

    2D Pose

    │ │ │ │ -
    Author
    : Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -: Richard Roberts
    │ │ │ │ +

    3D Pose

    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,46 +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 | _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_._._. │ │ │ │ │ +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_2 │ │ │ │ │ -  A 2D pose (Point2,_R_o_t_2) _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_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +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_2_ _> │ │ │ │ │ +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_2_ _> │ │ │ │ │ +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_2_,_ _T_ _> │ │ │ │ │ +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_:_:_R_a_n_g_e_<_ _P_o_s_e_2_,_ _T_ _> │ │ │ │ │ +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::::PPoossee22PPaaiirr = std::pair< _P_o_s_e_2, _P_o_s_e_2 > │ │ │ │ │ + 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 > > │ │ │ │ │   │ │ │ │ │ -using  ggttssaamm::::PPoossee22PPaaiirrss = std::vector< Pose2Pair > │ │ │ │ │ +typedef std::vector< _P_o_s_e_3 >  ggttssaamm::::PPoossee33VVeeccttoorr │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ template<> │ │ │ │ │ - Matrix  ggttssaamm::::wweeddggee<< PPoossee22 >> (const Vector &xi) │ │ │ │ │ -  specialization for pose2 wedge function (generic template in _L_i_e_._h) │ │ │ │ │ + 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 ********** │ │ │ │ │ -2D Pose │ │ │ │ │ - Author │ │ │ │ │ - : Frank Dellaert │ │ │ │ │ - : Richard Roberts │ │ │ │ │ +3D Pose │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_2_._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/a00398.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,9 @@ │ │ │ │ │ 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::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 >", "a00398.html#aefb606eb24889c0f418acf1515a2c723", 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/Pose3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,337 +98,334 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Pose2.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// \callgraph
    │ │ │ │ -
    20
    │ │ │ │ -
    21#pragma once
    │ │ │ │ -
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/geometry/Rot2.h>
    │ │ │ │ -
    26#include <gtsam/base/Lie.h>
    │ │ │ │ -
    27#include <gtsam/dllexport.h>
    │ │ │ │ +
    17// \callgraph
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/config.h>
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    25#include <gtsam/base/Lie.h>
    │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    │ │ │ │ -
    36class Pose2: public LieGroup<Pose2, 3> {
    │ │ │ │ -
    37
    │ │ │ │ +
    29class Pose2;
    │ │ │ │ +
    30// forward declare
    │ │ │ │ +
    31
    │ │ │ │ +
    │ │ │ │ +
    37class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
    │ │ │ │
    38public:
    │ │ │ │
    39
    │ │ │ │ -
    41 typedef Rot2 Rotation;
    │ │ │ │ -
    42 typedef Point2 Translation;
    │ │ │ │ +
    41 typedef Rot3 Rotation;
    │ │ │ │ +
    42 typedef Point3 Translation;
    │ │ │ │
    43
    │ │ │ │
    44private:
    │ │ │ │
    45
    │ │ │ │ -
    46 Rot2 r_;
    │ │ │ │ -
    47 Point2 t_;
    │ │ │ │ +
    46 Rot3 R_;
    │ │ │ │ +
    47 Point3 t_;
    │ │ │ │
    48
    │ │ │ │
    49public:
    │ │ │ │
    50
    │ │ │ │
    53
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    56 r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
    │ │ │ │ -
    57 }
    │ │ │ │ +
    55 Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
    │ │ │ │ +
    56
    │ │ │ │ +
    │ │ │ │ +
    58 Pose3(const Pose3& pose) :
    │ │ │ │ +
    59 R_(pose.R_), t_(pose.t_) {
    │ │ │ │ +
    60 }
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    63 Pose3(const Rot3& R, const Point3& t) :
    │ │ │ │ +
    64 R_(R), t_(t) {
    │ │ │ │ +
    65 }
    │ │ │ │ +
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    117
    │ │ │ │ +
    132 Pose3 interpolateRt(const Pose3& T, double t) const;
    │ │ │ │ +
    133
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 };
    │ │ │ │ -
    │ │ │ │ +
    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 // 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);
    │ │ │ │ +
    199
    │ │ │ │ +
    201 static Matrix6 ExpmapDerivative(const Vector6& xi);
    │ │ │ │
    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;
    │ │ │ │ +
    204 static Matrix6 LogmapDerivative(const Pose3& xi);
    │ │ │ │ +
    205
    │ │ │ │ +
    206 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    231 Matrix transformFrom(const Matrix& points) const;
    │ │ │ │ -
    232
    │ │ │ │ -
    │ │ │ │ -
    234 inline Point2 operator*(const Point2& point) const {
    │ │ │ │ -
    235 return transformFrom(point);
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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 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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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> {};
    │ │ │ │ +
    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
    │ │ │ │ -
    370template <typename T>
    │ │ │ │ -
    371struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
    │ │ │ │ -
    372
    │ │ │ │ -
    373} // namespace gtsam
    │ │ │ │ -
    374
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    3D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Matrix wedge< Pose2 >(const Vector &xi)
    specialization for pose2 wedge function (generic template in Lie.h)
    Definition Pose2.h:341
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Matrix wedge< Pose3 >(const Vector &xi)
    wedge for Pose3:
    Definition Pose3.h:417
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    Definition BearingRange.h:34
    │ │ │ │
    Definition BearingRange.h:40
    │ │ │ │
    Definition BearingRange.h:180
    │ │ │ │
    Definition BearingRange.h:194
    │ │ │ │
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ -
    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
    │ │ │ │ -
    Pose2 operator*(const Pose2 &p2) const
    compose syntactic sugar
    Definition Pose2.h:128
    │ │ │ │ -
    GTSAM_EXPORT void print(const std::string &s="") const
    print with optional string
    Definition Pose2.cpp:50
    │ │ │ │ -
    const Rot2 & rotation() const
    rotation
    Definition Pose2.h:261
    │ │ │ │ -
    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
    │ │ │ │ -
    static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
    Derivative of Logmap.
    Definition Pose2.cpp:179
    │ │ │ │ -
    double y() const
    get y
    Definition Pose2.h:246
    │ │ │ │ -
    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_EXPORT Pose2 inverse() const
    inverse
    Definition Pose2.cpp:201
    │ │ │ │ -
    GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
    Output stream operator.
    Definition Pose2.cpp:55
    │ │ │ │ -
    Point2 operator*(const Point2 &point) const
    syntactic sugar for transformFrom
    Definition Pose2.h:234
    │ │ │ │ -
    Pose2(double x, double y, double theta)
    construct from (x,y,theta)
    Definition Pose2.h:68
    │ │ │ │ -
    const Point2 & t() const
    translation
    Definition Pose2.h:252
    │ │ │ │ -
    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
    │ │ │ │ -
    Pose2(double theta, const Point2 &t)
    construct from rotation and translation
    Definition Pose2.h:73
    │ │ │ │ -
    double x() const
    get x
    Definition Pose2.h:243
    │ │ │ │ -
    const Rot2 & r() const
    rotation
    Definition Pose2.h:255
    │ │ │ │ -
    Pose2(const Vector &v)
    Construct from canonical coordinates (Lie algebra)
    Definition Pose2.h:91
    │ │ │ │ -
    Vector3 Adjoint(const Vector3 &xi) const
    Apply AdjointMap to twist xi.
    Definition Pose2.h:149
    │ │ │ │ -
    static Matrix3 wedge(double vx, double vy, double w)
    wedge for SE(2):
    Definition Pose2.h:183
    │ │ │ │ -
    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
    │ │ │ │ -
    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_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
    │ │ │ │ -
    double theta() const
    get theta
    Definition Pose2.h:249
    │ │ │ │ -
    static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
    Derivative of Expmap.
    Definition Pose2.cpp:148
    │ │ │ │ -
    Rot2 Rotation
    Pose Concept requirements.
    Definition Pose2.h:41
    │ │ │ │ -
    Pose2()
    default constructor = origin
    Definition Pose2.h:55
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static Pose2 Identity()
    identity for group operation
    Definition Pose2.h:122
    │ │ │ │ -
    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
    │ │ │ │ -
    Pose2(const Rot2 &r, const Point2 &t)
    construct from r,t
    Definition Pose2.h:78
    │ │ │ │ -
    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
    │ │ │ │ -
    Pose2(const Pose2 &pose)
    copy constructor
    Definition Pose2.h:60
    │ │ │ │ -
    const Point2 & translation() const
    translation
    Definition Pose2.h:258
    │ │ │ │ -
    Pose2(const Matrix &T)
    Constructor from 3*3 matrix.
    Definition Pose2.h:81
    │ │ │ │ -
    Definition Pose2.h:198
    │ │ │ │ -
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ -
    double theta() const
    return angle (RADIANS)
    Definition Rot2.h:187
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    static Pose3 Identity()
    identity for group operation
    Definition Pose3.h:106
    │ │ │ │ +
    Pose3(const Pose3 &pose)
    Copy constructor.
    Definition Pose3.h:58
    │ │ │ │ +
    Pose3()
    Default constructor is origin.
    Definition Pose3.h:55
    │ │ │ │ +
    static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
    wedge for Pose3:
    Definition Pose3.h:233
    │ │ │ │ +
    Pose3(const Rot3 &R, const Point3 &t)
    Construct from R,t.
    Definition Pose3.h:63
    │ │ │ │ +
    Pose3(const Matrix &T)
    Constructor from 4*4 matrix.
    Definition Pose3.h:71
    │ │ │ │ +
    double z() const
    get z
    Definition Pose3.h:302
    │ │ │ │ +
    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
    │ │ │ │ +
    Pose3 operator*(const Pose3 &T) const
    compose syntactic sugar
    Definition Pose3.h:114
    │ │ │ │ +
    Rot3 Rotation
    Pose Concept requirements.
    Definition Pose3.h:41
    │ │ │ │ +
    double y() const
    get y
    Definition Pose3.h:297
    │ │ │ │ +
    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
    │ │ │ │ +
    Point3 operator*(const Point3 &point) const
    syntactic sugar for transformFrom
    Definition Pose3.h:260
    │ │ │ │ +
    double x() const
    get x
    Definition Pose3.h:292
    │ │ │ │ +
    Definition Pose3.h:207
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,280 +1,305 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Pose2.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// \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 │ │ │ │ │ +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 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_6class _P_o_s_e_2: public _L_i_e_G_r_o_u_p { │ │ │ │ │ -37 │ │ │ │ │ +29class Pose2; │ │ │ │ │ +30// forward declare │ │ │ │ │ +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_2 _R_o_t_a_t_i_o_n; │ │ │ │ │ -42 typedef _P_o_i_n_t_2 Translation; │ │ │ │ │ +_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_2 r_; │ │ │ │ │ -47 _P_o_i_n_t_2 t_; │ │ │ │ │ +46 _R_o_t_3 R_; │ │ │ │ │ +47 _P_o_i_n_t_3 t_; │ │ │ │ │ 48 │ │ │ │ │ 49public: │ │ │ │ │ 50 │ │ │ │ │ 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_) {} │ │ │ │ │ +_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_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 } │ │ │ │ │ +_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 │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ -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 = │ │ │ │ │ +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 } │ │ │ │ │ +117 │ │ │ │ │ +132 _P_o_s_e_3 interpolateRt(const _P_o_s_e_3& T, double t) const; │ │ │ │ │ +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); │ │ │ │ │ +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); │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 }; │ │ │ │ │ +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 // 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); │ │ │ │ │ +199 │ │ │ │ │ +201 static Matrix6 ExpmapDerivative(const Vector6& xi); │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +_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 │ │ │ │ │ -_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; │ │ │ │ │ +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 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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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 {}; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ _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 │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +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_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ operator*,... │ │ │ │ │ @@ -296,177 +321,82 @@ │ │ │ │ │ _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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _P_o_s_e_2_._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/a00401_source.html │ │ │ │ @@ -114,15 +114,15 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ + │ │ │ │
    22#include <gtsam/base/VectorSpace.h>
    │ │ │ │
    23#include <boost/serialization/nvp.hpp>
    │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │
    │ │ │ │
    32class GTSAM_EXPORT StereoPoint2 {
    │ │ │ │ @@ -216,16 +216,16 @@ │ │ │ │
    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)); }
    │ │ │ │ +
    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:
    │ │ │ │ @@ -247,16 +247,16 @@ │ │ │ │
    163
    │ │ │ │
    164template<>
    │ │ │ │
    165struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
    │ │ │ │
    166
    │ │ │ │
    167template<>
    │ │ │ │
    168struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
    │ │ │ │
    169}
    │ │ │ │ -
    T between(const T &t1, const T &t2)
    binary functions
    Definition lieProxies.h:36
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    T between(const T &t1, const T &t2)
    binary functions
    Definition lieProxies.h:36
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    VectorSpace provides both Testable and VectorSpaceTraits.
    Definition VectorSpace.h:207
    │ │ │ │
    A 2D stereo point, v will be same for rectified images.
    Definition StereoPoint2.h:32
    │ │ │ │
    double uR() const
    get uR
    Definition StereoPoint2.h:109
    │ │ │ ├── ./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/SO4.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,48 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Cal3DS2.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    SO4.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    4*4 matrix representation of SO(4) │ │ │ │ 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...
     
    struct  gtsam::traits< Cal3DS2 >
     
    struct  gtsam::traits< const Cal3DS2 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ 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

    │ │ │ │ -

    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
    │ │ │ │ +

    4*4 matrix representation of SO(4)

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Luca Carlone
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,40 +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 │ │ │ │ │ -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 │ │ │ │ │ - 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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -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 │ │ │ │ │ +4*4 matrix representation of SO(4) │ │ │ │ │ Author │ │ │ │ │ - ydjian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2_._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/a00404.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00404 = [ │ │ │ │ │ - ["gtsam::traits< Cal3DS2 >", "a02984.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3DS2 >", "a02988.html", null] │ │ │ │ │ + ["stiefel", "a00404.html#a3e57e4771f13855a495ec2c6454c9121", null], │ │ │ │ │ + ["topLeft", "a00404.html#a8d21728e3da4cf1a96e1b6f87a97da47", null] │ │ │ │ │ ]; │ │ │ ├── ./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/Cal3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,52 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Rot2.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    2D rotation │ │ │ │ +

    Common code for all Calibration models. │ │ │ │ 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 >
    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

    │ │ │ │ -

    2D rotation

    │ │ │ │ -
    Date
    Dec 9, 2009
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -John Lambert
    │ │ │ │ +

    Common code for all Calibration models.

    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 | _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_:_:_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_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -2D rotation │ │ │ │ │ - Date │ │ │ │ │ - Dec 9, 2009 │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - John Lambert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_2_._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/a00407.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00407 = [ │ │ │ │ │ - ["gtsam::Rot2::ChartAtOrigin", "a03216.html", null], │ │ │ │ │ - ["gtsam::traits< Rot2 >", "a03220.html", null], │ │ │ │ │ - ["gtsam::traits< const Rot2 >", "a03224.html", null] │ │ │ │ │ + ["calibrateJacobians", "a00407.html#a9ac814fcc4484bce32e4616d69afb225", 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/Cal3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,230 +98,177 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Rot2.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
    │ │ │ │ - │ │ │ │ -
    23#include <gtsam/base/Lie.h>
    │ │ │ │ -
    24#include <boost/optional.hpp>
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │
    25
    │ │ │ │ -
    26#include <random>
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ -
    29
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    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);
    │ │ │ │ +
    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
    │ │ │ │ -
    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);
    │ │ │ │ +
    75 public:
    │ │ │ │ +
    76 enum { dimension = 5 };
    │ │ │ │ +
    78 using shared_ptr = boost::shared_ptr<Cal3>;
    │ │ │ │ +
    79
    │ │ │ │ +
    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
    │ │ │ │ +
    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 static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
    │ │ │ │ +
    129 virtual void print(const std::string& s = "") const;
    │ │ │ │
    130
    │ │ │ │ -
    132 Matrix1 AdjointMap() const { return I_1x1; }
    │ │ │ │ +
    132 bool equals(const Cal3& K, double tol = 10e-9) const;
    │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    137
    │ │ │ │ +
    139 inline double fx() const { return fx_; }
    │ │ │ │ +
    140
    │ │ │ │ +
    142 inline double fy() const { return fy_; }
    │ │ │ │
    143
    │ │ │ │ -
    144 // Chart at origin simply uses exponential map and its inverse
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    159
    │ │ │ │ -
    163 Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
    │ │ │ │ -
    164 OptionalJacobian<2, 2> H2 = boost::none) const;
    │ │ │ │ +
    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 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
    │ │ │ │ +
    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 Point2 unit() const {
    │ │ │ │ -
    183 return Point2(c_, s_);
    │ │ │ │ -
    184 }
    │ │ │ │ -
    │ │ │ │ -
    185
    │ │ │ │ -
    │ │ │ │ -
    187 double theta() const {
    │ │ │ │ -
    188 return ::atan2(s_, c_);
    │ │ │ │ -
    189 }
    │ │ │ │ -
    │ │ │ │ +
    182 inline virtual size_t dim() const { return Dim(); }
    │ │ │ │ +
    183
    │ │ │ │ +
    185 inline static size_t Dim() { return dimension; }
    │ │ │ │ +
    186
    │ │ │ │
    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> {};
    │ │ │ │ -
    232
    │ │ │ │ -
    233} // gtsam
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │ -
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    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
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ -
    Rot2 operator*(const Rot2 &R) const
    Compose - make a new rotation by adding angles.
    Definition Rot2.h:117
    │ │ │ │ -
    double c() const
    return cos
    Definition Rot2.h:198
    │ │ │ │ -
    static Matrix ExpmapDerivative(const Vector &)
    Left-trivialized derivative of the exponential map.
    Definition Rot2.h:135
    │ │ │ │ -
    Point2 unit() const
    Creates a unit vector as a Point2.
    Definition Rot2.h:182
    │ │ │ │ -
    double theta() const
    return angle (RADIANS)
    Definition Rot2.h:187
    │ │ │ │ -
    Rot2 inverse() const
    The inverse rotation - negative angle.
    Definition Rot2.h:114
    │ │ │ │ -
    Point2 operator*(const Point2 &p) const
    syntactic sugar for rotate
    Definition Rot2.h:167
    │ │ │ │ -
    double s() const
    return sin
    Definition Rot2.h:203
    │ │ │ │ -
    static Rot2 Identity()
    Identity.
    Definition Rot2.h:111
    │ │ │ │ -
    double degrees() const
    return angle (DEGREES)
    Definition Rot2.h:192
    │ │ │ │ -
    static Matrix LogmapDerivative(const Vector &)
    Left-trivialized derivative inverse of the exponential map.
    Definition Rot2.h:140
    │ │ │ │ -
    Matrix1 AdjointMap() const
    Calculate Adjoint map.
    Definition Rot2.h:132
    │ │ │ │ -
    Rot2(double theta)
    Constructor from angle in radians == exponential map at identity.
    Definition Rot2.h:59
    │ │ │ │ -
    static Rot2 fromDegrees(double theta)
    Named constructor from angle in degrees.
    Definition Rot2.h:67
    │ │ │ │ -
    Rot2()
    default constructor, zero rotation
    Definition Rot2.h:53
    │ │ │ │ -
    Rot2(const Rot2 &r)
    copy constructor
    Definition Rot2.h:56
    │ │ │ │ -
    static Rot2 fromAngle(double theta)
    Named constructor from angle in radians.
    Definition Rot2.h:62
    │ │ │ │ -
    Definition Rot2.h:145
    │ │ │ │ +
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ +
    Cal3(double fx, double fy, double s, double u0, double v0)
    constructor from doubles
    Definition Cal3.h:87
    │ │ │ │ +
    virtual Matrix3 K() const
    return calibration matrix K
    Definition Cal3.h:167
    │ │ │ │ +
    Cal3(const Vector5 &d)
    constructor from vector
    Definition Cal3.h:91
    │ │ │ │ +
    virtual ~Cal3()
    Virtual destructor.
    Definition Cal3.h:103
    │ │ │ │ +
    Vector5 vector() const
    vectorized form (column-wise)
    Definition Cal3.h:160
    │ │ │ │ +
    Cal3()=default
    Create a default calibration that leaves coordinates unchanged.
    │ │ │ │ +
    double px() const
    image center in x
    Definition Cal3.h:151
    │ │ │ │ +
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3.h:185
    │ │ │ │ +
    Point2 principalPoint() const
    return the principal point
    Definition Cal3.h:157
    │ │ │ │ +
    double fx() const
    focal length x
    Definition Cal3.h:139
    │ │ │ │ +
    double py() const
    image center in y
    Definition Cal3.h:154
    │ │ │ │ +
    double skew() const
    skew
    Definition Cal3.h:148
    │ │ │ │ +
    virtual size_t dim() const
    return DOF, dimensionality of tangent space
    Definition Cal3.h:182
    │ │ │ │ +
    double aspectRatio() const
    aspect ratio
    Definition Cal3.h:145
    │ │ │ │ +
    double fy() const
    focal length y
    Definition Cal3.h:142
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,277 +1,224 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Rot2.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_/_P_o_i_n_t_2_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -24#include │ │ │ │ │ +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 │ │ │ │ │ -26#include │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 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 { │ │ │ │ │ -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) {} │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 _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 static Vector1 Logmap(const _R_o_t_2& r, ChartJacobian H = boost::none); │ │ │ │ │ +129 virtual void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ 130 │ │ │ │ │ -_1_3_2 Matrix1 _A_d_j_o_i_n_t_M_a_p() const { return I_1x1; } │ │ │ │ │ +132 bool _e_q_u_a_l_s(const _C_a_l_3& K, double tol = 10e-9) const; │ │ │ │ │ 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 } │ │ │ │ │ +137 │ │ │ │ │ +_1_3_9 inline double _f_x() const { return fx_; } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 inline double _f_y() const { return fy_; } │ │ │ │ │ 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 │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 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 │ │ │ │ │ +_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 _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 } │ │ │ │ │ +_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 │ │ │ │ │ -_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 {}; │ │ │ │ │ -232 │ │ │ │ │ -233} // gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_2_._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/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/StereoPoint2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,40 +96,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Cal3_S2.cpp File Reference
    │ │ │ │ +
    StereoPoint2.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 Cal3_S2 &cal)
     
    │ │ │ │ +ostream & gtsam::operator<< (ostream &os, const StereoPoint2 &p)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

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

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Jan 26, 2010
    │ │ │ │ +
    Author
    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 │ │ │ │ │ -Cal3_S2.cpp File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ +StereoPoint2.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___S_2 &cal) │ │ │ │ │ +ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_t_e_r_e_o_P_o_i_n_t_2 &p) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ + Date │ │ │ │ │ + Jan 26, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_._c_p_p │ │ │ │ │ + * _S_t_e_r_e_o_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/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/SphericalCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3DS2_Base.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    SphericalCamera.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

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

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::EmptyCal
     Empty calibration. More...
     
    class  gtsam::SphericalCamera
     A spherical camera class that has a Pose3 and measures bearing vectors. More...
     
    struct  gtsam::traits< SphericalCamera >
     
    struct  gtsam::traits< const SphericalCamera >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ Namespaces

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

    │ │ │ │ -Functions

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

    Detailed Description

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

    Calibrated camera with spherical projection.

    │ │ │ │ +
    Date
    Aug 26, 2021
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +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 │ │ │ │ │ -Cal3DS2_Base.cpp File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ +_G_o_ _t_o_ _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_:_:_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_<_ _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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2___B_a_s_e &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Calibrated camera with spherical projection. │ │ │ │ │ Date │ │ │ │ │ - Feb 28, 2010 │ │ │ │ │ + Aug 26, 2021 │ │ │ │ │ Author │ │ │ │ │ - ydjian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00416.html │ │ │ │ @@ -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/Cal3_S2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    CalibratedCamera.cpp File Reference
    │ │ │ │ +
    Cal3_S2.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Calibrated camera for which only pose is unknown. │ │ │ │ +

    The most common 5DOF 3D->2D calibration. │ │ │ │ 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 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Calibrated camera for which only pose is unknown.

    │ │ │ │ -
    Date
    Aug 17, 2009
    │ │ │ │ +

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

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,29 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_:_:_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_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ - Date │ │ │ │ │ - Aug 17, 2009 │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ 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 │ │ │ │ │ + * _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/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/Cal3DS2_Base.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Line3.h File Reference
    │ │ │ │ +
    Cal3DS2_Base.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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.
     
    │ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2_Base &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    4 dimensional manifold of 3D lines

    │ │ │ │ -
    Author
    Akshay Krishnan
    │ │ │ │ +
    Date
    Feb 28, 2010
    │ │ │ │ +
    Author
    ydjian
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 | _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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 │ │ │ │ │ -_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. │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2___B_a_s_e &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -4 dimensional manifold of 3D lines │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2010 │ │ │ │ │ Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ydjian │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _L_i_n_e_3_._h │ │ │ │ │ + * _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/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/SimpleCamera.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Similarity3.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SimpleCamera.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation of Similarity3 transform. │ │ │ │ +

    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 Similarity3 &p)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Implementation of Similarity3 transform.

    │ │ │ │ -
    Author
    Paul Drews
    │ │ │ │ -
    │ │ │ │ -John Lambert
    │ │ │ │ +

    A simple camera class with a Cal3_S2 calibration.

    │ │ │ │ +
    Date
    June 30, 2012
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +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 │ │ │ │ │ -Similarity3.cpp File Reference │ │ │ │ │ -Implementation of Similarity3 transform. _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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ + Date │ │ │ │ │ + June 30, 2012 │ │ │ │ │ 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 │ │ │ │ │ + * _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/a00431.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Similarity2.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Pose3.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation of Similarity2 transform. │ │ │ │ +

    3D Pose │ │ │ │ More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::Similarity2
     2D similarity transform More...
     
    struct  gtsam::Similarity2::ChartAtOrigin
     Chart at the origin. More...
     
    struct  gtsam::traits< Similarity2 >
     
    struct  gtsam::traits< const Similarity2 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

    │ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Pose3 &pose)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Implementation of Similarity2 transform.

    │ │ │ │ -
    Author
    John Lambert, Varun Agrawal
    │ │ │ │ +

    3D Pose

    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -Similarity2.h File Reference │ │ │ │ │ -Implementation of Similarity2 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_i_m_i_l_a_r_i_t_y_2 │ │ │ │ │ -  2D similarity transform _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_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_i_m_i_l_a_r_i_t_y_2_ _> │ │ │ │ │ -  │ │ │ │ │ -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_2_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Pose3.cpp File Reference │ │ │ │ │ +3D 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 _P_o_s_e_3 &pose) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation of Similarity2 transform. │ │ │ │ │ - Author │ │ │ │ │ - John Lambert, Varun Agrawal │ │ │ │ │ +3D Pose │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_2_._h │ │ │ │ │ + * _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/a00437_source.html │ │ │ │ @@ -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/geometry/Cal3DS2_Base.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,211 +98,151 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SO3.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
    │ │ │ │ -
    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.
    │ │ │ │ -
    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);
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/geometry/Cal3.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
    │ │ │ │ -
    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);
    │ │ │ │ +
    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 {}
    │ │ │ │
    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).
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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_; }
    │ │ │ │ +
    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 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};
    │ │ │ │ +
    173};
    │ │ │ │
    │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    N*N matrix representation of SO(N).
    │ │ │ │ +
    174}
    │ │ │ │ +
    Common code for all Calibration models.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Functor implementing Exponential map.
    Definition SO3.h:134
    │ │ │ │ -
    Functor that implements Exponential map and its derivatives.
    Definition SO3.h:155
    │ │ │ │ -
    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_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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ -
    static SO ChordalMean(const std::vector< SO > &rotations)
    Named constructor that finds chordal mean , currently only defined for SO3.
    │ │ │ │ -
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ -
    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
    │ │ │ │ -
    static SO AxisAngle(const Vector3 &axis, double theta)
    Constructor from axis and angle. Only defined for SO3.
    │ │ │ │ -
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ -
    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
    │ │ │ │ -
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ -
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ -
    static SO ClosestTo(const MatrixNN &M)
    Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
    │ │ │ │ +
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ +
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │ +
    Cal3DS2_Base()=default
    Default Constructor with only unit focal length.
    │ │ │ │ +
    double p2() const
    Second tangential distortion coefficient.
    Definition Cal3DS2_Base.h:110
    │ │ │ │ +
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3DS2_Base.h:142
    │ │ │ │ +
    virtual boost::shared_ptr< Cal3DS2_Base > clone() const
    Definition Cal3DS2_Base.h:149
    │ │ │ │ +
    double k2() const
    Second distortion coefficient.
    Definition Cal3DS2_Base.h:104
    │ │ │ │ +
    double k1() const
    First distortion coefficient.
    Definition Cal3DS2_Base.h:101
    │ │ │ │ +
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3DS2_Base.h:139
    │ │ │ │ +
    double p1() const
    First tangential distortion coefficient.
    Definition Cal3DS2_Base.h:107
    │ │ │ │ +
    Vector4 k() const
    return distortion parameter vector
    Definition Cal3DS2_Base.h:113
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,266 +1,184 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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) │ │ │ │ │ 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_/_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. │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 {} │ │ │ │ │ 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). │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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_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 │ │ │ │ │ -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_; } │ │ │ │ │ +_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 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). │ │ │ │ │ +173}; │ │ │ │ │ +174} │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_3_._h │ │ │ │ │ + * 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/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/Point2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,63 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    Cyclic.cpp File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Point2.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Cyclic group implementation. │ │ │ │ +

    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< Point2gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
     
    │ │ │ │ +list< Point2gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
     
    list< Point2gtsam::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

    │ │ │ │ -

    Cyclic group implementation.

    │ │ │ │ +

    2D Point

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,48 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ +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. │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ + Point2Pair &p) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Cyclic group implementation. │ │ │ │ │ +2D Point │ │ │ │ │ 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 │ │ │ │ │ + * _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/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/Cal3Fisheye.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,56 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    CalibratedCamera.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3Fisheye.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Functions

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

    Detailed Description

    │ │ │ │ -

    Calibrated camera for which only pose is unknown.

    │ │ │ │ -
    Date
    Aug 17, 2009
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Apr 8, 2020
    │ │ │ │ +
    Author
    ghaggin
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -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_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3Fisheye.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_F_i_s_h_e_y_e &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2009 │ │ │ │ │ + Apr 8, 2020 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ghaggin │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/SimpleCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,92 +94,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    Point2.h File Reference
    │ │ │ │ +Typedefs
    │ │ │ │ +
    SimpleCamera.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    2D Point │ │ │ │ +

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    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< Point2gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
     
    │ │ │ │ -list< Point2gtsam::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< Point2gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
     Intersect 2 circles.
     
    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 Point

    │ │ │ │ +

    A simple camera class with a Cal3_S2 calibration.

    │ │ │ │ +
    Date
    Aug 16, 2009
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,70 +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 │ │ │ │ │ -Point2.h File Reference │ │ │ │ │ -2D Point _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_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -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. │ │ │ │ │ +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 Point │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ + Date │ │ │ │ │ + Aug 16, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._h │ │ │ │ │ + * _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/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] │ │ │ │ │ + ["PinholeCameraCal3_S2", "a00449.html#a3a2e8c622e65623a6853c84073bdb4f2", 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/SimpleCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,116 +98,141 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Point2.h
    │ │ │ │ +
    SimpleCamera.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/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);
    │ │ │ │ -
    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 <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ + │ │ │ │ +
    35 using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
    │ │ │ │ +
    36 using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
    │ │ │ │ +
    37 using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
    │ │ │ │ +
    38 using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
    │ │ │ │ +
    39
    │ │ │ │ +
    40#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
    │ │ │ │ +
    45class GTSAM_EXPORT SimpleCamera : public PinholeCameraCal3_S2 {
    │ │ │ │ +
    46
    │ │ │ │ +
    47 typedef PinholeCamera<Cal3_S2> Base;
    │ │ │ │ +
    48 typedef boost::shared_ptr<SimpleCamera> shared_ptr;
    │ │ │ │ +
    49
    │ │ │ │ +
    50public:
    │ │ │ │ +
    51
    │ │ │ │ +
    54
    │ │ │ │ +
    56 SimpleCamera() :
    │ │ │ │ +
    57 Base() {
    │ │ │ │ +
    58 }
    │ │ │ │ +
    59
    │ │ │ │ +
    61 explicit SimpleCamera(const Pose3& pose) :
    │ │ │ │ +
    62 Base(pose) {
    │ │ │ │ +
    63 }
    │ │ │ │ +
    64
    │ │ │ │ +
    66 SimpleCamera(const Pose3& pose, const Cal3_S2& K) :
    │ │ │ │ +
    67 Base(pose, K) {
    │ │ │ │ +
    68 }
    │ │ │ │ +
    69
    │ │ │ │ +
    73
    │ │ │ │ +
    81 static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2,
    │ │ │ │ +
    82 double height) {
    │ │ │ │ +
    83 return SimpleCamera(Base::LevelPose(pose2, height), K);
    │ │ │ │ +
    84 }
    │ │ │ │ +
    85
    │ │ │ │ +
    87 static SimpleCamera Level(const Pose2& pose2, double height) {
    │ │ │ │ +
    88 return SimpleCamera::Level(Cal3_S2(), pose2, height);
    │ │ │ │ +
    89 }
    │ │ │ │ +
    90
    │ │ │ │ +
    100 static SimpleCamera Lookat(const Point3& eye, const Point3& target,
    │ │ │ │ +
    101 const Point3& upVector, const Cal3_S2& K = Cal3_S2()) {
    │ │ │ │ +
    102 return SimpleCamera(Base::LookatPose(eye, target, upVector), K);
    │ │ │ │ +
    103 }
    │ │ │ │
    104
    │ │ │ │ +
    108
    │ │ │ │ +
    110 explicit SimpleCamera(const Vector &v) :
    │ │ │ │ +
    111 Base(v) {
    │ │ │ │ +
    112 }
    │ │ │ │ +
    113
    │ │ │ │ +
    115 SimpleCamera(const Vector &v, const Vector &K) :
    │ │ │ │ +
    116 Base(v, K) {
    │ │ │ │ +
    117 }
    │ │ │ │ +
    118
    │ │ │ │ +
    120 SimpleCamera::shared_ptr clone() const { return boost::make_shared<SimpleCamera>(*this); }
    │ │ │ │ +
    121
    │ │ │ │ +
    122
    │ │ │ │ +
    126
    │ │ │ │ +
    128 SimpleCamera retract(const Vector& d) const {
    │ │ │ │ +
    129 if ((size_t) d.size() == 6)
    │ │ │ │ +
    130 return SimpleCamera(this->pose().retract(d), calibration());
    │ │ │ │ +
    131 else
    │ │ │ │ +
    132 return SimpleCamera(this->pose().retract(d.head(6)),
    │ │ │ │ +
    133 calibration().retract(d.tail(calibration().dim())));
    │ │ │ │ +
    134 }
    │ │ │ │ +
    135
    │ │ │ │ +
    137
    │ │ │ │ +
    138};
    │ │ │ │ +
    139
    │ │ │ │ +
    141GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
    │ │ │ │ +
    142
    │ │ │ │ +
    143// manifold traits
    │ │ │ │ +
    144template <>
    │ │ │ │ +
    145struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
    │ │ │ │ +
    146
    │ │ │ │ +
    147template <>
    │ │ │ │ +
    148struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
    │ │ │ │ +
    149
    │ │ │ │ +
    150// range traits, used in RangeFactor
    │ │ │ │ +
    151template <typename T>
    │ │ │ │ +
    152struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
    │ │ │ │ +
    153
    │ │ │ │ +
    154#endif
    │ │ │ │ +
    155
    │ │ │ │ +
    156} // namespace gtsam
    │ │ │ │ +
    Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    Calibration used by Bundler.
    │ │ │ │ +
    Calibration of a fisheye camera.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    Unified Calibration Model, see Mei07icra for details.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    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
    │ │ │ │ -
    Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
    Calculate the two means of a set of Point2 pairs.
    Definition Point2.cpp:116
    │ │ │ │ -
    double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
    distance between two points
    Definition Point2.cpp:39
    │ │ │ │ -
    double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
    Distance of the point from the origin, with Jacobian.
    Definition Point2.cpp:27
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Definition BearingRange.h:40
    │ │ │ │ +
    gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2
    Convenient aliases for Pinhole camera classes with different calibrations.
    Definition SimpleCamera.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,140 +1,160 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Point2.h │ │ │ │ │ +SimpleCamera.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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 │ │ │ │ │ -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); │ │ │ │ │ -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 │ │ │ │ │ +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_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_D_S_2_._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_F_i_s_h_e_y_e_._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 <_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> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_4 using _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_>; │ │ │ │ │ +35 using PinholeCameraCal3Bundler = _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_>; │ │ │ │ │ +36 using PinholeCameraCal3DS2 = _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_>; │ │ │ │ │ +37 using PinholeCameraCal3Unified = _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_>; │ │ │ │ │ +38 using PinholeCameraCal3Fisheye = _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_>; │ │ │ │ │ +39 │ │ │ │ │ +40#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +45class GTSAM_EXPORT SimpleCamera : public _P_i_n_h_o_l_e_C_a_m_e_r_a_C_a_l_3___S_2 { │ │ │ │ │ +46 │ │ │ │ │ +47 typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3___S_2_> Base; │ │ │ │ │ +48 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +49 │ │ │ │ │ +50public: │ │ │ │ │ +51 │ │ │ │ │ +54 │ │ │ │ │ +56 SimpleCamera() : │ │ │ │ │ +57 Base() { │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +61 explicit SimpleCamera(const _P_o_s_e_3& pose) : │ │ │ │ │ +62 Base(pose) { │ │ │ │ │ +63 } │ │ │ │ │ +64 │ │ │ │ │ +66 SimpleCamera(const _P_o_s_e_3& pose, const _C_a_l_3___S_2& K) : │ │ │ │ │ +67 Base(pose, K) { │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +73 │ │ │ │ │ +81 static SimpleCamera Level(const _C_a_l_3___S_2 &K, const _P_o_s_e_2& pose2, │ │ │ │ │ +82 double height) { │ │ │ │ │ +83 return SimpleCamera(Base::LevelPose(pose2, height), K); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +87 static SimpleCamera Level(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ +88 return SimpleCamera::Level(_C_a_l_3___S_2(), pose2, height); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +100 static SimpleCamera Lookat(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ +101 const _P_o_i_n_t_3& upVector, const _C_a_l_3___S_2& K = _C_a_l_3___S_2()) { │ │ │ │ │ +102 return SimpleCamera(Base::LookatPose(eye, target, upVector), K); │ │ │ │ │ +103 } │ │ │ │ │ 104 │ │ │ │ │ +108 │ │ │ │ │ +110 explicit SimpleCamera(const Vector &v) : │ │ │ │ │ +111 Base(v) { │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +115 SimpleCamera(const Vector &v, const Vector &K) : │ │ │ │ │ +116 Base(v, K) { │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +120 SimpleCamera::shared_ptr clone() const { return boost:: │ │ │ │ │ +make_shared(*this); } │ │ │ │ │ +121 │ │ │ │ │ +122 │ │ │ │ │ +126 │ │ │ │ │ +128 SimpleCamera retract(const Vector& d) const { │ │ │ │ │ +129 if ((size_t) d.size() == 6) │ │ │ │ │ +130 return SimpleCamera(this->pose().retract(d), calibration()); │ │ │ │ │ +131 else │ │ │ │ │ +132 return SimpleCamera(this->pose().retract(d.head(6)), │ │ │ │ │ +133 calibration().retract(d.tail(calibration().dim()))); │ │ │ │ │ +134 } │ │ │ │ │ +135 │ │ │ │ │ +137 │ │ │ │ │ +138}; │ │ │ │ │ +139 │ │ │ │ │ +141GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); │ │ │ │ │ +142 │ │ │ │ │ +143// manifold traits │ │ │ │ │ +144template <> │ │ │ │ │ +145struct traits : public internal::Manifold {}; │ │ │ │ │ +146 │ │ │ │ │ +147template <> │ │ │ │ │ +148struct traits : public internal::Manifold │ │ │ │ │ +{}; │ │ │ │ │ +149 │ │ │ │ │ +150// range traits, used in RangeFactor │ │ │ │ │ +151template │ │ │ │ │ +152struct Range : HasRange {}; │ │ │ │ │ +153 │ │ │ │ │ +154#endif │ │ │ │ │ +155 │ │ │ │ │ +156} // namespace gtsam │ │ │ │ │ +_C_a_l_3_D_S_2_._h │ │ │ │ │ +Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ +Cal3DS2_Base. │ │ │ │ │ +_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ +_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +_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_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ _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 │ │ │ │ │ +_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 │ │ │ │ │ +_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 │ │ │ │ │ -_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_:_:_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_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_:_:_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_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._h │ │ │ │ │ + * _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/a00455.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Pose3.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SOn.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    3D Pose │ │ │ │ +

    Definitions of dynamic specializations of SO(n) │ │ │ │ More...

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

    │ │ │ │ Namespaces

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

    │ │ │ │ -Functions

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

    Detailed Description

    │ │ │ │ -

    3D Pose

    │ │ │ │ +

    Definitions of dynamic specializations of SO(n)

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    Date
    March 2019
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +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 │ │ │ │ │ -Pose3.cpp File Reference │ │ │ │ │ -3D Pose _M_o_r_e_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_o_s_e_3 &pose) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D Pose │ │ │ │ │ +Definitions of dynamic specializations of SO(n) │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_3_._c_p_p │ │ │ │ │ + * _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/a00458.html │ │ │ │ @@ -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/Line3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,58 +97,54 @@ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Similarity3.h File Reference
    │ │ │ │ +
    Line3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation of Similarity3 transform. │ │ │ │ +

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

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Similarity3
     3D similarity transform More...
    class  gtsam::Line3
     A 3D line (R,a,b) : (Rot3,Scalar,Scalar) More...
     
    struct  gtsam::Similarity3::ChartAtOrigin
     Chart at the origin. More...
    struct  gtsam::traits< Line3 >
     
    struct  gtsam::traits< Similarity3 >
     
    struct  gtsam::traits< const Similarity3 >
    struct  gtsam::traits< const Line3 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ Functions

    │ │ │ │ -template<>
    Matrix gtsam::wedge< Similarity3 > (const Vector &xi)
     
    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

    │ │ │ │ -

    Implementation of Similarity3 transform.

    │ │ │ │ -
    Author
    Paul Drews
    │ │ │ │ +

    4 dimensional manifold of 3D lines

    │ │ │ │ +
    Author
    Akshay Krishnan
    │ │ │ │
    │ │ │ │ -John Lambert
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Similarity3.h File Reference │ │ │ │ │ -Implementation of Similarity3 transform. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_S_i_m_i_l_a_r_i_t_y_3 │ │ │ │ │ -  3D similarity transform _M_o_r_e_._._. │ │ │ │ │ + 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_:_:_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_<_ _L_i_n_e_3_ _> │ │ │ │ │   │ │ │ │ │ -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_ _L_i_n_e_3_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template<> │ │ │ │ │ - Matrix  ggttssaamm::::wweeddggee<< SSiimmiillaarriittyy33 >> (const Vector &xi) │ │ │ │ │ +_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 ********** │ │ │ │ │ -Implementation of Similarity3 transform. │ │ │ │ │ +4 dimensional manifold of 3D lines │ │ │ │ │ Author │ │ │ │ │ - Paul Drews │ │ │ │ │ - John Lambert │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ + 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_._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/a00458.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,5 @@ │ │ │ │ │ var a00458 = [ │ │ │ │ │ - ["gtsam::Similarity3", "a03264.html", "a03264"], │ │ │ │ │ - ["gtsam::Similarity3::ChartAtOrigin", "a03268.html", null], │ │ │ │ │ - ["gtsam::traits< Similarity3 >", "a03272.html", null], │ │ │ │ │ - ["gtsam::traits< const Similarity3 >", "a03276.html", null] │ │ │ │ │ + ["gtsam::traits< Line3 >", "a03084.html", null], │ │ │ │ │ + ["gtsam::traits< const Line3 >", "a03088.html", null], │ │ │ │ │ + ["transformTo", "a00458.html#aae9294b064e306ac7993dea546c0d8fb", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00458_source.html │ │ │ │ @@ -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/geometry/Line3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,181 +98,130 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Similarity3.h
    │ │ │ │ +
    Line3.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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 private:
    │ │ │ │ -
    44 Rot3 R_;
    │ │ │ │ -
    45 Point3 t_;
    │ │ │ │ -
    46 double s_;
    │ │ │ │ -
    47
    │ │ │ │ -
    48 public:
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ -
    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;
    │ │ │ │ -
    103
    │ │ │ │ -
    115 Pose3 transformFrom(const Pose3& T) const;
    │ │ │ │ -
    116
    │ │ │ │ -
    118 Point3 operator*(const Point3& p) const;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 static Similarity3 Align(const Point3Pairs& abPointPairs);
    │ │ │ │ -
    124
    │ │ │ │ -
    135 static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
    │ │ │ │ -
    136
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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_; }
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    149 friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    152};
    │ │ │ │ +
    │ │ │ │ +
    153
    │ │ │ │ +
    154template<>
    │ │ │ │ +
    155struct traits<Line3> : public internal::Manifold<Line3> {};
    │ │ │ │ +
    156
    │ │ │ │ +
    157template<>
    │ │ │ │ +
    158struct traits<const Line3> : public internal::Manifold<Line3> {};
    │ │ │ │ +
    159}
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │
    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
    │ │ │ │ -
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │ -
    Matrix wedge(const Vector &x)
    Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
    │ │ │ │ +
    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
    │ │ │ │ +
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
    Definition Line3.h:44
    │ │ │ │ +
    Rot3 R() const
    Return the rotation of the line.
    Definition Line3.h:123
    │ │ │ │ +
    Line3()
    Default constructor is the Z axis.
    Definition Line3.h:53
    │ │ │ │ +
    Line3(const Rot3 &R, const double a, const double b)
    Constructor for general line from (R, a, b)
    Definition Line3.h:57
    │ │ │ │ +
    double b() const
    Return the y-coordinate of the intersection of the line with the xy plane.
    Definition Line3.h:137
    │ │ │ │ +
    double a() const
    Return the x-coordinate of the intersection of the line with the xy plane.
    Definition Line3.h:130
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    3D similarity transform
    Definition Similarity3.h:36
    │ │ │ │ -
    static size_t Dim()
    Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
    Definition Similarity3.h:194
    │ │ │ │ -
    Point3 translation() const
    Return a GTSAM translation.
    Definition Similarity3.h:188
    │ │ │ │ -
    Rot3 rotation() const
    Return a GTSAM rotation.
    Definition Similarity3.h:185
    │ │ │ │ -
    size_t dim() const
    Dimensionality of tangent space = 7 DOF.
    Definition Similarity3.h:197
    │ │ │ │ -
    static Matrix4 wedge(const Vector7 &xi)
    wedge for Similarity3:
    Definition Similarity3.cpp:198
    │ │ │ │ -
    double scale() const
    Return the scale.
    Definition Similarity3.h:191
    │ │ │ │ -
    Chart at the origin.
    Definition Similarity3.h:153
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,233 +1,163 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Similarity3.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, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights 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; │ │ │ │ │ -42 │ │ │ │ │ -43 private: │ │ │ │ │ -44 _R_o_t_3 R_; │ │ │ │ │ -45 _P_o_i_n_t_3 t_; │ │ │ │ │ -46 double s_; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ -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); │ │ │ │ │ -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; │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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_; } │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_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 │ │ │ │ │ -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_:_:_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_:_:_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_:_:_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_i_m_i_l_a_r_i_t_y_3_._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/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/Cal3Unified.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,193 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    triangulation.h File Reference
    │ │ │ │ +
    Cal3Unified.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, Valuesgtsam::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, Valuesgtsam::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 Cal3Unified &cal)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Functions for triangulation.

    │ │ │ │ -
    Date
    July 31, 2013
    │ │ │ │ -
    Author
    Chris Beall
    │ │ │ │ -
    │ │ │ │ -Akshay Krishnan
    │ │ │ │ -
    Date
    July 31, 2013
    │ │ │ │ -
    Author
    Chris Beall
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ +
    Date
    Mar 8, 2014
    │ │ │ │ +
    Author
    Jing Dong
    │ │ │ │
    │ │ │ │ -Akshay Krishnan
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,295 +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 │ │ │ │ │ -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 │ │ │ │ │ +Cal3Unified.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_U_n_i_f_i_e_d &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Functions for triangulation. │ │ │ │ │ - Date │ │ │ │ │ - July 31, 2013 │ │ │ │ │ - Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - July 31, 2013 │ │ │ │ │ + Mar 8, 2014 │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ + Jing Dong │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _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_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/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/SOn.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,178 +98,400 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    OrientedPlane3.h
    │ │ │ │ +
    SOn.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-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/*
    │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    25#include <string>
    │ │ │ │ +
    21#include <gtsam/base/Lie.h>
    │ │ │ │ +
    22#include <gtsam/base/Manifold.h>
    │ │ │ │ + │ │ │ │ +
    24#include <gtsam/dllexport.h>
    │ │ │ │ +
    25#include <Eigen/Core>
    │ │ │ │
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ +
    27#include <boost/serialization/nvp.hpp>
    │ │ │ │
    28
    │ │ │ │ -
    │ │ │ │ -
    36class GTSAM_EXPORT OrientedPlane3 {
    │ │ │ │ -
    37private:
    │ │ │ │ -
    38 Unit3 n_;
    │ │ │ │ -
    39 double d_;
    │ │ │ │ -
    40
    │ │ │ │ -
    41public:
    │ │ │ │ -
    42 enum {
    │ │ │ │ -
    43 dimension = 3
    │ │ │ │ -
    44 };
    │ │ │ │ -
    45
    │ │ │ │ -
    48
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    51 n_(), d_(0.0) {
    │ │ │ │ -
    52 }
    │ │ │ │ -
    │ │ │ │ -
    53
    │ │ │ │ -
    │ │ │ │ -
    55 OrientedPlane3(const Unit3& n, double d) :
    │ │ │ │ -
    56 n_(n), d_(d) {
    │ │ │ │ -
    57 }
    │ │ │ │ -
    │ │ │ │ +
    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}
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ +
    60
    │ │ │ │ +
    61 protected:
    │ │ │ │ +
    62 MatrixNN matrix_;
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    73 public:
    │ │ │ │ +
    76
    │ │ │ │ +
    78 template <int N_ = N, typename = IsFixed<N_>>
    │ │ │ │ +
    79 SO() : matrix_(MatrixNN::Identity()) {}
    │ │ │ │
    80
    │ │ │ │ -
    82
    │ │ │ │ -
    89 OrientedPlane3 transform(const Pose3& xr,
    │ │ │ │ -
    90 OptionalJacobian<3, 3> Hp = boost::none,
    │ │ │ │ -
    91 OptionalJacobian<3, 6> Hr = boost::none) const;
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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 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
    │ │ │ │ -
    │ │ │ │ -
    144template<> struct traits<const OrientedPlane3> : public internal::Manifold<
    │ │ │ │ -
    145OrientedPlane3> {
    │ │ │ │ -
    146};
    │ │ │ │ +
    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} // namespace gtsam
    │ │ │ │
    149
    │ │ │ │ -
    3D Pose
    │ │ │ │ +
    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;
    │ │ │ │ +
    165
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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"
    │ │ │ │ +
    make_shared trampoline function to ensure proper alignment
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Template implementations for SO(n)
    │ │ │ │ +
    constexpr int DimensionSO(int N)
    Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
    Definition SOn.h:39
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
    Definition OrientedPlane3.h:36
    │ │ │ │ -
    OrientedPlane3(double a, double b, double c, double d)
    Construct from four numbers of plane coeffcients (a, b, c, d)
    Definition OrientedPlane3.h:64
    │ │ │ │ -
    Vector4 planeCoefficients() const
    Returns the plane coefficients.
    Definition OrientedPlane3.h:122
    │ │ │ │ -
    OrientedPlane3(const Unit3 &n, double d)
    Construct from a Unit3 and a distance.
    Definition OrientedPlane3.h:55
    │ │ │ │ -
    size_t dim() const
    Dimensionality of tangent space = 3 DOF.
    Definition OrientedPlane3.h:110
    │ │ │ │ -
    bool equals(const OrientedPlane3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition OrientedPlane3.h:77
    │ │ │ │ -
    OrientedPlane3(const Vector4 &vec)
    Construct from a vector of plane coefficients.
    Definition OrientedPlane3.h:60
    │ │ │ │ -
    static size_t Dim()
    Dimensionality of tangent space = 3 DOF.
    Definition OrientedPlane3.h:105
    │ │ │ │ -
    OrientedPlane3()
    Default constructor.
    Definition OrientedPlane3.h:50
    │ │ │ │ -
    double distance(OptionalJacobian< 1, 3 > H=boost::none) const
    Return the perpendicular distance to the origin.
    Definition OrientedPlane3.h:134
    │ │ │ │ -
    Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
    Return the normal.
    Definition OrientedPlane3.h:128
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Vector.
    Definition Unit3.cpp:151
    │ │ │ │ -
    bool equals(const Unit3 &s, double tol=1e-9) const
    The equals function with tolerance.
    Definition Unit3.h:115
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ +
    static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
    Named constructor from Eigen Matrix.
    Definition SOn.h:95
    │ │ │ │ +
    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
    │ │ │ │ +
    static Matrix VectorizedGenerators()
    Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
    Definition SOn.h:300
    │ │ │ │ +
    SO inverse() const
    inverse of a rotation = transpose
    Definition SOn.h:193
    │ │ │ │ +
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ +
    static SO ChordalMean(const std::vector< SO > &rotations)
    Named constructor that finds chordal mean , currently only defined for SO3.
    │ │ │ │ +
    SO operator*(const SO &other) const
    Multiplication.
    Definition SOn.h:175
    │ │ │ │ +
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ +
    MatrixNN matrix_
    Rotation matrix.
    Definition SOn.h:62
    │ │ │ │ +
    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
    │ │ │ │ +
    static SO AxisAngle(const Vector3 &axis, double theta)
    Constructor from axis and angle. Only defined for SO3.
    │ │ │ │ +
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ +
    static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
    In-place version of Hat (see details there), implements recursion.
    │ │ │ │ +
    static SO Identity()
    SO<N> identity for N >= 2.
    Definition SOn.h:182
    │ │ │ │ +
    static int Dim()
    Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
    Definition SOn.h:203
    │ │ │ │ +
    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
    │ │ │ │ +
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ +
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ +
    SO(const SO< M > &R)
    Construct dynamic SO(n) from Fixed SO<M>
    Definition SOn.h:111
    │ │ │ │ +
    SO(size_t n=0)
    Construct SO<N> identity for N == Eigen::Dynamic.
    Definition SOn.h:83
    │ │ │ │ +
    SO()
    Construct SO<N> identity for N >= 2.
    Definition SOn.h:79
    │ │ │ │ +
    static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
    Named constructor from lower dimensional matrix.
    Definition SOn.h:101
    │ │ │ │ +
    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
    │ │ │ │ +
    static SO Identity(size_t n=0)
    SO<N> identity for N == Eigen::Dynamic.
    Definition SOn.h:188
    │ │ │ │ +
    const MatrixNN & matrix() const
    Return matrix.
    Definition SOn.h:155
    │ │ │ │ +
    static SO Random(std::mt19937 &rng)
    Random SO(N) element (no big claims about uniformity)
    Definition SOn.h:145
    │ │ │ │ +
    static SO ClosestTo(const MatrixNN &M)
    Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
    │ │ │ │ +
    SO(const Eigen::AngleAxisd &angleAxis)
    Constructor from AngleAxisd.
    Definition SOn.h:115
    │ │ │ │ +
    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
    │ │ │ │ +
    SO(const Eigen::MatrixBase< Derived > &R)
    Constructor from Eigen Matrix, dynamic version.
    Definition SOn.h:91
    │ │ │ │ +
    Definition SOn.h:240
    │ │ │ │ +
    static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
    Inverse of Retract.
    Definition SOn-inl.h:50
    │ │ │ │ +
    static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
    Retract uses Cayley map.
    Definition SOn-inl.h:40
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,201 +1,507 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -OrientedPlane3.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 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * 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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ +27#include │ │ │ │ │ 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 } │ │ │ │ │ +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} │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +73 public: │ │ │ │ │ +76 │ │ │ │ │ +78 template > │ │ │ │ │ +_7_9 _S_O() : _m_a_t_r_i_x__(MatrixNN::_I_d_e_n_t_i_t_y()) {} │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_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_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 │ │ │ │ │ +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 │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ +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 void print(const std::string& s = std::string()) const; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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" │ │ │ │ │ +_m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ +make_shared trampoline function to ensure proper alignment │ │ │ │ │ +_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___I_F │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_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 │ │ │ │ │ _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_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_:_:_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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * OOrriieenntteeddPPllaannee33..hh │ │ │ │ │ + * _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/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/Cal3Unified.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,52 +96,49 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PinholePose.h File Reference
    │ │ │ │ +
    Cal3Unified.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Pinhole camera with known calibration. │ │ │ │ +

    Unified Calibration Model, see Mei07icra for details. │ │ │ │ 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::Cal3Unified
     Calibration of a omni-directional camera with mirror + lens radial distortion. More...
     
    class  gtsam::PinholePose< CALIBRATION >
     A pinhole camera class that has a Pose3 and a fixed Calibration. More...
    struct  gtsam::traits< Cal3Unified >
     
    struct  gtsam::traits< PinholePose< CALIBRATION > >
     
    struct  gtsam::traits< const PinholePose< CALIBRATION > >
    struct  gtsam::traits< const Cal3Unified >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Pinhole camera with known calibration.

    │ │ │ │ -
    Author
    Yong-Dian Jian
    │ │ │ │ +

    Unified Calibration Model, see Mei07icra for details.

    │ │ │ │ +
    Date
    Mar 8, 2014
    │ │ │ │ +
    Author
    Jing Dong
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Feb 20, 2015
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +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 │ │ │ │ │ -PinholePose.h File Reference │ │ │ │ │ -Pinhole camera with known calibration. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ - 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_a_l_3_U_n_i_f_i_e_d_ _> │ │ │ │ │   │ │ │ │ │ -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_ _>_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Pinhole camera with known calibration. │ │ │ │ │ - Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ Date │ │ │ │ │ - Feb 20, 2015 │ │ │ │ │ + Mar 8, 2014 │ │ │ │ │ + Author │ │ │ │ │ + Jing Dong │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _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_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/a00467.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00467 = [ │ │ │ │ │ - ["gtsam::traits< PinholePose< CALIBRATION > >", "a03128.html", null], │ │ │ │ │ - ["gtsam::traits< const PinholePose< CALIBRATION > >", "a03132.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3Unified >", "a03012.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3Unified >", "a03016.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00467_source.html │ │ │ │ @@ -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/geometry/Cal3Unified.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,503 +98,127 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PinholePose.h
    │ │ │ │ +
    Cal3Unified.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <boost/make_shared.hpp>
    │ │ │ │ +
    24#pragma once
    │ │ │ │
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ +
    26#include <gtsam/geometry/Cal3DS2_Base.h>
    │ │ │ │
    27
    │ │ │ │ -
    33template<typename CALIBRATION>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    35
    │ │ │ │ -
    36private:
    │ │ │ │ -
    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;
    │ │ │ │ -
    42
    │ │ │ │ -
    43public:
    │ │ │ │ -
    44
    │ │ │ │ -
    45 typedef CALIBRATION CalibrationType;
    │ │ │ │ -
    46
    │ │ │ │ -
    49
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    52 }
    │ │ │ │ -
    │ │ │ │ -
    53
    │ │ │ │ -
    │ │ │ │ -
    55 explicit PinholeBaseK(const Pose3& pose) :
    │ │ │ │ - │ │ │ │ -
    57 }
    │ │ │ │ -
    │ │ │ │ -
    58
    │ │ │ │ -
    62
    │ │ │ │ -
    63 explicit PinholeBaseK(const Vector &v) :
    │ │ │ │ -
    64 PinholeBase(v) {
    │ │ │ │ -
    65 }
    │ │ │ │ -
    66
    │ │ │ │ -
    70
    │ │ │ │ -
    71 virtual ~PinholeBaseK() {
    │ │ │ │ -
    72 }
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    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
    │ │ │ │ -
    75 virtual const CALIBRATION& calibration() const = 0;
    │ │ │ │ +
    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 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 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    88
    │ │ │ │ -
    95 template <class POINT>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    98 OptionalJacobian<2, DimK> Dcal) const {
    │ │ │ │ -
    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;
    │ │ │ │ -
    113
    │ │ │ │ -
    114 return pi;
    │ │ │ │ -
    115 }
    │ │ │ │ -
    │ │ │ │ -
    116
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    133 OptionalJacobian<2, 2> Dpoint = boost::none,
    │ │ │ │ -
    134 OptionalJacobian<2, DimK> Dcal = boost::none) const {
    │ │ │ │ -
    135 return _project(pw, Dpose, Dpoint, Dcal);
    │ │ │ │ -
    136 }
    │ │ │ │ -
    │ │ │ │ +
    89 bool equals(const Cal3Unified& K, double tol = 10e-9) const;
    │ │ │ │ +
    90
    │ │ │ │ +
    94
    │ │ │ │ +
    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
    │ │ │ │ +
    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 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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    232};
    │ │ │ │ -
    │ │ │ │ -
    233// end of class PinholeBaseK
    │ │ │ │ -
    234
    │ │ │ │ -
    242template<typename CALIBRATION>
    │ │ │ │ -
    │ │ │ │ -
    243class PinholePose: public PinholeBaseK<CALIBRATION> {
    │ │ │ │ -
    244
    │ │ │ │ -
    245private:
    │ │ │ │ -
    246
    │ │ │ │ - │ │ │ │ -
    248 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │ -
    249
    │ │ │ │ -
    250public:
    │ │ │ │ -
    251
    │ │ │ │ -
    252 enum {
    │ │ │ │ -
    253 dimension = 6
    │ │ │ │ -
    254 };
    │ │ │ │ -
    255
    │ │ │ │ -
    258
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    380 OptionalJacobian<2, 3> Dpoint = boost::none) const {
    │ │ │ │ -
    381 return Base::project(pw, Dpose, Dpoint);
    │ │ │ │ -
    382 }
    │ │ │ │ -
    │ │ │ │ -
    383
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    427 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
    │ │ │ │ -
    428 }
    │ │ │ │ -
    │ │ │ │ -
    430
    │ │ │ │ -
    431private:
    │ │ │ │ -
    432
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Calibrated camera for which only pose is unknown.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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}
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
    Definition CalibratedCamera.h:52
    │ │ │ │ -
    static Matrix26 Dpose(const Point2 &pn, double d)
    Calculate Jacobian with respect to pose.
    Definition CalibratedCamera.cpp:27
    │ │ │ │ -
    virtual void print(const std::string &s="PinholeBase") const
    print
    Definition CalibratedCamera.cpp:74
    │ │ │ │ -
    std::pair< Point2, bool > projectSafe(const Point3 &pw) const
    Project a point into the image and check depth.
    Definition CalibratedCamera.cpp:109
    │ │ │ │ -
    const Pose3 & pose() const
    return pose, constant version
    Definition CalibratedCamera.h:152
    │ │ │ │ -
    static Pose3 LevelPose(const Pose2 &pose2, double height)
    Create a level pose at the given 2D pose and height.
    Definition CalibratedCamera.cpp:49
    │ │ │ │ -
    bool equals(const PinholeBase &camera, double tol=1e-9) const
    assert equality up to a tolerance
    Definition CalibratedCamera.cpp:69
    │ │ │ │ -
    static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
    Calculate Jacobian with respect to point.
    Definition CalibratedCamera.cpp:37
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A Calibrated camera class [R|-R't], calibration K=I.
    Definition CalibratedCamera.h:247
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    std::pair< Point2, bool > projectSafe(const Point3 &pw) const
    Project a point into the image and check depth.
    Definition PinholePose.h:82
    │ │ │ │ -
    PinholeBaseK()
    default constructor
    Definition PinholePose.h:51
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual const CALIBRATION & calibration() const =0
    return calibration
    │ │ │ │ -
    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
    │ │ │ │ -
    PinholeBaseK(const Pose3 &pose)
    constructor with pose
    Definition PinholePose.h:55
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Unit3 backprojectPointAtInfinity(const Point2 &p) const
    backproject a 2-dimensional point to a 3-dimensional point at infinity
    Definition PinholePose.h:170
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition PinholePose.h:222
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ -
    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
    │ │ │ │ -
    PinholePose()
    default constructor
    Definition PinholePose.h:260
    │ │ │ │ -
    Matrix34 cameraProjectionMatrix() const
    for Linear Triangulation
    Definition PinholePose.h:420
    │ │ │ │ -
    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
    │ │ │ │ -
    const CALIBRATION & calibration() const override
    return calibration
    Definition PinholePose.h:370
    │ │ │ │ -
    Vector defaultErrorWhenTriangulatingBehindCamera() const
    for Nonlinear Triangulation
    Definition PinholePose.h:426
    │ │ │ │ -
    friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
    stream operator
    Definition PinholePose.h:339
    │ │ │ │ -
    static PinholePose Identity()
    for Canonical
    Definition PinholePose.h:415
    │ │ │ │ -
    PinholePose(const Vector &v, const Vector &K)
    Init from Vector and calibration.
    Definition PinholePose.h:319
    │ │ │ │ -
    const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
    return shared pointer to calibration
    Definition PinholePose.h:365
    │ │ │ │ -
    size_t dim() const
    Definition PinholePose.h:395
    │ │ │ │ -
    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
    │ │ │ │ -
    PinholePose retract(const Vector6 &d) const
    move a cameras according to d
    Definition PinholePose.h:405
    │ │ │ │ -
    PinholePose(const Vector &v)
    Init from 6D vector.
    Definition PinholePose.h:314
    │ │ │ │ -
    PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
    constructor with pose and calibration
    Definition PinholePose.h:269
    │ │ │ │ -
    static size_t Dim()
    Definition PinholePose.h:400
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition PinholePose.h:434
    │ │ │ │ -
    bool equals(const Base &camera, double tol=1e-9) const
    assert equality up to a tolerance
    Definition PinholePose.h:333
    │ │ │ │ -
    void print(const std::string &s="PinholePose") const override
    print
    Definition PinholePose.h:349
    │ │ │ │ -
    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
    │ │ │ │ -
    PinholePose(const Pose3 &pose)
    constructor with pose, uses default calibration
    Definition PinholePose.h:264
    │ │ │ │ -
    static PinholePose Level(const Pose2 &pose2, double height)
    PinholePose::level with default calibration.
    Definition PinholePose.h:290
    │ │ │ │ -
    Vector6 localCoordinates(const PinholePose &p) const
    return canonical coordinate
    Definition PinholePose.h:410
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │ -
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
    Use RQ to calculate roll-pitch-yaw angle representation.
    Definition Rot3.cpp:192
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │ +
    Calibration of a omni-directional camera with mirror + lens radial distortion.
    Definition Cal3Unified.h:45
    │ │ │ │ +
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3Unified.h:133
    │ │ │ │ +
    Cal3Unified()=default
    Default Constructor with only unit focal length.
    │ │ │ │ +
    double xi() const
    mirror parameter
    Definition Cal3Unified.h:96
    │ │ │ │ +
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3Unified.h:136
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,662 +1,151 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PinholePose.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 │ │ │ │ │ +24#pragma once │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +26#include │ │ │ │ │ 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: │ │ │ │ │ -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; │ │ │ │ │ -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 } │ │ │ │ │ -66 │ │ │ │ │ -70 │ │ │ │ │ -71 virtual ~PinholeBaseK() { │ │ │ │ │ -72 } │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -_7_5 virtual const CALIBRATION& _c_a_l_i_b_r_a_t_i_o_n() const = 0; │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -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 { │ │ │ │ │ -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; │ │ │ │ │ -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:: │ │ │ │ │ +89 bool equals(const Cal3Unified& K, double tol = 10e-9) const; │ │ │ │ │ +90 │ │ │ │ │ +94 │ │ │ │ │ +_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, │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ +_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() │ │ │ │ │ -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 │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Unified.h:136 │ │ │ │ │ * _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_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/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/Point2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,92 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    SimpleCamera.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    Point2.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    2D Point │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    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< Point2gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
     
    │ │ │ │ +list< Point2gtsam::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< Point2gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
     Intersect 2 circles.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A simple camera class with a Cal3_S2 calibration.

    │ │ │ │ -
    Date
    June 30, 2012
    │ │ │ │ +

    2D Point

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,70 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 │ │ │ │ │ +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 ********** │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ - Date │ │ │ │ │ - June 30, 2012 │ │ │ │ │ +2D Point │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/HybridConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    GaussianMixture.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    HybridConditional.cpp 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...
     
    struct  gtsam::traits< GaussianMixture >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ Namespaces

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

    │ │ │ │ -Functions

    │ │ │ │ -std::set< DiscreteKeygtsam::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
    │ │ │ │ -
    Date
    Mar 12, 2022
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +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 │ │ │ │ │ -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_:_:_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_<_ _G_a_u_s_s_i_a_n_M_i_x_t_u_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::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. │ │ │ │ │ + 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_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/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/HybridBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,59 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    HybridGaussianFactorGraph.cpp File Reference
    │ │ │ │ +
    HybridBayesNet.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Hybrid factor graph that uses type erasure. │ │ │ │ +

    A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ 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.
     
    std::function< double(const Assignment< Key > &, double)> gtsam::prunerFunc (const DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional)
     Helper function to get the pruner functional.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Hybrid factor graph that uses type erasure.

    │ │ │ │ +

    A Bayes net of Gaussian Conditionals indexed by discrete keys.

    │ │ │ │
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │ Varun Agrawal
    │ │ │ │
    │ │ │ │ +Shangjie Xue
    │ │ │ │ +
    │ │ │ │ Frank Dellaert
    │ │ │ │ -
    Date
    Mar 11, 2022
    │ │ │ │ +
    Date
    January 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,43 +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 │ │ │ │ │ -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 | _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::::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. │ │ │ │ │ +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 ********** │ │ │ │ │ -Hybrid factor graph that uses type erasure. │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Shangjie Xue │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + January 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_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_._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/a00485.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00485 = [ │ │ │ │ │ - ["EliminateHybrid", "a00485.html#gacbc9f97e48e6aa3abb5f789af8c3bfe5", null], │ │ │ │ │ - ["HybridOrdering", "a00485.html#a584e4e05d52a617c1d313e1f53c8ce58", null] │ │ │ │ │ + ["prunerFunc", "a00485.html#ae6415c63d70d1b582a0162652b16b868", null] │ │ │ │ │ ]; │ │ │ ├── ./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/HybridEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridJunctionTree.h File Reference
    │ │ │ │ +
    HybridEliminationTree.cpp 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │
    Date
    Mar 11, 2022
    │ │ │ │
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** 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_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/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/HybridGaussianISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    HybridBayesNet.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    HybridGaussianISAM.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ -More...

    │ │ │ │ +

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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.

    │ │ │ │ +
    Date
    March 31, 2022
    │ │ │ │
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Frank Dellaert │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    Date
    March 31, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │ -Shangjie Xue
    │ │ │ │ +Frank Dellaert │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    January 2022
    │ │ │ │ +Richard Roberts │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -HybridBayesNet.cpp File Reference │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ + March 31, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ - Varun Agrawal │ │ │ │ │ - Shangjie Xue │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - January 2022 │ │ │ │ │ + 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_B_a_y_e_s_N_e_t_._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/a00491.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00491 = [ │ │ │ │ │ - ["prunerFunc", "a00491.html#ae6415c63d70d1b582a0162652b16b868", null] │ │ │ │ │ + ["gtsam::traits< HybridGaussianISAM >", "a03472.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/GaussianMixture.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    HybridBayesTree.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    GaussianMixture.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ +

    A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

    │ │ │ │ +std::set< DiscreteKeygtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
     Return the DiscreteKey vector as a set.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

    │ │ │ │ -
    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,37 +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 │ │ │ │ │ -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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ - 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_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _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/a00494.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00494 = [ │ │ │ │ │ - ["gtsam::HybridBayesTree", "a03424.html", "a03424"], │ │ │ │ │ - ["gtsam::traits< HybridBayesTree >", "a03428.html", null], │ │ │ │ │ - ["gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >", "a03432.html", "a03432"] │ │ │ │ │ + ["DiscreteKeysAsSet", "a00494.html#ad92a1966f54e02a7c2ce061c1228c7b3", null] │ │ │ │ │ ]; │ │ │ ├── ./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/HybridFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    HybridValues.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    HybridFactorGraph.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Factor graph with utilities for hybrid factors. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    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 >
    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

    │ │ │ │ -
    Date
    Jul 28, 2022
    │ │ │ │ -
    Author
    Shangjie Xue
    │ │ │ │ +

    Factor graph with utilities for hybrid factors.

    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    May 28, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -HybridValues.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 │ │ │ │ │ +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_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_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Jul 28, 2022 │ │ │ │ │ +Factor graph with utilities for hybrid factors. │ │ │ │ │ Author │ │ │ │ │ - Shangjie Xue │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + May 28, 2022 │ │ │ │ │ * _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_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/a00497.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00497 = [ │ │ │ │ │ - ["gtsam::HybridValues", "a03504.html", "a03504"], │ │ │ │ │ - ["gtsam::traits< HybridValues >", "a03508.html", null] │ │ │ │ │ + ["gtsam::HybridFactorGraph", "a03456.html", "a03456"] │ │ │ │ │ ]; │ │ │ ├── ./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/HybridFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,270 +98,92 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridValues.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27#include <map>
    │ │ │ │ -
    28#include <string>
    │ │ │ │ -
    29#include <vector>
    │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ +
    25#include <boost/format.hpp>
    │ │ │ │ +
    26#include <unordered_map>
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    30class DiscreteFactor;
    │ │ │ │ +
    31class Ordering;
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    58 : continuous_(cv), discrete_(dv){};
    │ │ │ │ -
    │ │ │ │ -
    59
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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; };
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    138 continuous_.insert(values);
    │ │ │ │ -
    139 return *this;
    │ │ │ │ -
    140 }
    │ │ │ │ -
    │ │ │ │ -
    141
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    145 discrete_.insert(values);
    │ │ │ │ -
    146 return *this;
    │ │ │ │ -
    147 }
    │ │ │ │ -
    │ │ │ │ -
    148
    │ │ │ │ -
    │ │ │ │ -
    151 HybridValues& insert(const Values& values) {
    │ │ │ │ -
    152 nonlinear_.insert(values);
    │ │ │ │ -
    153 return *this;
    │ │ │ │ -
    154 }
    │ │ │ │ -
    │ │ │ │ -
    155
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    182 continuous_.update(values);
    │ │ │ │ -
    183 return *this;
    │ │ │ │ -
    184 }
    │ │ │ │ -
    │ │ │ │ -
    185
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    191 discrete_.update(values);
    │ │ │ │ -
    192 return *this;
    │ │ │ │ -
    193 }
    │ │ │ │ -
    │ │ │ │ -
    194
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    200 continuous_.update(values.continuous());
    │ │ │ │ -
    201 discrete_.update(values.discrete());
    │ │ │ │ -
    202 return *this;
    │ │ │ │ -
    203 }
    │ │ │ │ -
    │ │ │ │ -
    204
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ -
    An assignment from labels to a discrete value index (size_t)
    │ │ │ │ - │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    33using SharedFactor = boost::shared_ptr<Factor>;
    │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ +
    39class HybridFactorGraph : public FactorGraph<Factor> {
    │ │ │ │ +
    40 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    43 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    44
    │ │ │ │ + │ │ │ │ +
    46 using Indices = KeyVector;
    │ │ │ │ +
    47
    │ │ │ │ +
    48 public:
    │ │ │ │ +
    51
    │ │ │ │ +
    53 HybridFactorGraph() = default;
    │ │ │ │ +
    54
    │ │ │ │ +
    60 template <class DERIVEDFACTOR>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const
    Output as a html table.
    Definition DiscreteValues.cpp:104
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print required by Testable.
    Definition DiscreteValues.cpp:31
    │ │ │ │ -
    bool equals(const DiscreteValues &x, double tol=1e-9) const
    equals required by Testable for unit testing.
    Definition DiscreteValues.cpp:40
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    void print(const std::string &s="HybridValues", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print required by Testable for unit testing
    Definition HybridValues.h:70
    │ │ │ │ -
    void insert(Key j, const Vector &value)
    Insert a vector value with key j.
    Definition HybridValues.h:117
    │ │ │ │ -
    HybridValues & insert(const DiscreteValues &values)
    Insert all discrete values from values.
    Definition HybridValues.h:144
    │ │ │ │ -
    HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v)
    Construct from all values types.
    Definition HybridValues.h:61
    │ │ │ │ -
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output as a html table.
    Definition HybridValues.h:224
    │ │ │ │ -
    VectorValues continuousSubset(const KeyVector &keys) const
    Extract continuous values with given keys.
    Definition HybridValues.h:206
    │ │ │ │ -
    void insert_or_assign(Key j, const Vector &value)
    insert_or_assign() , similar to Values.h
    Definition HybridValues.h:126
    │ │ │ │ -
    HybridValues & insert(const Values &values)
    Insert all values from values.
    Definition HybridValues.h:151
    │ │ │ │ -
    bool equals(const HybridValues &other, double tol=1e-9) const
    equals required by Testable for unit testing
    Definition HybridValues.h:79
    │ │ │ │ -
    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
    │ │ │ │ -
    const DiscreteValues & discrete() const
    Return the discrete values.
    Definition HybridValues.h:92
    │ │ │ │ -
    bool existsDiscrete(Key j)
    Check whether a variable with key j exists in DiscreteValues.
    Definition HybridValues.h:101
    │ │ │ │ -
    const VectorValues & continuous() const
    Return the multi-dimensional vector values.
    Definition HybridValues.h:89
    │ │ │ │ -
    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
    │ │ │ │ -
    bool exists(Key j)
    Check whether a variable with key j exists.
    Definition HybridValues.h:109
    │ │ │ │ -
    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
    │ │ │ │ -
    HybridValues & insert(const HybridValues &values)
    Insert all values from values.
    Definition HybridValues.h:158
    │ │ │ │ -
    HybridValues & insert(const VectorValues &values)
    Insert all continuous values from values.
    Definition HybridValues.h:137
    │ │ │ │ -
    void insert(Key j, size_t value)
    Insert a discrete value with key j.
    Definition HybridValues.h:123
    │ │ │ │ -
    const Values & nonlinear() const
    Return the nonlinear values.
    Definition HybridValues.h:95
    │ │ │ │ -
    bool existsNonlinear(Key j)
    Check whether a variable with key j exists in values.
    Definition HybridValues.h:104
    │ │ │ │ -
    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
    │ │ │ │ -
    HybridValues()=default
    Default constructor creates an empty HybridValues.
    │ │ │ │ -
    bool existsVector(Key j)
    Check whether a variable with key j exists in VectorValues.
    Definition HybridValues.h:98
    │ │ │ │ -
    HybridValues(const VectorValues &cv, const DiscreteValues &dv)
    Construct from DiscreteValues and VectorValues.
    Definition HybridValues.h:57
    │ │ │ │ -
    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
    │ │ │ │ -
    void insert_or_assign(Key j, size_t value)
    insert_or_assign() , similar to Values.h
    Definition HybridValues.h:131
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    iterator insert(const std::pair< Key, Vector > &key_value)
    Insert a vector value with key j.
    Definition VectorValues.cpp:91
    │ │ │ │ -
    void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print required by Testable for unit testing
    Definition VectorValues.cpp:158
    │ │ │ │ -
    bool equals(const VectorValues &x, double tol=1e-9) const
    equals required by Testable for unit testing
    Definition VectorValues.cpp:166
    │ │ │ │ -
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │ -
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output as a html table.
    Definition VectorValues.cpp:387
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ +
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ +
    HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridFactorGraph.h:61
    │ │ │ │ +
    KeySet discreteKeySet() const
    Get all the discrete keys in the factor graph, as a set.
    Definition HybridFactorGraph.cpp:46
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridFactorGraph.h:43
    │ │ │ │ +
    std::unordered_map< Key, DiscreteKey > discreteKeyMap() const
    Get a map from Key to corresponding DiscreteKey.
    Definition HybridFactorGraph.cpp:56
    │ │ │ │ +
    const KeySet continuousKeySet() const
    Get all the continuous keys in the factor graph.
    Definition HybridFactorGraph.cpp:65
    │ │ │ │ +
    HybridFactorGraph()=default
    Default constructor.
    │ │ │ │ +
    std::set< DiscreteKey > discreteKeys() const
    Get all the discrete keys in the factor graph.
    Definition HybridFactorGraph.cpp:28
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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
    │ │ │ │ -
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,392 +1,116 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridValues.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 │ │ │ │ │ -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 { │ │ │ │ │ +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 │ │ │ │ │ +30class DiscreteFactor; │ │ │ │ │ +31class Ordering; │ │ │ │ │ 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; }; │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ -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_:_:_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_:_:_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 │ │ │ │ │ * _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_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/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/HybridNonlinearFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridJunctionTree.cpp File Reference
    │ │ │ │ +
    HybridNonlinearFactorGraph.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::HybridNonlinearFactorGraph
     
    struct  gtsam::traits< HybridNonlinearFactorGraph >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    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,20 +1,28 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_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_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. │ │ │ │ │   │ │ │ │ │ ********** 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_J_u_n_c_t_i_o_n_T_r_e_e_._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_._h │ │ │ │ │ * Generated on Tue Jan 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/HybridGaussianFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,40 +95,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    HybridNonlinearISAM.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    HybridGaussianFactorGraph.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Linearized Hybrid factor graph that uses type erasure. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HybridNonlinearISAM
     Wrapper class to manage ISAM in a nonlinear context. More...
    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

    │ │ │ │ -
    Date
    Sep 12, 2022
    │ │ │ │ -
    Author
    Varun Agrawal
    │ │ │ │ +

    Linearized Hybrid factor graph that uses type erasure.

    │ │ │ │ +
    Author
    Fan Jiang, Varun Agrawal, Frank Dellaert
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -HybridNonlinearISAM.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 │ │ │ │ │ +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_I_S_A_M │ │ │ │ │ -  Wrapper class to manage _I_S_A_M in a nonlinear context. _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_<_ _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 │ │ │ │ │   │ │ │ │ │ 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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Sep 12, 2022 │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Fan Jiang, Varun Agrawal, Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + 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_I_S_A_M_._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/a00503.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00503 = [ │ │ │ │ │ - ["gtsam::HybridNonlinearISAM", "a03496.html", "a03496"] │ │ │ │ │ + ["gtsam::EliminationTraits< HybridGaussianFactorGraph >", "a03460.html", "a03460"], │ │ │ │ │ + ["EliminateHybrid", "a00503.html#gacbc9f97e48e6aa3abb5f789af8c3bfe5", null], │ │ │ │ │ + ["HybridOrdering", "a00503.html#a584e4e05d52a617c1d313e1f53c8ce58", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00503_source.html │ │ │ │ @@ -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/hybrid/HybridGaussianFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,139 +98,192 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridNonlinearISAM.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
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    │ │ │ │ -
    27class GTSAM_EXPORT HybridNonlinearISAM {
    │ │ │ │ -
    28 protected:
    │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │
    31
    │ │ │ │ - │ │ │ │ -
    34
    │ │ │ │ - │ │ │ │ -
    37
    │ │ │ │ - │ │ │ │ -
    40
    │ │ │ │ - │ │ │ │ -
    43 int reorderCounter_;
    │ │ │ │ -
    44
    │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    48 public:
    │ │ │ │ -
    51
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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 <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    71 typedef HybridConditional
    │ │ │ │ + │ │ │ │ +
    73 typedef HybridBayesNet
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    87 const FactorGraphType& graph,
    │ │ │ │ +
    88 boost::optional<const VariableIndex&> variableIndex) {
    │ │ │ │ +
    89 return HybridOrdering(graph);
    │ │ │ │ +
    90 }
    │ │ │ │ +
    │ │ │ │ +
    91};
    │ │ │ │ +
    │ │ │ │ +
    92
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    114 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    115
    │ │ │ │ + │ │ │ │ + │ │ │ │
    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);
    │ │ │ │ +
    121
    │ │ │ │ + │ │ │ │
    124
    │ │ │ │ -
    126 void reorder_relinearize();
    │ │ │ │ -
    127
    │ │ │ │ -
    129};
    │ │ │ │ -
    130
    │ │ │ │ -
    131} // namespace gtsam
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    Nonlinear hybrid factor graph that uses type erasure.
    │ │ │ │ +
    130 template <class DERIVEDFACTOR>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    170 const VectorValues& continuousValues) const;
    │ │ │ │ +
    171
    │ │ │ │ +
    178 double probPrime(const HybridValues& values) const;
    │ │ │ │ +
    179
    │ │ │ │ +
    189 GaussianFactorGraphTree assembleGraphTree() const;
    │ │ │ │ +
    190
    │ │ │ │ +
    192};
    │ │ │ │ +
    │ │ │ │ +
    193
    │ │ │ │ +
    194} // namespace gtsam
    │ │ │ │ +
    Factor graph with utilities for hybrid factors.
    │ │ │ │ + │ │ │ │ +
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Variable elimination algorithms for factor graphs.
    │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    void prune(const size_t maxNumberLeaves)
    Prune the underlying Bayes tree.
    Definition HybridBayesTree.cpp:175
    │ │ │ │ -
    Definition HybridGaussianISAM.h:34
    │ │ │ │ -
    Definition HybridNonlinearFactorGraph.h:33
    │ │ │ │ -
    Wrapper class to manage ISAM in a nonlinear context.
    Definition HybridNonlinearISAM.h:27
    │ │ │ │ -
    const HybridNonlinearFactorGraph & getFactorsUnsafe() const
    get underlying nonlinear graph
    Definition HybridNonlinearISAM.h:96
    │ │ │ │ -
    DiscreteValues assignment_
    The discrete assignment.
    Definition HybridNonlinearISAM.h:36
    │ │ │ │ -
    const Values & getLinearizationPoint() const
    Return the current linearization point.
    Definition HybridNonlinearISAM.h:90
    │ │ │ │ -
    HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
    Periodically reorder and relinearize.
    Definition HybridNonlinearISAM.h:59
    │ │ │ │ -
    HybridNonlinearFactorGraph factors_
    The original factors, used when relinearizing.
    Definition HybridNonlinearISAM.h:39
    │ │ │ │ -
    const DiscreteValues & assignment() const
    Return the current discrete assignment.
    Definition HybridNonlinearISAM.h:93
    │ │ │ │ -
    int reorderCounter() const
    TODO: comment.
    Definition HybridNonlinearISAM.h:102
    │ │ │ │ -
    void prune(const size_t maxNumberLeaves)
    Prune the underlying Bayes tree.
    Definition HybridNonlinearISAM.h:87
    │ │ │ │ -
    int reorderInterval_
    The reordering interval and counter.
    Definition HybridNonlinearISAM.h:42
    │ │ │ │ -
    int reorderInterval() const
    get counters
    Definition HybridNonlinearISAM.h:101
    │ │ │ │ -
    Values linPoint_
    The current linearization point.
    Definition HybridNonlinearISAM.h:33
    │ │ │ │ -
    gtsam::HybridGaussianISAM isam_
    The internal iSAM object.
    Definition HybridNonlinearISAM.h:30
    │ │ │ │ -
    HybridGaussianFactorGraph::Eliminate eliminationFunction_
    The elimination function.
    Definition HybridNonlinearISAM.h:46
    │ │ │ │ -
    const HybridGaussianISAM & bayesTree() const
    access the underlying bayes tree
    Definition HybridNonlinearISAM.h:80
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    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
    │ │ │ │ +
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ +
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ +
    A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
    Definition HybridBayesNet.h:35
    │ │ │ │ +
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ +
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ +
    Elimination Tree type for Hybrid Factor Graphs.
    Definition HybridEliminationTree.h:32
    │ │ │ │ +
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ +
    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
    │ │ │ │ +
    HybridConditional ConditionalType
    Type of conditionals from elimination.
    Definition HybridGaussianFactorGraph.h:72
    │ │ │ │ +
    HybridGaussianFactorGraph FactorGraphType
    Type of the factor graph (e.g.
    Definition HybridGaussianFactorGraph.h:69
    │ │ │ │ +
    HybridJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition HybridGaussianFactorGraph.h:78
    │ │ │ │ +
    HybridEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition HybridGaussianFactorGraph.h:76
    │ │ │ │ +
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition HybridGaussianFactorGraph.h:86
    │ │ │ │ +
    Factor FactorType
    Type of factors in factor graph.
    Definition HybridGaussianFactorGraph.h:67
    │ │ │ │ +
    HybridBayesTree BayesTreeType
    Type of Bayes tree.
    Definition HybridGaussianFactorGraph.h:77
    │ │ │ │ +
    HybridBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition HybridGaussianFactorGraph.h:74
    │ │ │ │ +
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridGaussianFactorGraph.h:114
    │ │ │ │ +
    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
    │ │ │ │ +
    KeyVector Indices
    map from keys to values
    Definition HybridGaussianFactorGraph.h:117
    │ │ │ │ +
    EliminateableFactorGraph< This > BaseEliminateable
    for elimination
    Definition HybridGaussianFactorGraph.h:113
    │ │ │ │ +
    HybridGaussianFactorGraph()=default
    Default constructor.
    │ │ │ │ +
    HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridGaussianFactorGraph.h:131
    │ │ │ │ +
    An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition HybridJunctionTree.h:53
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ +
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    the error.
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,191 +1,287 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridNonlinearISAM.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 │ │ │ │ │ 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__; │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ -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 │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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_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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_I_S_A_M_._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/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/HybridJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridNonlinearISAM.cpp File Reference
    │ │ │ │ +
    HybridJunctionTree.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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    Sep 12, 2022
    │ │ │ │ -
    Author
    Varun Agrawal
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridNonlinearISAM.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +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_:_:_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 ********** │ │ │ │ │ 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_._c_p_p │ │ │ │ │ + * _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/a00509.html │ │ │ │ @@ -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/hybrid/HybridJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,15 +95,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    HybridConditional.cpp File Reference
    │ │ │ │ +
    HybridJunctionTree.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -113,13 +113,13 @@ │ │ │ │
    Date
    Mar 11, 2022
    │ │ │ │
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +HybridJunctionTree.cpp File Reference │ │ │ │ │ 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_C_o_n_d_i_t_i_o_n_a_l_._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/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.

    │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ +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/HybridValues.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridEliminationTree.cpp File Reference
    │ │ │ │ +
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Mar 11, 2022
    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ +
    Date
    Jul 28, 2022
    │ │ │ │ +
    Author
    Shangjie Xue
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridEliminationTree.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. │ │ │ │ │   │ │ │ │ │ ********** 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_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._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/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/GaussianMixtureFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    HybridFactorGraph.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianMixtureFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor graph with utilities for hybrid factors. │ │ │ │ +

    A set of Gaussian factors indexed by a set of 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 >
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Factor graph with utilities for hybrid factors.

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

    A set of Gaussian factors indexed by a set of discrete keys.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │ -
    Date
    May 28, 2022
    │ │ │ │ +
    Date
    Mar 12, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SShhaarreeddFFaaccttoorr = boost::shared_ptr< _F_a_c_t_o_r > │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor graph with utilities for hybrid factors. │ │ │ │ │ +A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ │ Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + Mar 12, 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 │ │ │ │ │ + * _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/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/HybridEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridFactorGraph.cpp File Reference
    │ │ │ │ +
    HybridEliminationTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor graph with utilities for hybrid factors. │ │ │ │ -More...

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::HybridEliminationTree
     Elimination Tree type for Hybrid Factor Graphs. 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,25 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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_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/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/HybridBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridBayesNet.h File Reference
    │ │ │ │ +
    HybridBayesTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ +

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ 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...
    class  gtsam::HybridBayesTreeClique
     A clique in a HybridBayesTree which is a HybridConditional internally. More...
     
    struct  gtsam::traits< HybridBayesNet >
     traits 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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
    │ │ │ │ -
    │ │ │ │ -Fan Jiang
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    December 2021
    │ │ │ │ +

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -HybridBayesNet.h File Reference │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ +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_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_._._. │ │ │ │ │ + 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_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ + 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 ********** │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ + Date │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ Fan Jiang │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December 2021 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00524.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00524 = [ │ │ │ │ │ - ["gtsam::traits< HybridBayesNet >", "a03412.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/a00524_source.html │ │ │ │ @@ -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/hybrid/HybridBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,161 +98,164 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridBayesNet.h
    │ │ │ │ +
    HybridBayesTree.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27namespace 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
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    28
    │ │ │ │ -
    │ │ │ │ -
    35class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
    │ │ │ │ -
    36 public:
    │ │ │ │ - │ │ │ │ -
    38 using This = HybridBayesNet;
    │ │ │ │ - │ │ │ │ -
    40 using shared_ptr = boost::shared_ptr<HybridBayesNet>;
    │ │ │ │ -
    41 using sharedConditional = boost::shared_ptr<ConditionalType>;
    │ │ │ │ -
    42
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    86
    │ │ │ │ -
    │ │ │ │ -
    99 void push_back(HybridConditional &&conditional) {
    │ │ │ │ -
    100 factors_.push_back(
    │ │ │ │ -
    101 boost::make_shared<HybridConditional>(std::move(conditional)));
    │ │ │ │ -
    102 }
    │ │ │ │ -
    │ │ │ │ -
    103
    │ │ │ │ -
    111 GaussianBayesNet choose(const DiscreteValues &assignment) const;
    │ │ │ │ -
    112
    │ │ │ │ -
    114 double evaluate(const HybridValues &values) const;
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    50 Base;
    │ │ │ │ +
    51 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    52 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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 HybridBayesNet prune(size_t maxNrLeaves);
    │ │ │ │ -
    189
    │ │ │ │ -
    197 AlgebraicDecisionTree<Key> logProbability(
    │ │ │ │ -
    198 const VectorValues &continuousValues) const;
    │ │ │ │ -
    199
    │ │ │ │ -
    200 using BayesNet::logProbability; // expose HybridValues version
    │ │ │ │ -
    201
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Bayes network.
    │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    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 <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    141 public:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ -
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
    Definition HybridBayesNet.h:35
    │ │ │ │ -
    void push_back(boost::shared_ptr< HybridConditional > conditional)
    Add a hybrid conditional using a shared_ptr.
    Definition HybridBayesNet.h:69
    │ │ │ │ -
    void emplace_back(Conditional *conditional)
    Preferred: add a conditional directly using a pointer.
    Definition HybridBayesNet.h:82
    │ │ │ │ -
    void push_back(HybridConditional &&conditional)
    Add a conditional using a shared_ptr, using implicit conversion to a HybridConditional.
    Definition HybridBayesNet.h:99
    │ │ │ │ -
    HybridBayesNet()=default
    Construct empty Bayes net.
    │ │ │ │ -
    double operator()(const HybridValues &values) const
    Evaluate hybrid probability density for given HybridValues, sugar.
    Definition HybridBayesNet.h:117
    │ │ │ │ +
    A clique in a HybridBayesTree which is a HybridConditional internally.
    Definition HybridBayesTree.h:46
    │ │ │ │ +
    HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > &conditional)
    Copy constructor.
    Definition HybridBayesTree.h:54
    │ │ │ │ +
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ +
    HybridBayesTree()=default
    Default constructor, creates an empty Bayes tree.
    │ │ │ │ +
    BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
    Construct a new Bayes Tree Orphan Wrapper object.
    Definition HybridBayesTree.h:152
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print utility
    Definition HybridBayesTree.h:163
    │ │ │ │
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ -
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    Definition BayesTree.h:276
    │ │ │ │ +
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ +
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,203 +1,208 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridBayesNet.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 * 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 │ │ │ │ │ -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 { │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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. │ │ │ │ │ +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_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_._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. │ │ │ │ │ _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_:_:_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_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_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_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_:_:_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 │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._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/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/HybridBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    MixtureFactor.h File Reference
    │ │ │ │ +
    HybridBayesTree.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Nonlinear Mixture factor of continuous and discrete. │ │ │ │ +

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Nonlinear Mixture factor of continuous and discrete.

    │ │ │ │ -
    Author
    Kevin Doherty, kdohe.nosp@m.rty@.nosp@m.mit.e.nosp@m.du
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    Date
    December 2021
    │ │ │ │ +

    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang, Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear Mixture factor of continuous and discrete. │ │ │ │ │ - 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 │ │ │ │ │ - Varun Agrawal │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ Date │ │ │ │ │ - December 2021 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang, Varun Agrawal │ │ │ │ │ * _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_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/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/HybridNonlinearISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridSmoother.cpp 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::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,22 +1,25 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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_._c_p_p │ │ │ │ │ + * _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/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/HybridNonlinearISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,39 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianMixtureFactor.cpp File Reference
    │ │ │ │ +
    HybridNonlinearISAM.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ -More...

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

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    A set of Gaussian factors indexed by a set of discrete keys.

    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Mar 12, 2022
    │ │ │ │ +
    Date
    Sep 12, 2022
    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,20 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +HybridNonlinearISAM.cpp File Reference │ │ │ │ │ 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. │ │ │ │ │ + Date │ │ │ │ │ + Sep 12, 2022 │ │ │ │ │ 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 │ │ │ │ │ + * _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/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/HybridConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,51 +96,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridGaussianISAM.h File Reference
    │ │ │ │ +
    HybridConditional.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HybridGaussianISAM
    class  gtsam::HybridConditional
     Hybrid Conditional Density. More...
     
    struct  gtsam::traits< HybridGaussianISAM >
     traits More...
    struct  gtsam::traits< HybridConditional >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    March 31, 2022
    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    Date
    March 31, 2022
    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 │ │ │ │ │ -HybridGaussianISAM.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_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ + 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_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_<_ _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 │ │ │ │ │ - March 31, 2022 │ │ │ │ │ + Mar 11, 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_G_a_u_s_s_i_a_n_I_S_A_M_._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/a00536.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00536 = [ │ │ │ │ │ - ["gtsam::traits< HybridGaussianISAM >", "a03472.html", null] │ │ │ │ │ + ["gtsam::traits< HybridConditional >", "a03440.html", 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/HybridConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,206 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridGaussianISAM.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
    │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ -
    34class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
    │ │ │ │ -
    35 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    38 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    39
    │ │ │ │ -
    42
    │ │ │ │ - │ │ │ │ -
    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> {};
    │ │ │ │ -
    91
    │ │ │ │ -
    92} // namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ -
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/inference/Key.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
    │ │ │ │ + │ │ │ │ +
    66 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    114 const boost::shared_ptr<GaussianConditional>& continuousConditional);
    │ │ │ │ +
    115
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    164 return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
    │ │ │ │ +
    165 }
    │ │ │ │ +
    │ │ │ │ +
    166
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ + │ │ │ │ +
    A hybrid conditional in the Conditional Linear Gaussian scheme.
    │ │ │ │ + │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ -
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ -
    Definition HybridGaussianISAM.h:34
    │ │ │ │ -
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ +
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition DiscreteConditional.h:44
    │ │ │ │ +
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ +
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition HybridConditional.h:69
    │ │ │ │ +
    HybridFactor BaseFactor
    Typedef to our factor base class.
    Definition HybridConditional.h:67
    │ │ │ │ +
    boost::shared_ptr< Factor > inner_
    Type-erased pointer to the inner type.
    Definition HybridConditional.h:73
    │ │ │ │ +
    DiscreteConditional::shared_ptr asDiscrete() const
    Return conditional as a DiscreteConditional.
    Definition HybridConditional.h:172
    │ │ │ │ +
    GaussianMixture::shared_ptr asMixture() const
    Return HybridConditional as a GaussianMixture.
    Definition HybridConditional.h:154
    │ │ │ │ +
    boost::shared_ptr< Factor > inner() const
    Get the type-erased pointer to the inner type.
    Definition HybridConditional.h:177
    │ │ │ │ +
    bool frontalsIn(const VectorValues &measurements) const
    Check if VectorValues measurements contains all frontal keys.
    Definition HybridConditional.h:196
    │ │ │ │ +
    HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, size_t nFrontals)
    Construct a new Hybrid Conditional object.
    Definition HybridConditional.h:89
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition HybridConditional.h:66
    │ │ │ │ +
    GaussianConditional::shared_ptr asGaussian() const
    Return HybridConditional as a GaussianConditional.
    Definition HybridConditional.h:163
    │ │ │ │ +
    HybridConditional This
    Typedef to this class.
    Definition HybridConditional.h:65
    │ │ │ │ +
    HybridConditional()=default
    Default constructor needed for serialization.
    │ │ │ │ +
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,99 +1,275 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridGaussianISAM.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 │ │ │ │ │ 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_/_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 {}; │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +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_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. │ │ │ │ │ +_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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/HybridBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridSmoother.h File Reference
    │ │ │ │ +
    HybridBayesNet.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    An incremental smoother for hybrid factor graphs. │ │ │ │ +

    A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HybridSmoother
    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

    │ │ │ │ -

    An incremental smoother for hybrid factor graphs.

    │ │ │ │ -
    Author
    Varun Agrawal
    │ │ │ │ -
    Date
    October 2022
    │ │ │ │ +

    A Bayes net of Gaussian Conditionals indexed by discrete keys.

    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │ +
    │ │ │ │ +Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    December 2021
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -HybridSmoother.h File Reference │ │ │ │ │ -An incremental smoother for hybrid factor graphs. _M_o_r_e_._._. │ │ │ │ │ +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_S_m_o_o_t_h_e_r │ │ │ │ │ + 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 ********** │ │ │ │ │ -An incremental smoother for hybrid factor graphs. │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Fan Jiang │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - October 2022 │ │ │ │ │ + December 2021 │ │ │ │ │ * _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_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/a00539.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00539 = [ │ │ │ │ │ - ["gtsam::HybridSmoother", "a03500.html", "a03500"] │ │ │ │ │ + ["gtsam::traits< HybridBayesNet >", "a03412.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/HybridBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,73 +98,161 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridSmoother.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    │ │ │ │ +
    35class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
    │ │ │ │ +
    36 public:
    │ │ │ │ + │ │ │ │ +
    38 using This = HybridBayesNet;
    │ │ │ │ + │ │ │ │ +
    40 using shared_ptr = boost::shared_ptr<HybridBayesNet>;
    │ │ │ │ +
    41 using sharedConditional = boost::shared_ptr<ConditionalType>;
    │ │ │ │ +
    42
    │ │ │ │ +
    45
    │ │ │ │ +
    47 HybridBayesNet() = default;
    │ │ │ │ +
    48
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    86
    │ │ │ │ +
    │ │ │ │ +
    99 void push_back(HybridConditional &&conditional) {
    │ │ │ │ +
    100 factors_.push_back(
    │ │ │ │ +
    101 boost::make_shared<HybridConditional>(std::move(conditional)));
    │ │ │ │ +
    102 }
    │ │ │ │ +
    │ │ │ │ +
    103
    │ │ │ │ +
    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 HybridBayesNet prune(size_t maxNrLeaves);
    │ │ │ │ +
    189
    │ │ │ │ +
    197 AlgebraicDecisionTree<Key> logProbability(
    │ │ │ │ +
    198 const VectorValues &continuousValues) const;
    │ │ │ │ +
    199
    │ │ │ │ +
    200 using BayesNet::logProbability; // expose HybridValues version
    │ │ │ │ +
    201
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Bayes network.
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ +
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │
    A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
    Definition HybridBayesNet.h:35
    │ │ │ │ +
    void push_back(boost::shared_ptr< HybridConditional > conditional)
    Add a hybrid conditional using a shared_ptr.
    Definition HybridBayesNet.h:69
    │ │ │ │ +
    void emplace_back(Conditional *conditional)
    Preferred: add a conditional directly using a pointer.
    Definition HybridBayesNet.h:82
    │ │ │ │ +
    void push_back(HybridConditional &&conditional)
    Add a conditional using a shared_ptr, using implicit conversion to a HybridConditional.
    Definition HybridBayesNet.h:99
    │ │ │ │ +
    HybridBayesNet()=default
    Construct empty Bayes net.
    │ │ │ │ +
    double operator()(const HybridValues &values) const
    Evaluate hybrid probability density for given HybridValues, sugar.
    Definition HybridBayesNet.h:117
    │ │ │ │ +
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ -
    Definition HybridSmoother.h:25
    │ │ │ │ -
    GaussianMixture::shared_ptr gaussianMixture(size_t index) const
    Get the Gaussian Mixture from the Bayes Net posterior at index.
    Definition HybridSmoother.cpp:131
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    const HybridBayesNet & hybridBayesNet() const
    Return the Bayes Net posterior.
    Definition HybridSmoother.cpp:137
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,88 +1,203 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridSmoother.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#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); │ │ │ │ │ +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 │ │ │ │ │ +45 │ │ │ │ │ +_4_7 _H_y_b_r_i_d_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ +48 │ │ │ │ │ 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_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. │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_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_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_:_:_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 │ │ │ │ │ * _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_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/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/HybridGaussianFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,60 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    HybridEliminationTree.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    HybridGaussianFactorGraph.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Hybrid factor graph that uses type erasure. │ │ │ │ +More...

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

    │ │ │ │ -Classes

    class  gtsam::HybridEliminationTree
     Elimination Tree type for Hybrid Factor Graphs. 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

    │ │ │ │ -
    Date
    Mar 11, 2022
    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ +

    Hybrid factor graph that uses type erasure.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -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 | _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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +Hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Mar 11, 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_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/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/HybridSmoother.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,56 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    HybridGaussianFactorGraph.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    HybridSmoother.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Linearized Hybrid factor graph that uses type erasure. │ │ │ │ +

    An incremental smoother for hybrid factor graphs. │ │ │ │ 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
    │ │ │ │ +

    An incremental smoother for hybrid factor graphs.

    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │ +
    Date
    October 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +An incremental smoother for hybrid factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang, Varun Agrawal, Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + October 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_F_a_c_t_o_r_G_r_a_p_h_._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/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/HybridFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    GaussianMixture.cpp 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::HybridFactor
     Base class for truly hybrid probabilistic factors. More...
     
    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< DiscreteKeygtsam::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
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Mar 12, 2022
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -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 | _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_:_:_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_<_ _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 │ │ │ │ │ - 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_._c_p_p │ │ │ │ │ + * _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/a00551.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00551 = [ │ │ │ │ │ - ["DiscreteKeysAsSet", "a00551.html#ad92a1966f54e02a7c2ce061c1228c7b3", null] │ │ │ │ │ + ["gtsam::traits< HybridFactor >", "a03452.html", null], │ │ │ │ │ + ["GaussianFactorGraphTree", "a00551.html#a8b4a8e0d32b81b0fa2fa15c437b89e7c", null] │ │ │ │ │ ]; │ │ │ ├── ./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/GaussianMixture.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,45 +95,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    HybridNonlinearFactorGraph.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    GaussianMixture.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +

    A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HybridNonlinearFactorGraph
    class  gtsam::GaussianMixture
     A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network. More...
     
    struct  gtsam::traits< HybridNonlinearFactorGraph >
    struct  gtsam::traits< GaussianMixture >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

    │ │ │ │ +std::set< DiscreteKeygtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
     Return the DiscreteKey vector as a set.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Nonlinear hybrid factor graph that uses type erasure.

    │ │ │ │ -
    Author
    Varun Agrawal
    │ │ │ │ -
    Date
    May 28, 2022
    │ │ │ │ +

    A hybrid conditional in the Conditional Linear Gaussian scheme.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    Date
    Mar 12, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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_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_:_:_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_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_<_ _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 ********** │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + Mar 12, 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 │ │ │ │ │ + * _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/a00554.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00554 = [ │ │ │ │ │ - ["gtsam::HybridNonlinearFactorGraph", "a03488.html", "a03488"], │ │ │ │ │ - ["gtsam::traits< HybridNonlinearFactorGraph >", "a03492.html", null] │ │ │ │ │ + ["gtsam::traits< GaussianMixture >", "a03396.html", null], │ │ │ │ │ + ["DiscreteKeysAsSet", "a00554.html#ad92a1966f54e02a7c2ce061c1228c7b3", 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/GaussianMixture.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,182 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridNonlinearFactorGraph.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
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    25class HybridGaussianFactorGraph;
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ -
    33class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
    │ │ │ │ -
    34 protected:
    │ │ │ │ -
    35 public:
    │ │ │ │ -
    36 using Base = HybridFactorGraph;
    │ │ │ │ - │ │ │ │ -
    38 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    39
    │ │ │ │ - │ │ │ │ -
    41 using Indices = KeyVector;
    │ │ │ │ -
    42
    │ │ │ │ -
    45
    │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    53 template <class DERIVEDFACTOR>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    84 : public Testable<HybridNonlinearFactorGraph> {};
    │ │ │ │ -
    │ │ │ │ -
    85
    │ │ │ │ -
    86} // namespace gtsam
    │ │ │ │ -
    Factor graph with utilities for hybrid factors.
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/discrete/DecisionTree-inl.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>;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    61
    │ │ │ │ + │ │ │ │ +
    64
    │ │ │ │ +
    65 private:
    │ │ │ │ +
    66 Conditionals conditionals_;
    │ │ │ │ +
    67 double logConstant_;
    │ │ │ │ +
    68
    │ │ │ │ +
    72 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
    │ │ │ │ +
    73
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ + │ │ │ │ +
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ + │ │ │ │ +
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
    Return the DiscreteKey vector as a set.
    Definition GaussianMixture.cpp:221
    │ │ │ │
    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
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ -
    Definition HybridNonlinearFactorGraph.h:33
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridNonlinearFactorGraph.h:38
    │ │ │ │ -
    HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridNonlinearFactorGraph.h:54
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ +
    An assignment from labels to value index (size_t).
    Definition Assignment.h:37
    │ │ │ │ + │ │ │ │ +
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ +
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ +
    A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network.
    Definition GaussianMixture.h:55
    │ │ │ │ +
    double logNormalizationConstant() const override
    The log normalization constant is max of the the individual log-normalization constants.
    Definition GaussianMixture.h:161
    │ │ │ │ +
    double operator()(const HybridValues &values) const
    Evaluate probability density, sugar.
    Definition GaussianMixture.h:231
    │ │ │ │ +
    GaussianMixture()=default
    Default constructor, mainly for serialization.
    │ │ │ │ +
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,109 +1,236 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridNonlinearFactorGraph.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 │ │ │ │ │ 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_/_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; │ │ │ │ │ -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. │ │ │ │ │ +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> │ │ │ │ │ +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 │ │ │ │ │ +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_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 │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ +_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_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_:_:_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 │ │ │ │ │ -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 │ │ │ │ │ +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_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._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/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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,64 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    HybridFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    HybridNonlinearFactorGraph.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +More...

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

    │ │ │ │ -Classes

    class  gtsam::HybridFactor
     Base class for truly hybrid probabilistic factors. More...
     
    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

    │ │ │ │ -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,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 │ │ │ │ │ -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_:_:_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_<_ _H_y_b_r_i_d_F_a_c_t_o_r_ _> │ │ │ │ │ -  │ │ │ │ │ +_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_._._. │ │ │ │ │ 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_._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/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/GaussianMixtureFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,41 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    HybridConditional.h File Reference
    │ │ │ │ +
    GaussianMixtureFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HybridConditional
     Hybrid Conditional Density. More...
    class  gtsam::GaussianMixtureFactor
     Implementation of a discrete conditional mixture factor. More...
     
    struct  gtsam::traits< HybridConditional >
    struct  gtsam::traits< GaussianMixtureFactor >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    Mar 11, 2022
    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ +

    A set of GaussianFactors, indexed by a set of discrete keys.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    Mar 12, 2022
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -HybridConditional.h File Reference │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ + 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_<_ _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +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 │ │ │ │ │ - * _H_y_b_r_i_d_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_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/a00560.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00560 = [ │ │ │ │ │ - ["gtsam::traits< HybridConditional >", "a03440.html", null] │ │ │ │ │ + ["gtsam::traits< GaussianMixtureFactor >", "a03404.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00560_source.html │ │ │ │ @@ -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/hybrid/GaussianMixtureFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,206 +98,149 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridConditional.h
    │ │ │ │ +
    GaussianMixtureFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/inference/Key.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
    │ │ │ │ - │ │ │ │ -
    66 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    114 const boost::shared_ptr<GaussianConditional>& continuousConditional);
    │ │ │ │ -
    115
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    164 return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
    │ │ │ │ -
    165 }
    │ │ │ │ -
    │ │ │ │ -
    166
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    A hybrid conditional in the Conditional Linear Gaussian scheme.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    32class HybridValues;
    │ │ │ │ +
    33class DiscreteValues;
    │ │ │ │ +
    34class VectorValues;
    │ │ │ │ +
    35
    │ │ │ │ +
    │ │ │ │ +
    47class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    │ │ │ │ +
    48 public:
    │ │ │ │ +
    49 using Base = HybridFactor;
    │ │ │ │ + │ │ │ │ +
    51 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    52
    │ │ │ │ +
    53 using sharedFactor = boost::shared_ptr<GaussianFactor>;
    │ │ │ │ +
    54
    │ │ │ │ + │ │ │ │ +
    57
    │ │ │ │ +
    58 private:
    │ │ │ │ +
    60 Factors factors_;
    │ │ │ │ +
    61
    │ │ │ │ +
    68 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
    │ │ │ │ +
    69
    │ │ │ │ +
    70 public:
    │ │ │ │ +
    73
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    130
    │ │ │ │ +
    138 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
    │ │ │ │ +
    139
    │ │ │ │ +
    144 double error(const HybridValues &values) const override;
    │ │ │ │ +
    145
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ +
    Algebraic Decision Trees.
    │ │ │ │ +
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ + │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition DiscreteConditional.h:44
    │ │ │ │ +
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ + │ │ │ │
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ -
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition HybridConditional.h:69
    │ │ │ │ -
    HybridFactor BaseFactor
    Typedef to our factor base class.
    Definition HybridConditional.h:67
    │ │ │ │ -
    boost::shared_ptr< Factor > inner_
    Type-erased pointer to the inner type.
    Definition HybridConditional.h:73
    │ │ │ │ -
    DiscreteConditional::shared_ptr asDiscrete() const
    Return conditional as a DiscreteConditional.
    Definition HybridConditional.h:172
    │ │ │ │ -
    GaussianMixture::shared_ptr asMixture() const
    Return HybridConditional as a GaussianMixture.
    Definition HybridConditional.h:154
    │ │ │ │ -
    boost::shared_ptr< Factor > inner() const
    Get the type-erased pointer to the inner type.
    Definition HybridConditional.h:177
    │ │ │ │ -
    bool frontalsIn(const VectorValues &measurements) const
    Check if VectorValues measurements contains all frontal keys.
    Definition HybridConditional.h:196
    │ │ │ │ -
    HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, size_t nFrontals)
    Construct a new Hybrid Conditional object.
    Definition HybridConditional.h:89
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition HybridConditional.h:66
    │ │ │ │ -
    GaussianConditional::shared_ptr asGaussian() const
    Return HybridConditional as a GaussianConditional.
    Definition HybridConditional.h:163
    │ │ │ │ -
    HybridConditional This
    Typedef to this class.
    Definition HybridConditional.h:65
    │ │ │ │ -
    HybridConditional()=default
    Default constructor needed for serialization.
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ +
    Implementation of a discrete conditional mixture factor.
    Definition GaussianMixtureFactor.h:47
    │ │ │ │ +
    GaussianMixtureFactor()=default
    Default constructor, mainly for serialization.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor)
    Add MixtureFactor to a Sum, syntactic sugar.
    Definition GaussianMixtureFactor.h:147
    │ │ │ │
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    the error.
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,275 +1,191 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridConditional.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 │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_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. │ │ │ │ │ _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. │ │ │ │ │ +_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_:_:_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_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:44 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_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_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/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/HybridFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianMixtureFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    HybridFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +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

    │ │ │ │ -

    A set of GaussianFactors, indexed by a set of discrete keys.

    │ │ │ │ -
    Author
    Fan Jiang
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Mar 12, 2022
    │ │ │ │ +
    Date
    Mar 11, 2022
    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ + Date │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ 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_._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/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/HybridFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    HybridNonlinearFactorGraph.cpp File Reference
    │ │ │ │ +
    HybridFactorGraph.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +

    Factor graph with utilities for hybrid factors. │ │ │ │ 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
    │ │ │ │ +

    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 │ │ │ │ │ -HybridNonlinearFactorGraph.cpp File Reference │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. _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 ********** │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ +Factor graph with utilities for hybrid factors. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + January, 2023 │ │ │ │ │ * _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_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/a00572.html │ │ │ │ @@ -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/inference/ClusterTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,83 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
    │ │ │ │ -
    graph.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    ClusterTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Graph algorithm using boost library. │ │ │ │ +

    Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    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::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::SGraph< KEY >
    struct  gtsam::ClusterTree< GRAPH >::Cluster
     A Cluster is just a collection of factors. More...
     
    class  gtsam::PredecessorMap< KEY >
     Map from variable key to parent key. 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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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< Valuesgtsam::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

    │ │ │ │ -

    Graph algorithm using boost library.

    │ │ │ │ -
    Author
    : Kai Ni
    │ │ │ │ -
    Date
    Jan 11, 2010
    │ │ │ │ +

    Collects factorgraph fragments defined on variable clusters, arranged in a tree.

    │ │ │ │ +
    Date
    Oct 8, 2013
    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,75 +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 | _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 │ │ │ │ │ +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_:_:_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_:_:_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_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │ +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_:_:_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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -Graph algorithm using boost library. │ │ │ │ │ - Author │ │ │ │ │ - : Kai Ni │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ Date │ │ │ │ │ - Jan 11, 2010 │ │ │ │ │ + Oct 8, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Kai Ni │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _g_r_a_p_h_._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/a00572.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,5 @@ │ │ │ │ │ var a00572 = [ │ │ │ │ │ - ["gtsam::SDGraph< KEY >", "a03616.html", null], │ │ │ │ │ - ["gtsam::SGraph< KEY >", "a03620.html", null], │ │ │ │ │ - ["gtsam::PredecessorMap< KEY >", "a03624.html", "a03624"], │ │ │ │ │ - ["composePoses", "a00572.html#a62b34d6038ccdfa84d8a5bb0c15328b1", null], │ │ │ │ │ - ["findMinimumSpanningTree", "a00572.html#a7771a103b41708e88763ff67059ff01d", null], │ │ │ │ │ - ["predecessorMap2Graph", "a00572.html#ae67d0d86c4e55b175859967a70b36983", null], │ │ │ │ │ - ["predecessorMap2Keys", "a00572.html#a18a265afdea057ea54be10262396be12", null], │ │ │ │ │ - ["split", "a00572.html#a3453247aec1d8b4642947ffd4bca426b", null], │ │ │ │ │ - ["toBoostGraph", "a00572.html#a6c0319a6ebcc83d4a5cf9ff51729bb80", 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/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/ClusterTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,115 +98,273 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    graph.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
    │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    31
    │ │ │ │ -
    32 // type definitions :
    │ │ │ │ +
    12#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    15
    │ │ │ │ +
    16namespace gtsam {
    │ │ │ │ +
    17
    │ │ │ │ +
    24template <class GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    26 public:
    │ │ │ │ +
    27 typedef GRAPH FactorGraphType;
    │ │ │ │ + │ │ │ │ +
    29 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    30
    │ │ │ │ +
    31 typedef typename GRAPH::FactorType FactorType;
    │ │ │ │ +
    32 typedef boost::shared_ptr<FactorType> sharedFactor;
    │ │ │ │
    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 };
    │ │ │ │ -
    │ │ │ │ +
    35 // TODO(frank): re-factor JunctionTree so we can make members private
    │ │ │ │ +
    │ │ │ │ +
    36 struct Cluster {
    │ │ │ │ + │ │ │ │ +
    38 Children children;
    │ │ │ │ +
    39
    │ │ │ │ +
    40 typedef Ordering Keys;
    │ │ │ │ + │ │ │ │ +
    42
    │ │ │ │ + │ │ │ │
    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);
    │ │ │ │ -
    71
    │ │ │ │ -
    78 template<class G, class F, class KEY> SDGraph<KEY> toBoostGraph(const G& graph);
    │ │ │ │ +
    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
    │ │ │ │ -
    85 template<class G, class V, class KEY>
    │ │ │ │ -
    86 boost::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
    │ │ │ │ +
    80 size_t nrFactors() const {
    │ │ │ │ +
    81 return factors.size();
    │ │ │ │ +
    82 }
    │ │ │ │ +
    83
    │ │ │ │ +
    84 size_t nrFrontals() const {
    │ │ │ │ +
    85 return orderedFrontalKeys.size();
    │ │ │ │ +
    86 }
    │ │ │ │
    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
    │ │ │ │ +
    88 int problemSize() const {
    │ │ │ │ +
    89 return problemSize_;
    │ │ │ │ +
    90 }
    │ │ │ │ +
    91
    │ │ │ │ +
    93 virtual void print(const std::string& s = "",
    │ │ │ │ +
    94 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │
    95
    │ │ │ │ -
    99 template<class G, class KEY, class FACTOR2>
    │ │ │ │ -
    100 PredecessorMap<KEY> findMinimumSpanningTree(const G& g) ;
    │ │ │ │ +
    97 std::vector<size_t> nrFrontalsOfChildren() const;
    │ │ │ │ +
    98
    │ │ │ │ +
    100 void merge(const boost::shared_ptr<Cluster>& cluster);
    │ │ │ │
    101
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    112#include <gtsam/inference/graph-inl.h>
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    113 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
    │ │ │ │ +
    114
    │ │ │ │ +
    115 protected:
    │ │ │ │ + │ │ │ │ +
    117
    │ │ │ │ +
    120
    │ │ │ │ +
    │ │ │ │ +
    123 ClusterTree(const This& other) {
    │ │ │ │ +
    124 *this = other;
    │ │ │ │ +
    125 }
    │ │ │ │ +
    │ │ │ │ +
    126
    │ │ │ │ +
    128
    │ │ │ │ +
    129 public:
    │ │ │ │ +
    130
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    185 public:
    │ │ │ │ +
    186 typedef BAYESTREE BayesTreeType;
    │ │ │ │ +
    187 typedef GRAPH FactorGraphType;
    │ │ │ │ + │ │ │ │ +
    189 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    190
    │ │ │ │ +
    191 typedef typename BAYESTREE::ConditionalType ConditionalType;
    │ │ │ │ +
    192 typedef boost::shared_ptr<ConditionalType>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    232 return remainingFactors_;
    │ │ │ │ +
    233 }
    │ │ │ │ +
    │ │ │ │ +
    234
    │ │ │ │ +
    236
    │ │ │ │ +
    237 protected:
    │ │ │ │ +
    240
    │ │ │ │ +
    243 This& operator=(const This& other);
    │ │ │ │ +
    244
    │ │ │ │ + │ │ │ │ +
    247
    │ │ │ │ +
    249};
    │ │ │ │ +
    │ │ │ │ +
    250}
    │ │ │ │ +
    251
    │ │ │ │ + │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
    find the minimum spanning tree using boost graph library
    Definition graph-inl.h:232
    │ │ │ │ -
    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
    │ │ │ │ -
    SDGraph is undirected graph with variable keys and double edge weights.
    Definition graph.h:40
    │ │ │ │ -
    Definition graph.h:47
    │ │ │ │ -
    Map from variable key to parent key.
    Definition graph.h:58
    │ │ │ │ -
    void insert(const KEY &key, const KEY &parent)
    convenience insert so we can pass ints for TypedSymbol keys
    Definition graph.h:61
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ +
    EliminatableClusterTree< BAYESTREE, GRAPH > This
    This class.
    Definition ClusterTree.h:188
    │ │ │ │ +
    EliminatableClusterTree()
    Default constructor to be used in derived classes.
    Definition ClusterTree.h:246
    │ │ │ │ +
    BAYESTREE::ConditionalType ConditionalType
    The type of conditionals.
    Definition ClusterTree.h:191
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< ConditionalType > sharedConditional
    Shared pointer to a conditional.
    Definition ClusterTree.h:193
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:197
    │ │ │ │ +
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition ClusterTree.h:231
    │ │ │ │ +
    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
    │ │ │ │ +
    BAYESTREE BayesTreeType
    The BayesTree type produced by elimination.
    Definition ClusterTree.h:186
    │ │ │ │ +
    GRAPH::FactorType FactorType
    The type of factors.
    Definition ClusterTree.h:196
    │ │ │ │ +
    GRAPH FactorGraphType
    The factor graph type.
    Definition ClusterTree.h:187
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition ClusterTree.h:189
    │ │ │ │ +
    GRAPH::Eliminate Eliminate
    Typedef for an eliminate subroutine.
    Definition ClusterTree.h:195
    │ │ │ │ +
    A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
    Definition ClusterTree.h:25
    │ │ │ │ +
    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
    │ │ │ │ +
    ClusterTree< GRAPH > This
    This class.
    Definition ClusterTree.h:28
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition ClusterTree.h:29
    │ │ │ │ +
    FastVector< sharedNode > roots_
    concept check
    Definition ClusterTree.h:116
    │ │ │ │ +
    GRAPH::FactorType FactorType
    The type of factors.
    Definition ClusterTree.h:31
    │ │ │ │ +
    GRAPH FactorGraphType
    The factor graph type.
    Definition ClusterTree.h:27
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Print the cluster tree.
    Definition ClusterTree-inst.h:98
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:32
    │ │ │ │ +
    const FastVector< sharedNode > & roots() const
    Return the set of roots (one for a tree, multiple for a forest)
    Definition ClusterTree.h:159
    │ │ │ │ +
    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
    │ │ │ │ +
    ClusterTree()
    Default constructor.
    Definition ClusterTree.h:132
    │ │ │ │ +
    boost::shared_ptr< Cluster > sharedCluster
    Shared pointer to Cluster.
    Definition ClusterTree.h:106
    │ │ │ │ +
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │ +
    Cluster(Key key, const CONTAINER &factorsToAdd)
    Construct from factors associated with a single key.
    Definition ClusterTree.h:57
    │ │ │ │ +
    Children children
    sub-trees
    Definition ClusterTree.h:38
    │ │ │ │ +
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition ClusterTree-inst.h:26
    │ │ │ │ +
    Keys orderedFrontalKeys
    Frontal keys of this node.
    Definition ClusterTree.h:41
    │ │ │ │ +
    void mergeChildren(const std::vector< bool > &merge)
    Merge all children for which bit is set into this node.
    Definition ClusterTree-inst.h:56
    │ │ │ │ +
    void merge(const boost::shared_ptr< Cluster > &cluster)
    Merge in given cluster.
    Definition ClusterTree-inst.h:44
    │ │ │ │ +
    std::vector< size_t > nrFrontalsOfChildren() const
    Return a vector with nrFrontal keys for each child.
    Definition ClusterTree-inst.h:34
    │ │ │ │ +
    FactorGraphType factors
    Factors associated with this node.
    Definition ClusterTree.h:43
    │ │ │ │ +
    void addChild(const boost::shared_ptr< Cluster > &cluster)
    Add a child cluster.
    Definition ClusterTree.h:71
    │ │ │ │ +
    void addFactors(Key key, const CONTAINER &factorsToAdd)
    Add factors associated with a single key.
    Definition ClusterTree.h:64
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,142 +1,384 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -graph.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 │ │ │ │ │ -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 : │ │ │ │ │ +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 │ │ │ │ │ -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 }; │ │ │ │ │ +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 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); │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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) ; │ │ │ │ │ +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 │ │ │ │ │ -106 template │ │ │ │ │ -107 void _s_p_l_i_t(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; │ │ │ │ │ -108 │ │ │ │ │ -109 │ │ │ │ │ -110} // namespace gtsam │ │ │ │ │ +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 │ │ │ │ │ -112#include │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _g_r_a_p_h_._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/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/graph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,52 +95,83 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    BayesTree.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions
    │ │ │ │ +
    graph.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ +

    Graph algorithm using boost library. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::BayesTreeCliqueStats
     clique statistics More...
    class  gtsam::SDGraph< KEY >
     SDGraph is undirected graph with variable keys and double edge weights. More...
     
    struct  gtsam::BayesTreeCliqueData
     store all the sizes
    │ │ │ │ - More...
    class  gtsam::SGraph< KEY >
     
    class  gtsam::BayesTree< CLIQUE >
     Bayes tree. More...
     
    class  gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >
    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< Valuesgtsam::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

    │ │ │ │ -

    Bayes Tree is a tree of cliques of a Bayes Chain.

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

    Graph algorithm using boost library.

    │ │ │ │ +
    Author
    : Kai Ni
    │ │ │ │ +
    Date
    Jan 11, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -BayesTree.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 | _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 │ │ │ │ │ -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_:_:_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_:_:_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_:_:_S_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │   │ │ │ │ │ - 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_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +Graph algorithm using boost library. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + : Kai Ni │ │ │ │ │ + Date │ │ │ │ │ + Jan 11, 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 │ │ │ │ │ + * _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/a00575.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,11 @@ │ │ │ │ │ var a00575 = [ │ │ │ │ │ - ["gtsam::BayesTreeCliqueStats", "a03532.html", null], │ │ │ │ │ - ["gtsam::BayesTreeCliqueData", "a03536.html", null], │ │ │ │ │ - ["gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >", "a03544.html", "a03544"] │ │ │ │ │ + ["gtsam::SDGraph< KEY >", "a03616.html", null], │ │ │ │ │ + ["gtsam::SGraph< KEY >", "a03620.html", null], │ │ │ │ │ + ["gtsam::PredecessorMap< KEY >", "a03624.html", "a03624"], │ │ │ │ │ + ["composePoses", "a00575.html#a62b34d6038ccdfa84d8a5bb0c15328b1", null], │ │ │ │ │ + ["findMinimumSpanningTree", "a00575.html#a7771a103b41708e88763ff67059ff01d", null], │ │ │ │ │ + ["predecessorMap2Graph", "a00575.html#ae67d0d86c4e55b175859967a70b36983", null], │ │ │ │ │ + ["predecessorMap2Keys", "a00575.html#a18a265afdea057ea54be10262396be12", null], │ │ │ │ │ + ["split", "a00575.html#a3453247aec1d8b4642947ffd4bca426b", null], │ │ │ │ │ + ["toBoostGraph", "a00575.html#a6c0319a6ebcc83d4a5cf9ff51729bb80", 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/graph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,295 +98,115 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BayesTree.h
    │ │ │ │ +
    graph.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>
    │ │ │ │ - │ │ │ │ -
    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 };
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    54 /* ************************************************************************* */
    │ │ │ │ -
    65 template<class CLIQUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    67 {
    │ │ │ │ -
    68 protected:
    │ │ │ │ -
    69 typedef BayesTree<CLIQUE> This;
    │ │ │ │ -
    70 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    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
    │ │ │ │ -
    72 public:
    │ │ │ │ -
    73 typedef CLIQUE Clique;
    │ │ │ │ -
    74 typedef boost::shared_ptr<Clique> sharedClique;
    │ │ │ │ -
    75 typedef Clique Node;
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    90
    │ │ │ │ - │ │ │ │ -
    93
    │ │ │ │ - │ │ │ │ -
    96
    │ │ │ │ -
    97 protected:
    │ │ │ │ -
    98
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    104
    │ │ │ │ -
    107
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    159
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    242
    │ │ │ │ -
    243 protected:
    │ │ │ │ -
    244
    │ │ │ │ -
    246 void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
    │ │ │ │ -
    247 int parentnum = 0) const;
    │ │ │ │ -
    248
    │ │ │ │ - │ │ │ │ -
    251
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    112#include <gtsam/inference/graph-inl.h>
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition ConcurrentMap.h:68
    │ │ │ │ -
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ -
    clique statistics
    Definition BayesTree.h:39
    │ │ │ │ -
    store all the sizes
    Definition BayesTree.h:48
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    Nodes nodes_
    Map from indices to Clique.
    Definition BayesTree.h:100
    │ │ │ │ -
    void removeClique(sharedClique clique)
    remove a clique: warning, can result in a forest
    Definition BayesTree-inst.h:424
    │ │ │ │ -
    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
    │ │ │ │ -
    void fillNodesIndex(const sharedClique &subtree)
    Fill the nodes index for a subtree.
    Definition BayesTree-inst.h:246
    │ │ │ │ -
    void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output to graphviz format, stream version.
    Definition BayesTree-inst.h:64
    │ │ │ │ -
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ -
    sharedClique operator[](Key j) const
    Access node by variable.
    Definition BayesTree.h:146
    │ │ │ │ -
    This & operator=(const This &other)
    Assignment operator.
    Definition BayesTree-inst.h:199
    │ │ │ │ -
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ -
    BayesTree()
    Create an empty Bayes Tree.
    Definition BayesTree.h:109
    │ │ │ │ -
    Clique Node
    Synonym for Clique (TODO: remove)
    Definition BayesTree.h:75
    │ │ │ │ -
    void clear()
    Remove all nodes.
    Definition BayesTree-inst.h:408
    │ │ │ │ -
    Roots roots_
    Root cliques.
    Definition BayesTree.h:103
    │ │ │ │ -
    void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
    add a clique (top down)
    Definition BayesTree-inst.h:142
    │ │ │ │ -
    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
    │ │ │ │ -
    sharedClique sharedNode
    Synonym for sharedClique (TODO: remove)
    Definition BayesTree.h:76
    │ │ │ │ -
    Key findParentClique(const CONTAINER &parents) const
    Find parent clique of a conditional.
    Definition BayesTree-inst.h:238
    │ │ │ │ -
    size_t size() const
    number of cliques
    Definition BayesTree-inst.h:133
    │ │ │ │ -
    void deleteCachedShortcuts()
    Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
    Definition BayesTree-inst.h:416
    │ │ │ │ -
    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
    │ │ │ │ -
    const Nodes & nodes() const
    Return nodes.
    Definition BayesTree.h:143
    │ │ │ │ -
    FastList< sharedClique > Cliques
    A convenience class for a list of shared cliques.
    Definition BayesTree.h:89
    │ │ │ │ -
    CLIQUE Clique
    The clique type, normally BayesTreeClique.
    Definition BayesTree.h:73
    │ │ │ │ -
    sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
    Return marginal on any variable.
    Definition BayesTree-inst.h:270
    │ │ │ │ -
    const Roots & roots() const
    return root cliques
    Definition BayesTree.h:149
    │ │ │ │ -
    const sharedClique & clique(Key j) const
    alternate syntax for matlab: find the clique that contains the variable with Key j
    Definition BayesTree.h:152
    │ │ │ │ -
    ConcurrentMap< Key, sharedClique > Nodes
    Map from keys to Clique.
    Definition BayesTree.h:92
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BayesTree.h:263
    │ │ │ │ -
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTree-inst.h:55
    │ │ │ │ -
    BayesTreeCliqueData getCliqueData() const
    Gather data on all cliques.
    Definition BayesTree-inst.h:35
    │ │ │ │ -
    Cliques removeSubtree(const sharedClique &subtree)
    Remove the requested subtree.
    Definition BayesTree-inst.h:496
    │ │ │ │ -
    bool empty() const
    Check if there are any cliques in the tree.
    Definition BayesTree.h:138
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ -
    void insertRoot(const sharedClique &subtree)
    Insert a new subtree with known parent clique.
    Definition BayesTree-inst.h:260
    │ │ │ │ -
    void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    output to file with graphviz format.
    Definition BayesTree-inst.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    FastVector< sharedClique > Roots
    Root cliques.
    Definition BayesTree.h:95
    │ │ │ │ -
    Definition BayesTree.h:276
    │ │ │ │ -
    BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
    Construct a new Bayes Tree Orphan Wrapper object.
    Definition BayesTree.h:293
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
    find the minimum spanning tree using boost graph library
    Definition graph-inl.h:232
    │ │ │ │ +
    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
    │ │ │ │ +
    SDGraph is undirected graph with variable keys and double edge weights.
    Definition graph.h:40
    │ │ │ │ +
    Definition graph.h:47
    │ │ │ │ +
    Map from variable key to parent key.
    Definition graph.h:58
    │ │ │ │ +
    void insert(const KEY &key, const KEY &parent)
    convenience insert so we can pass ints for TypedSymbol keys
    Definition graph.h:61
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,451 +1,142 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesTree.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/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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> │ │ │ │ │ -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 }; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -_1_0_0 _N_o_d_e_s _n_o_d_e_s__; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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} │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ +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_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_:_:_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 │ │ │ │ │ -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_:_:_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 │ │ │ │ │ - * _B_a_y_e_s_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/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/ISAM-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,143 +98,100 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Conditional-inst.h
    │ │ │ │ +
    ISAM-inst.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
    │ │ │ │ - │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │
    22
    │ │ │ │ -
    23#include <cmath>
    │ │ │ │ -
    24#include <iostream>
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28/* ************************************************************************* */
    │ │ │ │ -
    29template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    42 double tol) const {
    │ │ │ │ -
    43 return nrFrontals_ == c.nrFrontals_;
    │ │ │ │ -
    44}
    │ │ │ │ -
    │ │ │ │ +
    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);
    │ │ │ │
    45
    │ │ │ │ -
    46/* ************************************************************************* */
    │ │ │ │ -
    47template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    49 const HybridValues& c) const {
    │ │ │ │ -
    50 throw std::runtime_error("Conditional::logProbability is not implemented");
    │ │ │ │ -
    51}
    │ │ │ │ -
    │ │ │ │ -
    52
    │ │ │ │ -
    53/* ************************************************************************* */
    │ │ │ │ -
    54template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    56 const HybridValues& c) const {
    │ │ │ │ -
    57 throw std::runtime_error("Conditional::evaluate is not implemented");
    │ │ │ │ +
    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 FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    61template<class BAYESTREE>
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    63 const {
    │ │ │ │ -
    64 throw std::runtime_error(
    │ │ │ │ -
    65 "Conditional::logNormalizationConstant is not implemented");
    │ │ │ │ +
    62void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
    │ │ │ │ +
    63 const Eliminate& function) {
    │ │ │ │ +
    │ │ │ │ +
    64 Cliques orphans;
    │ │ │ │ +
    65 this->updateInternal(newFactors, &orphans, function);
    │ │ │ │
    66}
    │ │ │ │
    │ │ │ │ -
    67
    │ │ │ │ -
    68/* ************************************************************************* */
    │ │ │ │ -
    69template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    71 return std::exp(logNormalizationConstant());
    │ │ │ │ -
    72}
    │ │ │ │ -
    │ │ │ │ -
    73
    │ │ │ │ -
    74/* ************************************************************************* */
    │ │ │ │ -
    75template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    76template <class VALUES>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    98} // namespace gtsam
    │ │ │ │
    │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ +
    67
    │ │ │ │ +
    68}
    │ │ │ │ + │ │ │ │ +
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
    Check invariants of this conditional, given the values x.
    Definition Conditional-inst.h:77
    │ │ │ │ -
    size_t nrFrontals_
    The first nrFrontal variables are frontal and the rest are parents.
    Definition Conditional.h:67
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equals(const This &c, double tol=1e-9) const
    check equality
    Definition Conditional-inst.h:41
    │ │ │ │ -
    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
    │ │ │ │ -
    double normalizationConstant() const
    Non-virtual, exponentiate logNormalizationConstant.
    Definition Conditional-inst.h:70
    │ │ │ │ -
    void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print with optional formatter
    Definition Conditional-inst.h:30
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ + │ │ │ │ +
    Definition BayesTree.h:276
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,167 +1,113 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Conditional-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) │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +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); │ │ │ │ │ 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"); │ │ │ │ │ +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_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"); │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * CCoonnddiittiioonnaall--iinnsstt..hh │ │ │ │ │ + * _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/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/JunctionTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,50 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Ordering.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
    │ │ │ │ +

    The junction tree, template bodies.

    │ │ │ │ +
    Date
    Feb 4, 2010
    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ -Andrew Melim
    │ │ │ │ -
    Date
    Sep 2, 2010
    │ │ │ │ +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 │ │ │ │ │ -Ordering.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 │ │ │ │ │ - Andrew Melim │ │ │ │ │ - Date │ │ │ │ │ - Sep 2, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/BayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,41 +96,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Conditional.h File Reference
    │ │ │ │ +
    BayesTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Base class for conditional densities. │ │ │ │ +

    Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Conditional< FACTOR, DERIVEDCONDITIONAL >
    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 conditional densities.

    │ │ │ │ +

    Bayes Tree is a tree of cliques of a Bayes Chain.

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +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 │ │ │ │ │ -Conditional.h File Reference │ │ │ │ │ -Base class for conditional densities. _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_:_:_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_ _> │ │ │ │ │ +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 conditional densities. │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00587.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00587 = [ │ │ │ │ │ - ["gtsam::Conditional< FACTOR, DERIVEDCONDITIONAL >", "a03572.html", "a03572"] │ │ │ │ │ + ["gtsam::BayesTreeCliqueStats", "a03532.html", null], │ │ │ │ │ + ["gtsam::BayesTreeCliqueData", "a03536.html", null], │ │ │ │ │ + ["gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >", "a03544.html", "a03544"] │ │ │ │ │ ]; │ │ │ ├── ./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/BayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,185 +98,295 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Conditional.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// \callgraph
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ -
    21#include <boost/range.hpp>
    │ │ │ │ -
    22
    │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    27 class HybridValues; // forward declaration.
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │
    28
    │ │ │ │ -
    62 template<class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    64 {
    │ │ │ │ -
    65 protected:
    │ │ │ │ - │ │ │ │ -
    68
    │ │ │ │ -
    69 private:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    86
    │ │ │ │ - │ │ │ │ -
    89
    │ │ │ │ -
    91
    │ │ │ │ -
    92 public:
    │ │ │ │ -
    95
    │ │ │ │ -
    97 void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
    │ │ │ │ +
    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 };
    │ │ │ │ +
    │ │ │ │ +
    53
    │ │ │ │ +
    54 /* ************************************************************************* */
    │ │ │ │ +
    65 template<class CLIQUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    67 {
    │ │ │ │ +
    68 protected:
    │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    90
    │ │ │ │ + │ │ │ │ +
    93
    │ │ │ │ + │ │ │ │ +
    96
    │ │ │ │ +
    97 protected:
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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);
    │ │ │ │ + │ │ │ │ +
    104
    │ │ │ │ +
    107
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    159
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    204
    │ │ │ │ -
    205 private:
    │ │ │ │ -
    206
    │ │ │ │ +
    207 template<class CONTAINER>
    │ │ │ │ +
    208 Key findParentClique(const CONTAINER& parents) const;
    │ │ │ │
    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)); }
    │ │ │ │ +
    211 void clear();
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    242
    │ │ │ │ +
    243 protected:
    │ │ │ │ +
    244
    │ │ │ │ +
    246 void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
    │ │ │ │ +
    247 int parentnum = 0) const;
    │ │ │ │ +
    248
    │ │ │ │ + │ │ │ │ +
    251
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition ConcurrentMap.h:68
    │ │ │ │ +
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
    Check invariants of this conditional, given the values x.
    Definition Conditional-inst.h:77
    │ │ │ │ -
    size_t nrFrontals_
    The first nrFrontal variables are frontal and the rest are parents.
    Definition Conditional.h:67
    │ │ │ │ -
    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
    │ │ │ │ -
    Key firstFrontalKey() const
    Convenience function to get the first frontal key.
    Definition Conditional.h:116
    │ │ │ │ -
    FACTOR::const_iterator endFrontals() const
    Iterator pointing past the last frontal key.
    Definition Conditional.h:163
    │ │ │ │ -
    FACTOR::iterator endParents()
    Mutable iterator pointing past the last parent key.
    Definition Conditional.h:184
    │ │ │ │ -
    FACTOR::iterator endFrontals()
    Mutable iterator pointing past the last frontal key.
    Definition Conditional.h:178
    │ │ │ │ -
    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
    │ │ │ │ -
    Parents parents() const
    return a view of the parent keys
    Definition Conditional.h:127
    │ │ │ │ -
    double operator()(const HybridValues &x) const
    Evaluate probability density, sugar.
    Definition Conditional.h:142
    │ │ │ │ -
    boost::iterator_range< typename FACTOR::const_iterator > Parents
    View of the separator keys (call parents())
    Definition Conditional.h:78
    │ │ │ │ -
    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
    │ │ │ │ -
    size_t nrFrontals() const
    return the number of frontals
    Definition Conditional.h:110
    │ │ │ │ -
    Conditional()
    Empty Constructor to make serialization possible.
    Definition Conditional.h:85
    │ │ │ │ -
    Conditional(size_t nrFrontals)
    Constructor.
    Definition Conditional.h:88
    │ │ │ │ -
    FACTOR::iterator beginParents()
    Mutable iterator pointing to the first parent key.
    Definition Conditional.h:181
    │ │ │ │ -
    size_t & nrFrontals()
    Mutable version of nrFrontals.
    Definition Conditional.h:172
    │ │ │ │ -
    FACTOR::iterator beginFrontals()
    Mutable iterator pointing to first frontal key.
    Definition Conditional.h:175
    │ │ │ │ -
    boost::iterator_range< typename FACTOR::const_iterator > Frontals
    View of the frontal keys (call frontals())
    Definition Conditional.h:75
    │ │ │ │ -
    Frontals frontals() const
    return a view of the frontal keys
    Definition Conditional.h:124
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition Conditional.h:217
    │ │ │ │ -
    FACTOR::const_iterator beginFrontals() const
    Iterator pointing to first frontal key.
    Definition Conditional.h:160
    │ │ │ │ -
    double normalizationConstant() const
    Non-virtual, exponentiate logNormalizationConstant.
    Definition Conditional-inst.h:70
    │ │ │ │ -
    size_t nrParents() const
    return the number of parents
    Definition Conditional.h:113
    │ │ │ │ -
    void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print with optional formatter
    Definition Conditional-inst.h:30
    │ │ │ │ -
    FACTOR::const_iterator endParents() const
    Iterator pointing past the last parent key.
    Definition Conditional.h:169
    │ │ │ │ -
    FACTOR::const_iterator beginParents() const
    Iterator pointing to the first parent key.
    Definition Conditional.h:166
    │ │ │ │ -
    the error.
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ +
    clique statistics
    Definition BayesTree.h:39
    │ │ │ │ +
    store all the sizes
    Definition BayesTree.h:48
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    Nodes nodes_
    Map from indices to Clique.
    Definition BayesTree.h:100
    │ │ │ │ +
    void removeClique(sharedClique clique)
    remove a clique: warning, can result in a forest
    Definition BayesTree-inst.h:424
    │ │ │ │ +
    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
    │ │ │ │ +
    void fillNodesIndex(const sharedClique &subtree)
    Fill the nodes index for a subtree.
    Definition BayesTree-inst.h:246
    │ │ │ │ +
    void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output to graphviz format, stream version.
    Definition BayesTree-inst.h:64
    │ │ │ │ +
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ +
    sharedClique operator[](Key j) const
    Access node by variable.
    Definition BayesTree.h:146
    │ │ │ │ +
    This & operator=(const This &other)
    Assignment operator.
    Definition BayesTree-inst.h:199
    │ │ │ │ +
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ +
    BayesTree()
    Create an empty Bayes Tree.
    Definition BayesTree.h:109
    │ │ │ │ +
    Clique Node
    Synonym for Clique (TODO: remove)
    Definition BayesTree.h:75
    │ │ │ │ +
    void clear()
    Remove all nodes.
    Definition BayesTree-inst.h:408
    │ │ │ │ +
    Roots roots_
    Root cliques.
    Definition BayesTree.h:103
    │ │ │ │ +
    void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
    add a clique (top down)
    Definition BayesTree-inst.h:142
    │ │ │ │ +
    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
    │ │ │ │ +
    sharedClique sharedNode
    Synonym for sharedClique (TODO: remove)
    Definition BayesTree.h:76
    │ │ │ │ +
    Key findParentClique(const CONTAINER &parents) const
    Find parent clique of a conditional.
    Definition BayesTree-inst.h:238
    │ │ │ │ +
    size_t size() const
    number of cliques
    Definition BayesTree-inst.h:133
    │ │ │ │ +
    void deleteCachedShortcuts()
    Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
    Definition BayesTree-inst.h:416
    │ │ │ │ +
    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
    │ │ │ │ +
    const Nodes & nodes() const
    Return nodes.
    Definition BayesTree.h:143
    │ │ │ │ +
    FastList< sharedClique > Cliques
    A convenience class for a list of shared cliques.
    Definition BayesTree.h:89
    │ │ │ │ +
    CLIQUE Clique
    The clique type, normally BayesTreeClique.
    Definition BayesTree.h:73
    │ │ │ │ +
    sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
    Return marginal on any variable.
    Definition BayesTree-inst.h:270
    │ │ │ │ +
    const Roots & roots() const
    return root cliques
    Definition BayesTree.h:149
    │ │ │ │ +
    const sharedClique & clique(Key j) const
    alternate syntax for matlab: find the clique that contains the variable with Key j
    Definition BayesTree.h:152
    │ │ │ │ +
    ConcurrentMap< Key, sharedClique > Nodes
    Map from keys to Clique.
    Definition BayesTree.h:92
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BayesTree.h:263
    │ │ │ │ +
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTree-inst.h:55
    │ │ │ │ +
    BayesTreeCliqueData getCliqueData() const
    Gather data on all cliques.
    Definition BayesTree-inst.h:35
    │ │ │ │ +
    Cliques removeSubtree(const sharedClique &subtree)
    Remove the requested subtree.
    Definition BayesTree-inst.h:496
    │ │ │ │ +
    bool empty() const
    Check if there are any cliques in the tree.
    Definition BayesTree.h:138
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ +
    void insertRoot(const sharedClique &subtree)
    Insert a new subtree with known parent clique.
    Definition BayesTree-inst.h:260
    │ │ │ │ +
    void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    output to file with graphviz format.
    Definition BayesTree-inst.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    FastVector< sharedClique > Roots
    Root cliques.
    Definition BayesTree.h:95
    │ │ │ │ +
    Definition BayesTree.h:276
    │ │ │ │ +
    BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
    Construct a new Bayes Tree Orphan Wrapper object.
    Definition BayesTree.h:293
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Conditional.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 │ │ │ │ │ @@ -15,271 +15,437 @@ │ │ │ │ │ 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 { │ │ │ │ │ -26 │ │ │ │ │ -27 class _H_y_b_r_i_d_V_a_l_u_e_s; // forward declaration. │ │ │ │ │ +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> │ │ │ │ │ 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 │ │ │ │ │ -_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; │ │ │ │ │ +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 }; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -100 bool _e_q_u_a_l_s(const This& c, double tol = 1e-9) const; │ │ │ │ │ +_1_0_0 _N_o_d_e_s _n_o_d_e_s__; │ │ │ │ │ 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 │ │ │ │ │ -_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 │ │ │ │ │ -_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); │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ -204 │ │ │ │ │ -205 private: │ │ │ │ │ -206 │ │ │ │ │ +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 │ │ │ │ │ -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)); } │ │ │ │ │ +211 void _c_l_e_a_r(); │ │ │ │ │ 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)); } │ │ │ │ │ +214 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s(); │ │ │ │ │ 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 │ │ │ │ │ +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_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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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 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. │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ - * _C_o_n_d_i_t_i_o_n_a_l_._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/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/VariableIndex.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    VariableSlots.h File Reference
    │ │ │ │ +
    VariableIndex.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...
    class  gtsam::VariableIndex
     The VariableIndex class computes and stores the block column structure of a factor graph. More...
     
    struct  gtsam::traits< VariableSlots >
     traits More...
    struct  gtsam::traits< VariableIndex >
     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
    Richard Roberts
    │ │ │ │ +
    Date
    March 26, 2013
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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_S_l_o_t_s │ │ │ │ │ -  A combined factor is assembled as one block of rows for each component │ │ │ │ │ - factor. _M_o_r_e_._._. │ │ │ │ │ + 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_S_l_o_t_s_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -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 │ │ │ │ │ + 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_._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/a00590.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00590 = [ │ │ │ │ │ - ["gtsam::VariableSlots", "a03708.html", "a03708"], │ │ │ │ │ - ["gtsam::traits< VariableSlots >", "a03712.html", null] │ │ │ │ │ + ["gtsam::VariableIndex", "a03700.html", "a03700"], │ │ │ │ │ + ["gtsam::traits< VariableIndex >", "a03704.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/VariableIndex.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,126 +98,199 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    VariableSlots.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 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/FastMap.h>
    │ │ │ │ - │ │ │ │ -
    24#include <gtsam/base/timing.h>
    │ │ │ │ -
    25#include <gtsam/base/Testable.h>
    │ │ │ │ -
    26
    │ │ │ │ -
    27#include <boost/tuple/tuple.hpp>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    21#include <gtsam/inference/Key.h>
    │ │ │ │ +
    22#include <gtsam/base/FastMap.h>
    │ │ │ │ + │ │ │ │ +
    24#include <gtsam/dllexport.h>
    │ │ │ │ +
    25
    │ │ │ │ +
    26#include <boost/optional/optional.hpp>
    │ │ │ │ +
    27#include <boost/smart_ptr/shared_ptr.hpp>
    │ │ │ │
    28
    │ │ │ │ -
    29#include <iostream>
    │ │ │ │ -
    30#include <string>
    │ │ │ │ +
    29#include <cassert>
    │ │ │ │ +
    30#include <stdexcept>
    │ │ │ │
    31
    │ │ │ │
    32namespace gtsam {
    │ │ │ │
    33
    │ │ │ │ -
    │ │ │ │ -
    52class VariableSlots : public FastMap<Key, FastVector<size_t> > {
    │ │ │ │ -
    53
    │ │ │ │ -
    54public:
    │ │ │ │ -
    55
    │ │ │ │ - │ │ │ │ -
    57 GTSAM_EXPORT static const size_t Empty;
    │ │ │ │ -
    58
    │ │ │ │ -
    61
    │ │ │ │ -
    67 template<class FG>
    │ │ │ │ -
    68 VariableSlots(const FG& factorGraph);
    │ │ │ │ -
    69
    │ │ │ │ -
    71
    │ │ │ │ +
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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>
    │ │ │ │ +
    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
    │ │ │ │
    │ │ │ │ -
    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}
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ + │ │ │ │ +
    The base class for all factors.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    std::uint64_t FactorIndex
    Integer nonlinear factor index type.
    Definition types.h:103
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A combined factor is assembled as one block of rows for each component factor.
    Definition VariableSlots.h:52
    │ │ │ │ -
    GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
    print
    Definition VariableSlots.cpp:29
    │ │ │ │ -
    VariableSlots(const FG &factorGraph)
    Constructor from a set of factors to be combined.
    Definition VariableSlots.h:89
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ +
    const FactorIndices & internalAt(Key variable) const
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:172
    │ │ │ │ +
    size_t nEntries() const
    The number of nonzero blocks, i.e. the number of variable-factor entries.
    Definition VariableIndex.h:86
    │ │ │ │ +
    const_iterator begin() const
    Iterator to the first variable entry.
    Definition VariableIndex.h:156
    │ │ │ │ +
    const_iterator find(Key key) const
    Find the iterator for the requested variable entry.
    Definition VariableIndex.h:162
    │ │ │ │ +
    const FactorIndices & operator[](Key variable) const
    Access a list of factors by variable.
    Definition VariableIndex.h:89
    │ │ │ │ +
    VariableIndex(const FG &factorGraph)
    Create a VariableIndex that computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:71
    │ │ │ │ +
    bool empty(Key variable) const
    Return true if no factors associated with a variable.
    Definition VariableIndex.h:98
    │ │ │ │ +
    size_t size() const
    The number of variable entries. This is equal to the number of unique variable Keys.
    Definition VariableIndex.h:80
    │ │ │ │ +
    FactorIndices & internalAt(Key variable)
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:179
    │ │ │ │ +
    const_iterator end() const
    Iterator to the first variable entry.
    Definition VariableIndex.h:159
    │ │ │ │ +
    VariableIndex()
    Default constructor, creates an empty VariableIndex.
    Definition VariableIndex.h:64
    │ │ │ │ +
    size_t nFactors() const
    The number of factors in the original factor graph.
    Definition VariableIndex.h:83
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,149 +1,255 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VariableSlots.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 │ │ │ │ │ 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> │ │ │ │ │ +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 <_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 │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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} │ │ │ │ │ +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. │ │ │ │ │ -_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. │ │ │ │ │ _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 │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ +_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_:_:_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_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_:_:_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 │ │ │ │ │ - * _V_a_r_i_a_b_l_e_S_l_o_t_s_._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/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/inference-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,87 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    FactorGraph-inst.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    inference-inst.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor Graph Base Class. │ │ │ │ +

    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.

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

    │ │ │ │ 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

    │ │ │ │ -

    Factor Graph Base Class.

    │ │ │ │ -
    Author
    Carlos Nieto
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Alireza Fathi
    │ │ │ │ -
    │ │ │ │ -Michael Kaess
    │ │ │ │ +

    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.

    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,45 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_. │ │ │ │ │ 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 ********** │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ +Contains ggeenneerriicc inference algorithms that convert between templated graphical │ │ │ │ │ +models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ │ Author │ │ │ │ │ - Carlos Nieto │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Michael Kaess │ │ │ │ │ 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 │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._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/a00593_source.html │ │ │ │ @@ -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/inference/inference-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,243 +98,109 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    FactorGraph-inst.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    77 size_t size_ = 0;
    │ │ │ │ -
    78 for (const sharedFactor& factor : factors_)
    │ │ │ │ -
    79 if (factor) size_++;
    │ │ │ │ -
    80 return size_;
    │ │ │ │ -
    81}
    │ │ │ │ -
    │ │ │ │ -
    82
    │ │ │ │ -
    83/* ************************************************************************* */
    │ │ │ │ -
    84template <class FACTOR>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    86 KeySet keys;
    │ │ │ │ -
    87 for (const sharedFactor& factor : this->factors_) {
    │ │ │ │ -
    88 if (factor) keys.insert(factor->begin(), factor->end());
    │ │ │ │ -
    89 }
    │ │ │ │ -
    90 return keys;
    │ │ │ │ -
    91}
    │ │ │ │ -
    │ │ │ │ -
    92
    │ │ │ │ -
    93/* ************************************************************************* */
    │ │ │ │ -
    94template <class FACTOR>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    23#include <utility>
    │ │ │ │ +
    24
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    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}
    │ │ │ │ + │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    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
    │ │ │ │ -
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    KeyVector keyVector() const
    Potentially slow function to return all keys involved, sorted, as a vector.
    Definition FactorGraph-inst.h:95
    │ │ │ │ -
    double error(const HybridValues &values) const
    Add error for all factors.
    Definition FactorGraph-inst.h:66
    │ │ │ │ -
    size_t nrFactors() const
    return the number of non-null factors
    Definition FactorGraph-inst.h:76
    │ │ │ │ -
    size_t size() const
    return the number of factors (including any null factors set by remove() ).
    Definition FactorGraph.h:326
    │ │ │ │ -
    bool equals(const This &fg, double tol=1e-9) const
    Check equality up to tolerance.
    Definition FactorGraph-inst.h:50
    │ │ │ │ -
    boost::shared_ptr< FACTOR > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ -
    FastVector< sharedFactor > factors_
    concept check, makes sure FACTOR defines print and equals
    Definition FactorGraph.h:131
    │ │ │ │ -
    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
    │ │ │ │ -
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< Vector2 > variablePos(Key key) const
    Return variable position or none.
    Definition DotWriter.cpp:79
    │ │ │ │ -
    void graphPreamble(std::ostream *os) const
    Write out preamble for graph, including size.
    Definition DotWriter.cpp:30
    │ │ │ │ +
    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,305 +1,127 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FactorGraph-inst.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 │ │ │ │ │ -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()); │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 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. │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._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/a00596_source.html │ │ │ │ @@ -114,17 +114,17 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/Vector.h>
    │ │ │ │ -
    23#include <gtsam/inference/Key.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
    │ │ │ │ @@ -171,17 +171,17 @@ │ │ │ │
    95 const KeyFormatter& keyFormatter,
    │ │ │ │
    96 const boost::optional<Vector2>& position,
    │ │ │ │
    97 std::ostream* os) const;
    │ │ │ │
    98};
    │ │ │ │ │ │ │ │
    99
    │ │ │ │
    100} // namespace gtsam
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ - │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │
    double figureWidthInches
    The figure width on paper in inches.
    Definition DotWriter.h:36
    │ │ │ │
    bool plotFactorPoints
    Plots each factor as a dot between the variables.
    Definition DotWriter.h:38
    │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -71,18 +71,18 @@ │ │ │ │ │ 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. │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ _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. │ │ │ ├── ./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/VariableIndex-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    MetisIndex.h File Reference
    │ │ │ │ +
    VariableIndex-inl.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
    Andrew Melim
    │ │ │ │ -
    Date
    Oct. 10, 2014
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    March 26, 2013
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -MetisIndex.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_:_:_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 │ │ │ │ │ - Andrew Melim │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct. 10, 2014 │ │ │ │ │ + March 26, 2013 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/VariableIndex-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,125 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    MetisIndex.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
    │ │ │ │
    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
    │ │ │ │ +
    9
    │ │ │ │ +
    10 * -------------------------------------------------------------------------- */
    │ │ │ │ +
    11
    │ │ │ │ +
    18#pragma once
    │ │ │ │
    19
    │ │ │ │ -
    20#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/types.h>
    │ │ │ │ -
    23#include <gtsam/base/timing.h>
    │ │ │ │ + │ │ │ │ +
    21#include <gtsam/base/timing.h>
    │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ -
    89 }
    │ │ │ │ -
    90 size_t nValues() const {
    │ │ │ │ -
    91 return nKeys_;
    │ │ │ │ +
    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 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
    │ │ │ │ - │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ - │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ - │ │ │ │ +
    93}
    │ │ │ │ +
    │ │ │ │ +
    94
    │ │ │ │ +
    95}
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
    Definition MetisIndex.h:45
    │ │ │ │ -
    MetisIndex()
    Default constructor, creates empty MetisIndex.
    Definition MetisIndex.h:61
    │ │ │ │ +
    const FactorIndices & internalAt(Key variable) const
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:172
    │ │ │ │ +
    void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
    Remove entries corresponding to the specified factors.
    Definition VariableIndex-inl.h:54
    │ │ │ │ +
    void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
    Remove unused empty variables (in debug mode verifies they are empty).
    Definition VariableIndex-inl.h:85
    │ │ │ │ +
    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,117 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MetisIndex.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 │ │ │ │ │ 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 │ │ │ │ │ +11 │ │ │ │ │ +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_/_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> │ │ │ │ │ +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// 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 │ │ │ │ │ -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_; │ │ │ │ │ -89 } │ │ │ │ │ -90 size_t nValues() const { │ │ │ │ │ -91 return nKeys_; │ │ │ │ │ +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 _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> │ │ │ │ │ +93} │ │ │ │ │ +94 │ │ │ │ │ +95} │ │ │ │ │ _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 │ │ │ │ │ +_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_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_:_:_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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _M_e_t_i_s_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/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/EliminationTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,483 +98,363 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EliminateableFactorGraph-inst.h
    │ │ │ │ +
    EliminationTree-inst.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)
    │ │ │ │ +
    3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4* Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5* 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <boost/tuple/tuple.hpp>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    27 /* ************************************************************************* */
    │ │ │ │ -
    28 template<class FACTORGRAPH>
    │ │ │ │ -
    29 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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())
    │ │ │ │ - │ │ │ │ -
    82 // Return the Bayes net
    │ │ │ │ -
    83 return bayesNet;
    │ │ │ │ -
    84 }
    │ │ │ │ -
    85 }
    │ │ │ │ -
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <boost/make_shared.hpp>
    │ │ │ │ +
    21#include <stack>
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/base/timing.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    32 /* ************************************************************************* */
    │ │ │ │ +
    33 template<class BAYESNET, class GRAPH>
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 /* ************************************************************************* */
    │ │ │ │ -
    88 template <class FACTORGRAPH>
    │ │ │ │ -
    89 boost::shared_ptr<
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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())
    │ │ │ │ - │ │ │ │ -
    146 // Return the Bayes tree
    │ │ │ │ -
    147 return bayesTree;
    │ │ │ │ -
    148 }
    │ │ │ │ -
    149 }
    │ │ │ │ +
    87 static const size_t none = std::numeric_limits<size_t>::max();
    │ │ │ │ +
    88
    │ │ │ │ +
    89 // Allocate result parent vector and vector of last factor columns
    │ │ │ │ + │ │ │ │ +
    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 /* ************************************************************************* */
    │ │ │ │ -
    152 template<class FACTORGRAPH>
    │ │ │ │ -
    153 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    155 const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
    │ │ │ │ -
    156 {
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    157 /* ************************************************************************* */
    │ │ │ │ +
    158 template<class BAYESNET, class GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 FACTORGRAPH>
    │ │ │ │ -
    │ │ │ │ -
    171 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
    │ │ │ │ +
    170 template<class BAYESNET, class GRAPH>
    │ │ │ │ + │ │ │ │
    │ │ │ │ - │ │ │ │ -
    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> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ +
    173 {
    │ │ │ │ +
    174 // Start by duplicating the tree.
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    284 roots_.swap(other.roots_);
    │ │ │ │ +
    285 remainingFactors_.swap(other.remainingFactors_);
    │ │ │ │ +
    286 }
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    287
    │ │ │ │ +
    288
    │ │ │ │ +
    289}
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ -
    Variable elimination algorithms for factor graphs.
    │ │ │ │ -
    Exceptions that may be thrown by inference algorithms.
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Contains generic inference algorithms that convert between templated graphical models,...
    │ │ │ │ + │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::JunctionTreeType JunctionTreeType
    Junction tree type that can do multifrontal elimination of this graph.
    Definition EliminateableFactorGraph.h:82
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::BayesTreeType BayesTreeType
    Bayes tree type produced by multifrontal elimination.
    Definition EliminateableFactorGraph.h:79
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::EliminationTreeType EliminationTreeType
    Elimination tree type that can do sequential elimination of this graph.
    Definition EliminateableFactorGraph.h:76
    │ │ │ │ -
    boost::optional< const VariableIndex & > OptionalVariableIndex
    Typedef for an optional variable index as an argument to elimination functions.
    Definition EliminateableFactorGraph.h:92
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< Ordering::OrderingType > OptionalOrderingType
    Typedef for an optional ordering type.
    Definition EliminateableFactorGraph.h:95
    │ │ │ │ -
    An inference algorithm was called with inconsistent arguments.
    Definition inferenceExceptions.h:29
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    Test whether the tree is equal to another.
    Definition EliminationTree-inst.h:214
    │ │ │ │ +
    void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    Print the tree to cout.
    Definition EliminationTree-inst.h:207
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FastVector< sharedNode > roots_
    concept check
    Definition EliminationTree.h:86
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition EliminationTree.h:60
    │ │ │ │ +
    GRAPH FactorGraphType
    The factor graph type.
    Definition EliminationTree.h:58
    │ │ │ │ +
    void swap(This &other)
    Swap the data of this tree with another one, this operation is very fast.
    Definition EliminationTree-inst.h:283
    │ │ │ │ +
    EliminationTree()
    Protected default constructor.
    Definition EliminationTree.h:161
    │ │ │ │ +
    boost::shared_ptr< Node > sharedNode
    Shared pointer to Node.
    Definition EliminationTree.h:80
    │ │ │ │ +
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition EliminationTree.h:154
    │ │ │ │ +
    Key key
    key associated with root
    Definition EliminationTree.h:70
    │ │ │ │ +
    Children children
    sub-trees
    Definition EliminationTree.h:72
    │ │ │ │ +
    Factors factors
    factors associated with root
    Definition EliminationTree.h:71
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    static Ordering Natural(const FACTOR_GRAPH &fg)
    Return a natural Ordering. Typically used by iterative solvers.
    Definition Ordering.h:190
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
    Compute an ordering determined by METIS from a VariableIndex.
    Definition Ordering.cpp:212
    │ │ │ │
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,609 +1,449 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminateableFactorGraph-inst.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_/_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 /* ************************************************************************* │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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 } │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ +59 │ │ │ │ │ +60 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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 /* ************************************************************************* │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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 } │ │ │ │ │ +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 / │ │ │ │ │ +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 / │ │ │ │ │ * ************************************************************************* */ │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -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 / │ │ │ │ │ +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 / │ │ │ │ │ * ************************************************************************* */ │ │ │ │ │ -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 / │ │ │ │ │ +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 / │ │ │ │ │ * ************************************************************************* */ │ │ │ │ │ -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 / │ │ │ │ │ +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 / │ │ │ │ │ * ************************************************************************* */ │ │ │ │ │ -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 / │ │ │ │ │ +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 / │ │ │ │ │ * ************************************************************************* */ │ │ │ │ │ -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. │ │ │ │ │ +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_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +_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 │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 > > 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 │ │ │ │ │ +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_:_:_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 │ │ │ │ │ - * EElliimmiinnaatteeaabblleeFFaaccttoorrGGrraapphh--iinnsstt..hh │ │ │ │ │ + * _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/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/EliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,338 +98,160 @@ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ -
    graph-inl.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)
    │ │ │ │ +
    3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4* Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5* 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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    62 boost::breadth_first_search(g, root, boost::visitor(vis));
    │ │ │ │ -
    63 return keys;
    │ │ │ │ -
    64}
    │ │ │ │ -
    │ │ │ │ +
    20#include <utility>
    │ │ │ │ +
    21#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/base/Testable.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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    52 {
    │ │ │ │ +
    53 protected:
    │ │ │ │ + │ │ │ │ +
    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/* ************************************************************************* */
    │ │ │ │ -
    67template<class G, class F, class KEY>
    │ │ │ │ -
    │ │ │ │ -
    68SDGraph<KEY> toBoostGraph(const G& graph) {
    │ │ │ │ -
    69 // convert the factor graph to boost graph
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ +
    │ │ │ │ +
    66 struct Node {
    │ │ │ │ +
    67 typedef FastVector<sharedFactor> Factors;
    │ │ │ │ +
    68 typedef FastVector<boost::shared_ptr<Node> > Children;
    │ │ │ │ +
    69
    │ │ │ │ + │ │ │ │ +
    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 };
    │ │ │ │
    │ │ │ │ -
    114
    │ │ │ │ -
    115/* ************************************************************************* */
    │ │ │ │ -
    116template<class G, class V, class KEY>
    │ │ │ │ -
    117boost::tuple<G, V, std::map<KEY,V> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ +
    79
    │ │ │ │ +
    80 typedef boost::shared_ptr<Node> sharedNode;
    │ │ │ │ +
    81
    │ │ │ │ +
    82 protected:
    │ │ │ │ +
    84 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
    │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    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 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 }
    │ │ │ │ +
    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 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 {
    │ │ │ │ +
    146 public:
    │ │ │ │ +
    149
    │ │ │ │ +
    151 const FastVector<sharedNode>& roots() const { return roots_; }
    │ │ │ │ +
    152
    │ │ │ │ +
    154 const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
    │ │ │ │
    155
    │ │ │ │ -
    156private:
    │ │ │ │ -
    157 boost::shared_ptr<Values> config_;
    │ │ │ │ +
    157 void swap(This& other);
    │ │ │ │
    158
    │ │ │ │ -
    159public:
    │ │ │ │ -
    160
    │ │ │ │ -
    161 compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
    │ │ │ │ +
    159 protected:
    │ │ │ │ + │ │ │ │
    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);
    │ │ │ │ - │ │ │ │ -
    225 boost::breadth_first_search(g, root, boost::visitor(vis));
    │ │ │ │ -
    226
    │ │ │ │ -
    227 return config;
    │ │ │ │ -
    228}
    │ │ │ │ -
    │ │ │ │ -
    229
    │ │ │ │ -
    230/* ************************************************************************* */
    │ │ │ │ -
    231template<class G, class KEY, class FACTOR2>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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 algorithm using boost library.
    │ │ │ │ +
    163 private:
    │ │ │ │ +
    165 friend class ::EliminationTreeTester;
    │ │ │ │ +
    166 };
    │ │ │ │ +
    │ │ │ │ +
    167
    │ │ │ │ +
    168}
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
    find the minimum spanning tree using boost graph library
    Definition graph-inl.h:232
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition graph-inl.h:38
    │ │ │ │ -
    Definition graph-inl.h:154
    │ │ │ │ -
    SDGraph is undirected graph with variable keys and double edge weights.
    Definition graph.h:40
    │ │ │ │ -
    Definition graph.h:47
    │ │ │ │ -
    Map from variable key to parent key.
    Definition graph.h:58
    │ │ │ │ -
    void insert(const KEY &key, const KEY &parent)
    convenience insert so we can pass ints for TypedSymbol keys
    Definition graph.h:61
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    Print the tree to cout.
    Definition EliminationTree-inst.h:207
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTree< BAYESNET, GRAPH > This
    This class.
    Definition EliminationTree.h:54
    │ │ │ │ +
    FastVector< sharedNode > roots_
    concept check
    Definition EliminationTree.h:86
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition EliminationTree.h:60
    │ │ │ │ +
    BAYESNET BayesNetType
    The BayesNet corresponding to FACTOR.
    Definition EliminationTree.h:61
    │ │ │ │ +
    boost::shared_ptr< ConditionalType > sharedConditional
    Shared pointer to a conditional.
    Definition EliminationTree.h:63
    │ │ │ │ +
    const FastVector< sharedNode > & roots() const
    Return the set of roots (one for a tree, multiple for a forest)
    Definition EliminationTree.h:151
    │ │ │ │ +
    GRAPH FactorGraphType
    The factor graph type.
    Definition EliminationTree.h:58
    │ │ │ │ +
    void swap(This &other)
    Swap the data of this tree with another one, this operation is very fast.
    Definition EliminationTree-inst.h:283
    │ │ │ │ +
    EliminationTree()
    Protected default constructor.
    Definition EliminationTree.h:161
    │ │ │ │ +
    BayesNetType::ConditionalType ConditionalType
    The type of conditionals.
    Definition EliminationTree.h:62
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition EliminationTree.h:55
    │ │ │ │ +
    GRAPH::FactorType FactorType
    The type of factors.
    Definition EliminationTree.h:59
    │ │ │ │ +
    boost::shared_ptr< Node > sharedNode
    Shared pointer to Node.
    Definition EliminationTree.h:80
    │ │ │ │ +
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition EliminationTree.h:154
    │ │ │ │ +
    Definition EliminationTree.h:66
    │ │ │ │ +
    Key key
    key associated with root
    Definition EliminationTree.h:70
    │ │ │ │ +
    Children children
    sub-trees
    Definition EliminationTree.h:72
    │ │ │ │ +
    Factors factors
    factors associated with root
    Definition EliminationTree.h:71
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,377 +1,237 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -graph-inl.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) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* 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 │ │ │ │ │ -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} │ │ │ │ │ +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 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 │ │ │ │ │ -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; │ │ │ │ │ +_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 // 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) { │ │ │ │ │ +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 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 } │ │ │ │ │ +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 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 { │ │ │ │ │ +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 │ │ │ │ │ -156private: │ │ │ │ │ -157 boost::shared_ptr config_; │ │ │ │ │ +157 void _s_w_a_p(_T_h_i_s& other); │ │ │ │ │ 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;} │ │ │ │ │ +159 protected: │ │ │ │ │ +_1_6_1 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e() {} │ │ │ │ │ 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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * ggrraapphh--iinnll..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/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/VariableIndex.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,59 +94,32 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    FactorGraph.h File Reference
    │ │ │ │ +
    VariableIndex.cpp 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

    │ │ │ │ -

    Factor Graph Base Class.

    │ │ │ │ -
    Author
    Carlos Nieto
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Michael Kaess
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    March 26, 2013
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +VariableIndex.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 │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + March 26, 2013 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00614.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    MetisIndex-inl.h File Reference
    │ │ │ │ +
    JunctionTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    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

    │ │ │ │ -
    Author
    Andrew Melim
    │ │ │ │ -
    Date
    Oct. 10, 2014
    │ │ │ │ +

    The junction tree.

    │ │ │ │ +
    Date
    Feb 4, 2010
    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +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 │ │ │ │ │ -MetisIndex-inl.h File Reference │ │ │ │ │ +_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 ********** │ │ │ │ │ - Author │ │ │ │ │ - Andrew Melim │ │ │ │ │ +The junction tree. │ │ │ │ │ Date │ │ │ │ │ - Oct. 10, 2014 │ │ │ │ │ + Feb 4, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Kai Ni │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + 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_-_i_n_l_._h │ │ │ │ │ + * _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/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/JunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,104 +98,79 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    MetisIndex-inl.h
    │ │ │ │ +
    JunctionTree.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 <map>
    │ │ │ │ -
    21#include <vector>
    │ │ │ │ +
    21#pragma once
    │ │ │ │
    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;
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 // 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();
    │ │ │ │ + │ │ │ │ +
    55 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ +
    72
    │ │ │ │ +
    73 protected:
    │ │ │ │ +
    74
    │ │ │ │ +
    75 // Private default constructor (used in static construction methods)
    │ │ │ │ +
    76 JunctionTree() {}
    │ │ │ │ +
    77
    │ │ │ │ +
    78 };
    │ │ │ │ +
    │ │ │ │ +
    79
    │ │ │ │ +
    80}
    │ │ │ │ +
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    void augment(const FACTORGRAPH &factors)
    Augment the variable index with new factors.
    Definition MetisIndex-inl.h:27
    │ │ │ │ +
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ +
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ +
    static This FromEliminationTree(const ETREE &eliminationTree)
    Build the junction tree from an elimination tree.
    Definition JunctionTree.h:65
    │ │ │ │ +
    JunctionTree< BAYESTREE, GRAPH > This
    This class.
    Definition JunctionTree.h:54
    │ │ │ │ +
    EliminatableClusterTree< BAYESTREE, GRAPH > Base
    Our base class.
    Definition JunctionTree.h:56
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition JunctionTree.h:55
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,105 +1,94 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MetisIndex-inl.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ +21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -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(); │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ + * _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/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/VariableSlots.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    JunctionTree-inst.h File Reference
    │ │ │ │ +
    VariableSlots.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
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Oct 5, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +VariableSlots.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 │ │ │ │ │ + Date │ │ │ │ │ + Oct 5, 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 │ │ │ │ │ + * _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/a00623_source.html │ │ │ │ @@ -113,17 +113,17 @@ │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    21#include <gtsam/base/timing.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    21#include <gtsam/base/timing.h>
    │ │ │ │
    22
    │ │ │ │
    23namespace gtsam {
    │ │ │ │
    24
    │ │ │ │
    25 /* ************************************************************************* */
    │ │ │ │
    26 template<class DERIVED, class FACTORGRAPH>
    │ │ │ │
    │ │ │ │ │ │ │ │ @@ -364,17 +364,17 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Base class for cliques of a BayesTree.
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    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
    │ │ │ │ │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
    Definition DiscreteFactorGraph.h:101
    │ │ │ │
    size_t treeSize() const
    The size of subtree rooted at this clique, i.e., nr of Cliques.
    Definition BayesTreeCliqueBase-inst.h:84
    │ │ │ ├── ./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/BayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,87 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    inference-inst.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    BayesTree.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Contains generic inference algorithms that convert between templated graphical models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ +

    Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ 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.

    │ │ │ │ +

    Bayes Tree is a tree of cliques of a Bayes Chain.

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ +Michael Kaess
    │ │ │ │ +
    │ │ │ │ +Viorela Ila
    │ │ │ │ +
    │ │ │ │ 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,45 +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 │ │ │ │ │ -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_. │ │ │ │ │ +_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_._._. │ │ │ │ │ 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. │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Michael Kaess │ │ │ │ │ + Viorela Ila │ │ │ │ │ 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 │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ + * _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/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/Ordering.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    VariableSlots.cpp File Reference
    │ │ │ │ +
    Ordering.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Variable ordering for the elimination algorithm. │ │ │ │ +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.
     
    │ │ │ │

    Detailed Description

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

    Variable ordering for the elimination algorithm.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Andrew Melim
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    Sep 2, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableSlots.cpp File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ +_G_o_ _t_o_ _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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Andrew Melim │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Oct 5, 2010 │ │ │ │ │ + Sep 2, 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_._c_p_p │ │ │ │ │ + * _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/a00632_source.html │ │ │ │ @@ -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/inference/EliminateableFactorGraph-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,79 +98,483 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JunctionTree.h
    │ │ │ │ +
    EliminateableFactorGraph-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)
    │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │ -
    21#pragma once
    │ │ │ │ -
    22
    │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <boost/tuple/tuple.hpp>
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    55 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    70
    │ │ │ │ -
    72
    │ │ │ │ -
    73 protected:
    │ │ │ │ -
    74
    │ │ │ │ -
    75 // Private default constructor (used in static construction methods)
    │ │ │ │ -
    76 JunctionTree() {}
    │ │ │ │ -
    77
    │ │ │ │ -
    78 };
    │ │ │ │ -
    │ │ │ │ -
    79
    │ │ │ │ -
    80}
    │ │ │ │ -
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ +
    27 /* ************************************************************************* */
    │ │ │ │ +
    28 template<class FACTORGRAPH>
    │ │ │ │ +
    29 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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())
    │ │ │ │ + │ │ │ │ +
    82 // Return the Bayes net
    │ │ │ │ +
    83 return bayesNet;
    │ │ │ │ +
    84 }
    │ │ │ │ +
    85 }
    │ │ │ │ +
    │ │ │ │ +
    86
    │ │ │ │ +
    87 /* ************************************************************************* */
    │ │ │ │ +
    88 template <class FACTORGRAPH>
    │ │ │ │ +
    89 boost::shared_ptr<
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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())
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    Variable elimination algorithms for factor graphs.
    │ │ │ │ +
    Exceptions that may be thrown by inference algorithms.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ -
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ -
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ -
    static This FromEliminationTree(const ETREE &eliminationTree)
    Build the junction tree from an elimination tree.
    Definition JunctionTree.h:65
    │ │ │ │ -
    JunctionTree< BAYESTREE, GRAPH > This
    This class.
    Definition JunctionTree.h:54
    │ │ │ │ -
    EliminatableClusterTree< BAYESTREE, GRAPH > Base
    Our base class.
    Definition JunctionTree.h:56
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition JunctionTree.h:55
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::JunctionTreeType JunctionTreeType
    Junction tree type that can do multifrontal elimination of this graph.
    Definition EliminateableFactorGraph.h:82
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::BayesTreeType BayesTreeType
    Bayes tree type produced by multifrontal elimination.
    Definition EliminateableFactorGraph.h:79
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::EliminationTreeType EliminationTreeType
    Elimination tree type that can do sequential elimination of this graph.
    Definition EliminateableFactorGraph.h:76
    │ │ │ │ +
    boost::optional< const VariableIndex & > OptionalVariableIndex
    Typedef for an optional variable index as an argument to elimination functions.
    Definition EliminateableFactorGraph.h:92
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::optional< Ordering::OrderingType > OptionalOrderingType
    Typedef for an optional ordering type.
    Definition EliminateableFactorGraph.h:95
    │ │ │ │ +
    An inference algorithm was called with inconsistent arguments.
    Definition inferenceExceptions.h:29
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    static Ordering Natural(const FACTOR_GRAPH &fg)
    Return a natural Ordering. Typically used by iterative solvers.
    Definition Ordering.h:190
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
    Compute an ordering determined by METIS from a VariableIndex.
    Definition Ordering.cpp:212
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,94 +1,609 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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_/_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 // 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; │ │ │ │ │ -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. │ │ │ │ │ +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 } │ │ │ │ │ +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 } │ │ │ │ │ +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); │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _J_u_n_c_t_i_o_n_T_r_e_e_._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/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/FactorGraph-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ClusterTree.h File Reference
    │ │ │ │ +
    FactorGraph-inst.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ +

    Factor Graph Base Class. │ │ │ │ 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...
     
    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

    │ │ │ │ -

    Collects factorgraph fragments defined on variable clusters, arranged in a tree.

    │ │ │ │ -
    Date
    Oct 8, 2013
    │ │ │ │ -
    Author
    Kai Ni
    │ │ │ │ +

    Factor Graph Base Class.

    │ │ │ │ +
    Author
    Carlos Nieto
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Alireza Fathi
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ +Michael Kaess │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +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 │ │ │ │ │ -ClusterTree.h File Reference │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. _M_o_r_e_._._. │ │ │ │ │ +_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_:_:_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 ********** │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ - Date │ │ │ │ │ - Oct 8, 2013 │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Carlos Nieto │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Michael Kaess │ │ │ │ │ + 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_._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/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/FactorGraph-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,273 +98,243 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ClusterTree.h
    │ │ │ │ +
    FactorGraph-inst.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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    15
    │ │ │ │ -
    16namespace gtsam {
    │ │ │ │ -
    17
    │ │ │ │ -
    24template <class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    26 public:
    │ │ │ │ -
    27 typedef GRAPH FactorGraphType;
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    38 Children children;
    │ │ │ │ -
    39
    │ │ │ │ -
    40 typedef Ordering Keys;
    │ │ │ │ - │ │ │ │ -
    42
    │ │ │ │ - │ │ │ │ -
    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 };
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    77 size_t size_ = 0;
    │ │ │ │ +
    78 for (const sharedFactor& factor : factors_)
    │ │ │ │ +
    79 if (factor) size_++;
    │ │ │ │ +
    80 return size_;
    │ │ │ │ +
    81}
    │ │ │ │ +
    │ │ │ │ +
    82
    │ │ │ │ +
    83/* ************************************************************************* */
    │ │ │ │ +
    84template <class FACTOR>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    86 KeySet keys;
    │ │ │ │ +
    87 for (const sharedFactor& factor : this->factors_) {
    │ │ │ │ +
    88 if (factor) keys.insert(factor->begin(), factor->end());
    │ │ │ │ +
    89 }
    │ │ │ │ +
    90 return keys;
    │ │ │ │ +
    91}
    │ │ │ │ +
    │ │ │ │ +
    92
    │ │ │ │ +
    93/* ************************************************************************* */
    │ │ │ │ +
    94template <class FACTOR>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 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:
    │ │ │ │ - │ │ │ │ -
    117
    │ │ │ │ -
    120
    │ │ │ │ -
    │ │ │ │ -
    123 ClusterTree(const This& other) {
    │ │ │ │ -
    124 *this = other;
    │ │ │ │ -
    125 }
    │ │ │ │ -
    │ │ │ │ +
    106/* ************************************************************************* */
    │ │ │ │ +
    107template <class FACTOR>
    │ │ │ │ +
    108template <typename CONTAINER, typename>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    128
    │ │ │ │ -
    129 public:
    │ │ │ │ -
    130
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    185 public:
    │ │ │ │ -
    186 typedef BAYESTREE BayesTreeType;
    │ │ │ │ -
    187 typedef GRAPH FactorGraphType;
    │ │ │ │ - │ │ │ │ -
    189 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    190
    │ │ │ │ -
    191 typedef typename BAYESTREE::ConditionalType ConditionalType;
    │ │ │ │ -
    192 typedef boost::shared_ptr<ConditionalType>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    232 return remainingFactors_;
    │ │ │ │ -
    233 }
    │ │ │ │ -
    │ │ │ │ -
    234
    │ │ │ │ -
    236
    │ │ │ │ -
    237 protected:
    │ │ │ │ -
    240
    │ │ │ │ -
    243 This& operator=(const This& other);
    │ │ │ │ -
    244
    │ │ │ │ - │ │ │ │ -
    247
    │ │ │ │ -
    249};
    │ │ │ │ -
    │ │ │ │ -
    250}
    │ │ │ │ -
    251
    │ │ │ │ - │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ -
    EliminatableClusterTree< BAYESTREE, GRAPH > This
    This class.
    Definition ClusterTree.h:188
    │ │ │ │ -
    EliminatableClusterTree()
    Default constructor to be used in derived classes.
    Definition ClusterTree.h:246
    │ │ │ │ -
    BAYESTREE::ConditionalType ConditionalType
    The type of conditionals.
    Definition ClusterTree.h:191
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< ConditionalType > sharedConditional
    Shared pointer to a conditional.
    Definition ClusterTree.h:193
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:197
    │ │ │ │ -
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition ClusterTree.h:231
    │ │ │ │ -
    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
    │ │ │ │ -
    BAYESTREE BayesTreeType
    The BayesTree type produced by elimination.
    Definition ClusterTree.h:186
    │ │ │ │ -
    GRAPH::FactorType FactorType
    The type of factors.
    Definition ClusterTree.h:196
    │ │ │ │ -
    GRAPH FactorGraphType
    The factor graph type.
    Definition ClusterTree.h:187
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition ClusterTree.h:189
    │ │ │ │ -
    GRAPH::Eliminate Eliminate
    Typedef for an eliminate subroutine.
    Definition ClusterTree.h:195
    │ │ │ │ -
    A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
    Definition ClusterTree.h:25
    │ │ │ │ -
    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
    │ │ │ │ -
    ClusterTree< GRAPH > This
    This class.
    Definition ClusterTree.h:28
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition ClusterTree.h:29
    │ │ │ │ -
    FastVector< sharedNode > roots_
    concept check
    Definition ClusterTree.h:116
    │ │ │ │ -
    GRAPH::FactorType FactorType
    The type of factors.
    Definition ClusterTree.h:31
    │ │ │ │ -
    GRAPH FactorGraphType
    The factor graph type.
    Definition ClusterTree.h:27
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Print the cluster tree.
    Definition ClusterTree-inst.h:98
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:32
    │ │ │ │ -
    const FastVector< sharedNode > & roots() const
    Return the set of roots (one for a tree, multiple for a forest)
    Definition ClusterTree.h:159
    │ │ │ │ -
    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
    │ │ │ │ -
    ClusterTree()
    Default constructor.
    Definition ClusterTree.h:132
    │ │ │ │ -
    boost::shared_ptr< Cluster > sharedCluster
    Shared pointer to Cluster.
    Definition ClusterTree.h:106
    │ │ │ │ -
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │ -
    Cluster(Key key, const CONTAINER &factorsToAdd)
    Construct from factors associated with a single key.
    Definition ClusterTree.h:57
    │ │ │ │ -
    Children children
    sub-trees
    Definition ClusterTree.h:38
    │ │ │ │ -
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition ClusterTree-inst.h:26
    │ │ │ │ -
    Keys orderedFrontalKeys
    Frontal keys of this node.
    Definition ClusterTree.h:41
    │ │ │ │ -
    void mergeChildren(const std::vector< bool > &merge)
    Merge all children for which bit is set into this node.
    Definition ClusterTree-inst.h:56
    │ │ │ │ -
    void merge(const boost::shared_ptr< Cluster > &cluster)
    Merge in given cluster.
    Definition ClusterTree-inst.h:44
    │ │ │ │ -
    std::vector< size_t > nrFrontalsOfChildren() const
    Return a vector with nrFrontal keys for each child.
    Definition ClusterTree-inst.h:34
    │ │ │ │ -
    FactorGraphType factors
    Factors associated with this node.
    Definition ClusterTree.h:43
    │ │ │ │ -
    void addChild(const boost::shared_ptr< Cluster > &cluster)
    Add a child cluster.
    Definition ClusterTree.h:71
    │ │ │ │ -
    void addFactors(Key key, const CONTAINER &factorsToAdd)
    Add factors associated with a single key.
    Definition ClusterTree.h:64
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ + │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    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
    │ │ │ │ +
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    KeyVector keyVector() const
    Potentially slow function to return all keys involved, sorted, as a vector.
    Definition FactorGraph-inst.h:95
    │ │ │ │ +
    double error(const HybridValues &values) const
    Add error for all factors.
    Definition FactorGraph-inst.h:66
    │ │ │ │ +
    size_t nrFactors() const
    return the number of non-null factors
    Definition FactorGraph-inst.h:76
    │ │ │ │ +
    size_t size() const
    return the number of factors (including any null factors set by remove() ).
    Definition FactorGraph.h:326
    │ │ │ │ +
    bool equals(const This &fg, double tol=1e-9) const
    Check equality up to tolerance.
    Definition FactorGraph-inst.h:50
    │ │ │ │ +
    boost::shared_ptr< FACTOR > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ +
    FastVector< sharedFactor > factors_
    concept check, makes sure FACTOR defines print and equals
    Definition FactorGraph.h:131
    │ │ │ │ +
    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
    │ │ │ │ +
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::optional< Vector2 > variablePos(Key key) const
    Return variable position or none.
    Definition DotWriter.cpp:79
    │ │ │ │ +
    void graphPreamble(std::ostream *os) const
    Write out preamble for graph, including size.
    Definition DotWriter.cpp:30
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,384 +1,305 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ClusterTree.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 │ │ │ │ │ -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; │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 }; │ │ │ │ │ +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} │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _C_l_u_s_t_e_r_T_r_e_e_._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/a00641.html │ │ │ │ @@ -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/inference/FactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,56 +95,58 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    Factor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    FactorGraph.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    The base class for all factors. │ │ │ │ +

    Factor Graph Base Class. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Factor
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Typedefs

    typedef FastVector< FactorIndexgtsam::FactorIndices
     Define collection types:
     
    │ │ │ │ -typedef FastSet< FactorIndexgtsam::FactorIndexSet
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    The base class for all factors.

    │ │ │ │ -
    Author
    Kai Ni
    │ │ │ │ +

    Factor Graph Base Class.

    │ │ │ │ +
    Author
    Carlos Nieto
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Michael Kaess │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 | _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 │ │ │ │ │ +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_:_:_F_a_c_t_o_r │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_._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/a00641.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,6 @@ │ │ │ │ │ var a00641 = [ │ │ │ │ │ - ["gtsam::Factor", "a03592.html", "a03592"], │ │ │ │ │ - ["FactorIndices", "a00641.html#a99e63c90a8ac740d99716e73fe97e054", null] │ │ │ │ │ + ["gtsam::CRefCallPushBack< C >", "a03596.html", null], │ │ │ │ │ + ["gtsam::RefCallPushBack< C >", "a03600.html", null], │ │ │ │ │ + ["gtsam::CRefCallAddCopy< C >", "a03604.html", null], │ │ │ │ │ + ["gtsam::FactorGraph< FACTOR >", "a03524.html", "a03524"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00641_source.html │ │ │ │ @@ -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/inference/FactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,183 +98,431 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Factor.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
    │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    29#include <gtsam/inference/Key.h>
    │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ -
    32
    │ │ │ │ - │ │ │ │ -
    35 typedef FastSet<FactorIndex> FactorIndexSet;
    │ │ │ │ +
    21// \callgraph
    │ │ │ │ +
    22
    │ │ │ │ +
    23#pragma once
    │ │ │ │ +
    24
    │ │ │ │ + │ │ │ │ +
    26#include <gtsam/inference/Key.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 class HybridValues; // forward declaration of a Value type for error.
    │ │ │ │ -
    38
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │
    │ │ │ │ -
    67 class GTSAM_EXPORT Factor
    │ │ │ │ -
    68 {
    │ │ │ │ + │ │ │ │ +
    68 C& obj;
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    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); }
    │ │ │ │ -
    │ │ │ │ -
    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(); }
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    98 public:
    │ │ │ │ +
    99 typedef FACTOR FactorType;
    │ │ │ │ +
    100 typedef boost::shared_ptr<FACTOR>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    131 Key front() const { return keys_.front(); }
    │ │ │ │ +
    130
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    134 bool isEqual(const FactorGraph& other) const {
    │ │ │ │ +
    135 return factors_ == other.factors_;
    │ │ │ │ +
    136 }
    │ │ │ │ +
    │ │ │ │ +
    137
    │ │ │ │ +
    140
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    175 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ -
    176
    │ │ │ │ -
    180
    │ │ │ │ -
    182 KeyVector& keys() { return keys_; }
    │ │ │ │ +
    177
    │ │ │ │ +
    182 void reserve(size_t size) { factors_.reserve(size); }
    │ │ │ │
    183
    │ │ │ │ -
    185 iterator begin() { return keys_.begin(); }
    │ │ │ │ -
    186
    │ │ │ │ -
    188 iterator end() { return keys_.end(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    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 };
    │ │ │ │ +
    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
    │ │ │ │ -
    208} // \namespace gtsam
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ - │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Graphviz formatter.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
    Definition FastSet.h:50
    │ │ │ │ + │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    const_iterator find(Key key) const
    find
    Definition Factor.h:137
    │ │ │ │ -
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ -
    Factor()
    Default constructor for I/O.
    Definition Factor.h:91
    │ │ │ │ -
    iterator end()
    Iterator at end of involved variable keys.
    Definition Factor.h:188
    │ │ │ │ -
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │ -
    KeyVector & keys()
    Definition Factor.h:182
    │ │ │ │ -
    static Factor FromIterators(ITERATOR first, ITERATOR last)
    Construct factor from iterator keys.
    Definition Factor.h:112
    │ │ │ │ -
    static Factor FromKeys(const CONTAINER &keys)
    Construct factor from container of keys.
    Definition Factor.h:106
    │ │ │ │ -
    iterator begin()
    Iterator at beginning of involved variable keys.
    Definition Factor.h:185
    │ │ │ │ -
    virtual ~Factor()=default
    Default destructor.
    │ │ │ │ -
    Factor(ITERATOR first, ITERATOR last)
    Construct factor from iterator keys.
    Definition Factor.h:101
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    const_iterator end() const
    Iterator at end of involved variable keys.
    Definition Factor.h:146
    │ │ │ │ -
    Factor(const CONTAINER &keys)
    Construct factor from container of keys.
    Definition Factor.h:96
    │ │ │ │ -
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ -
    Key back() const
    Last key.
    Definition Factor.h:134
    │ │ │ │ -
    Key front() const
    First key.
    Definition Factor.h:131
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    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
    │ │ │ │ +
    bool isEqual(const FactorGraph &other) const
    Check exact equality of the factor pointers. Useful for derived ==.
    Definition FactorGraph.h:134
    │ │ │ │ +
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ +
    bool empty() const
    Check if the graph is empty (null factors set by remove() will cause this to return false).
    Definition FactorGraph.h:330
    │ │ │ │ +
    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
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ +
    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
    │ │ │ │ +
    iterator erase(iterator item)
    Erase factor and rearrange other factors to take up the empty space.
    Definition FactorGraph.h:391
    │ │ │ │ +
    void add(const FACTOR_OR_CONTAINER &factorOrContainer)
    Add a factor or container of factors, including STL collections, BayesTrees, etc.
    Definition FactorGraph.h:269
    │ │ │ │ +
    iterator erase(iterator first, iterator last)
    Erase factors and rearrange other factors to take up the empty space.
    Definition FactorGraph.h:394
    │ │ │ │ +
    sharedFactor back() const
    Get the last factor.
    Definition FactorGraph.h:362
    │ │ │ │ +
    FactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition FactorGraph.h:152
    │ │ │ │ +
    void remove(size_t i)
    delete factor without re-arranging indexes by inserting a nullptr pointer
    Definition FactorGraph.h:385
    │ │ │ │ +
    KeyVector keyVector() const
    Potentially slow function to return all keys involved, sorted, as a vector.
    Definition FactorGraph-inst.h:95
    │ │ │ │ +
    HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
    Push back non-pointer objects in a container (factors are copied).
    Definition FactorGraph.h:260
    │ │ │ │ +
    FactorGraph()
    Default constructor.
    Definition FactorGraph.h:142
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
    Emplace a shared pointer to factor of given type.
    Definition FactorGraph.h:192
    │ │ │ │ +
    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
    │ │ │ │ +
    sharedFactor & at(size_t i)
    Get a specific factor by index (this checks array bounds and may throw an exception,...
    Definition FactorGraph.h:340
    │ │ │ │ +
    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
    │ │ │ │ +
    double error(const HybridValues &values) const
    Add error for all factors.
    Definition FactorGraph-inst.h:66
    │ │ │ │ +
    FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Constructor from iterator over factors (shared_ptr or plain objects)
    Definition FactorGraph.h:146
    │ │ │ │ +
    virtual void resize(size_t size)
    Directly resize the number of factors in the graph.
    Definition FactorGraph.h:381
    │ │ │ │ +
    size_t nrFactors() const
    return the number of non-null factors
    Definition FactorGraph-inst.h:76
    │ │ │ │ +
    size_t size() const
    return the number of factors (including any null factors set by remove() ).
    Definition FactorGraph.h:326
    │ │ │ │ +
    HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
    Push back many factors with an iterator (factors are copied)
    Definition FactorGraph.h:240
    │ │ │ │ +
    sharedFactor front() const
    Get the first factor.
    Definition FactorGraph.h:359
    │ │ │ │ +
    const_iterator end() const
    Iterator to end of factors.
    Definition FactorGraph.h:356
    │ │ │ │ +
    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
    │ │ │ │ +
    FACTOR FactorType
    factor type
    Definition FactorGraph.h:99
    │ │ │ │ +
    void replace(size_t index, sharedFactor factor)
    replace a factor by index
    Definition FactorGraph.h:388
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    iterator begin()
    non-const STL-style begin()
    Definition FactorGraph.h:372
    │ │ │ │ +
    const_iterator begin() const
    Iterator to beginning of factors.
    Definition FactorGraph.h:353
    │ │ │ │ +
    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
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
    add is a synonym for push_back.
    Definition FactorGraph.h:210
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition FactorGraph.h:438
    │ │ │ │ +
    boost::shared_ptr< FACTOR > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
    Constructor that takes an initializer list of shared pointers.
    Definition FactorGraph.h:171
    │ │ │ │ +
    iterator end()
    non-const STL-style end()
    Definition FactorGraph.h:375
    │ │ │ │ +
    FastVector< sharedFactor > factors_
    concept check, makes sure FACTOR defines print and equals
    Definition FactorGraph.h:131
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual ~FactorGraph()=default
    Default destructor Public and virtual so boost serialization can call it.
    │ │ │ │ +
    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
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ +
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ +
    Helper.
    Definition FactorGraph.h:54
    │ │ │ │ +
    Helper.
    Definition FactorGraph.h:67
    │ │ │ │ +
    Helper.
    Definition FactorGraph.h:80
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,260 +1,591 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Factor.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 │ │ │ │ │ -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; │ │ │ │ │ +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> │ │ │ │ │ +29 │ │ │ │ │ +30#include // for Eigen::aligned_allocator │ │ │ │ │ +31 │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ 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 { │ │ │ │ │ +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; │ │ │ │ │ +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 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 │ │ │ │ │ -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(); } │ │ │ │ │ +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 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 │ │ │ │ │ -_1_3_1 _K_e_y _f_r_o_n_t() const { return keys_.front(); } │ │ │ │ │ +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 _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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ -_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(); } │ │ │ │ │ +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 │ │ │ │ │ -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 }; │ │ │ │ │ +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 │ │ │ │ │ -208} // \namespace gtsam │ │ │ │ │ +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. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +_D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ +Graphviz formatter. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ +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 │ │ │ │ │ - * _F_a_c_t_o_r_._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/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/DotWriter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    BayesNet.h File Reference
    │ │ │ │ +
    DotWriter.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Bayes network. │ │ │ │ +

    Graphviz formatting for factor graphs. │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Bayes network.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +

    Graphviz formatting for factor graphs.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    December, 2021
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Bayes network. │ │ │ │ │ +Graphviz formatting for factor graphs. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_N_e_t_._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/a00647.html │ │ │ │ @@ -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/EliminateableFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM-inst.h File Reference
    │ │ │ │ +
    EliminateableFactorGraph.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Incremental update functionality (iSAM) for BayesTree. │ │ │ │ +

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Incremental update functionality (iSAM) for BayesTree.

    │ │ │ │ -
    Author
    Michael Kaess
    │ │ │ │ +

    Variable elimination algorithms for factor graphs.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Apr 21, 2013
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,32 @@ │ │ │ │ │ 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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Apr 21, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_-_i_n_s_t_._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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ │ @@ -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/inference/EliminateableFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,100 +98,250 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM-inst.h
    │ │ │ │ +
    EliminateableFactorGraph.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ -
    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()));
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    22#include <functional>
    │ │ │ │ +
    23#include <boost/variant.hpp>
    │ │ │ │ +
    24#include <boost/optional.hpp>
    │ │ │ │ +
    25
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    34 template<class GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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}
    │ │ │ │ -
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ - │ │ │ │ +
    55 template<class FACTOR_GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    57 {
    │ │ │ │ +
    58 private:
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ +
    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> >
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ + │ │ │ │ +
    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> >
    │ │ │ │ + │ │ │ │ +
    202 const Ordering& ordering,
    │ │ │ │ +
    203 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
    │ │ │ │ +
    204 OptionalVariableIndex variableIndex = boost::none) const;
    │ │ │ │ +
    205
    │ │ │ │ +
    210 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ + │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ - │ │ │ │ -
    Definition BayesTree.h:276
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ +
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::JunctionTreeType JunctionTreeType
    Junction tree type that can do multifrontal elimination of this graph.
    Definition EliminateableFactorGraph.h:82
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::BayesTreeType BayesTreeType
    Bayes tree type produced by multifrontal elimination.
    Definition EliminateableFactorGraph.h:79
    │ │ │ │ +
    EliminationTraitsType::BayesNetType BayesNetType
    Bayes net type produced by sequential elimination.
    Definition EliminateableFactorGraph.h:73
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraitsType::ConditionalType ConditionalType
    Conditional type stored in the Bayes net produced by elimination.
    Definition EliminateableFactorGraph.h:70
    │ │ │ │ +
    EliminationTraitsType::EliminationTreeType EliminationTreeType
    Elimination tree type that can do sequential elimination of this graph.
    Definition EliminateableFactorGraph.h:76
    │ │ │ │ +
    boost::optional< const VariableIndex & > OptionalVariableIndex
    Typedef for an optional variable index as an argument to elimination functions.
    Definition EliminateableFactorGraph.h:92
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    EliminationTraits< FactorGraphType > EliminationTraitsType
    Typedef to the specific EliminationTraits for this graph.
    Definition EliminateableFactorGraph.h:67
    │ │ │ │ +
    boost::optional< Ordering::OrderingType > OptionalOrderingType
    Typedef for an optional ordering type.
    Definition EliminateableFactorGraph.h:95
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,113 +1,347 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM-inst.h │ │ │ │ │ +EliminateableFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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> │ │ │ │ │ -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); │ │ │ │ │ -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())); │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +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> │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ +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} │ │ │ │ │ _V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_S_A_M_-_i_n_s_t_._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 │ │ │ │ │ * Generated on Tue Jan 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/inferenceExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    EliminationTree.h File Reference
    │ │ │ │ +
    inferenceExceptions.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Exceptions that may be thrown by inference algorithms. │ │ │ │ +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::InconsistentEliminationRequested
     An inference algorithm was called with inconsistent arguments. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    Date
    Oct 13, 2010
    │ │ │ │ +

    Exceptions that may be thrown by inference algorithms.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Apr 25, 2013
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -EliminationTree.h File Reference │ │ │ │ │ +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_:_:_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_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Exceptions that may be thrown by inference algorithms. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 13, 2010 │ │ │ │ │ + Apr 25, 2013 │ │ │ │ │ * _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_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/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::InconsistentEliminationRequested", "a03636.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/inferenceExceptions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,160 +98,59 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EliminationTree.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)
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * 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>
    │ │ │ │ -
    22
    │ │ │ │ -
    23#include <gtsam/base/Testable.h>
    │ │ │ │ - │ │ │ │ + │ │ │ │ +
    21#include <boost/lexical_cast.hpp>
    │ │ │ │ +
    22#include <exception>
    │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    52 {
    │ │ │ │ -
    53 protected:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    162
    │ │ │ │ -
    163 private:
    │ │ │ │ -
    165 friend class ::EliminationTreeTester;
    │ │ │ │ -
    166 };
    │ │ │ │ -
    │ │ │ │ -
    167
    │ │ │ │ -
    168}
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ +
    29 class InconsistentEliminationRequested : public std::exception {
    │ │ │ │ +
    30 public:
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ -
    void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    Print the tree to cout.
    Definition EliminationTree-inst.h:207
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTree< BAYESNET, GRAPH > This
    This class.
    Definition EliminationTree.h:54
    │ │ │ │ -
    FastVector< sharedNode > roots_
    concept check
    Definition EliminationTree.h:86
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition EliminationTree.h:60
    │ │ │ │ -
    BAYESNET BayesNetType
    The BayesNet corresponding to FACTOR.
    Definition EliminationTree.h:61
    │ │ │ │ -
    boost::shared_ptr< ConditionalType > sharedConditional
    Shared pointer to a conditional.
    Definition EliminationTree.h:63
    │ │ │ │ -
    const FastVector< sharedNode > & roots() const
    Return the set of roots (one for a tree, multiple for a forest)
    Definition EliminationTree.h:151
    │ │ │ │ -
    GRAPH FactorGraphType
    The factor graph type.
    Definition EliminationTree.h:58
    │ │ │ │ -
    void swap(This &other)
    Swap the data of this tree with another one, this operation is very fast.
    Definition EliminationTree-inst.h:283
    │ │ │ │ -
    EliminationTree()
    Protected default constructor.
    Definition EliminationTree.h:161
    │ │ │ │ -
    BayesNetType::ConditionalType ConditionalType
    The type of conditionals.
    Definition EliminationTree.h:62
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition EliminationTree.h:55
    │ │ │ │ -
    GRAPH::FactorType FactorType
    The type of factors.
    Definition EliminationTree.h:59
    │ │ │ │ -
    boost::shared_ptr< Node > sharedNode
    Shared pointer to Node.
    Definition EliminationTree.h:80
    │ │ │ │ -
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition EliminationTree.h:154
    │ │ │ │ -
    Definition EliminationTree.h:66
    │ │ │ │ -
    Key key
    key associated with root
    Definition EliminationTree.h:70
    │ │ │ │ -
    Children children
    sub-trees
    Definition EliminationTree.h:72
    │ │ │ │ -
    Factors factors
    factors associated with root
    Definition EliminationTree.h:71
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ +
    An inference algorithm was called with inconsistent arguments.
    Definition inferenceExceptions.h:29
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,237 +1,54 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminationTree.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) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * 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_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> │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ +_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_:_:_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_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 │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._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/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/BayesNet-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,125 +98,151 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    VariableIndex-inl.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24#include <boost/range/adaptor/reversed.hpp>
    │ │ │ │ +
    25#include <fstream>
    │ │ │ │ +
    26#include <string>
    │ │ │ │ +
    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 // 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 utilities.
    │ │ │ │ - │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    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();
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Bayes network.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    const FactorIndices & internalAt(Key variable) const
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:172
    │ │ │ │ -
    void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
    Remove entries corresponding to the specified factors.
    Definition VariableIndex-inl.h:54
    │ │ │ │ -
    void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
    Remove unused empty variables (in debug mode verifies they are empty).
    Definition VariableIndex-inl.h:85
    │ │ │ │ -
    void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
    Augment the variable index with new factors.
    Definition VariableIndex-inl.h:27
    │ │ │ │ +
    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
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print out graph
    Definition BayesNet-inst.h:32
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ +
    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
    │ │ │ │ +
    void digraphPreamble(std::ostream *os) const
    Write out preamble for digraph, including size.
    Definition DotWriter.cpp:36
    │ │ │ │ +
    boost::optional< Vector2 > variablePos(Key key) const
    Return variable position or none.
    Definition DotWriter.cpp:79
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,179 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ +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 // 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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_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_:_:_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 │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._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/a00659.html │ │ │ │ @@ -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/inference/Factor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    EliminationTree-inst.h File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    Factor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    The base class for all factors. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::Factor
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    typedef FastVector< FactorIndexgtsam::FactorIndices
     Define collection types:
     
    │ │ │ │ +typedef FastSet< FactorIndexgtsam::FactorIndexSet
     
    │ │ │ │

    Detailed Description

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

    The base class for all factors.

    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │ -
    Date
    Oct 13, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EliminationTree-inst.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 │ │ │ │ │ +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_:_:_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 ********** │ │ │ │ │ +The base class for all factors. │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 13, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00659_source.html │ │ │ │ @@ -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/inference/Factor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,363 +98,183 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EliminationTree-inst.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)
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * 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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    31
    │ │ │ │ -
    32 /* ************************************************************************* */
    │ │ │ │ -
    33 template<class BAYESNET, class GRAPH>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    29#include <gtsam/inference/Key.h>
    │ │ │ │ +
    30
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │ +
    32
    │ │ │ │ + │ │ │ │ +
    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 /* ************************************************************************* */
    │ │ │ │ -
    76 template<class BAYESNET, class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    78 const VariableIndex& structure, const Ordering& order)
    │ │ │ │ -
    79 {
    │ │ │ │ -
    80 gttic(EliminationTree_Contructor);
    │ │ │ │ +
    75 public:
    │ │ │ │ +
    77 typedef KeyVector::iterator iterator;
    │ │ │ │ +
    78
    │ │ │ │ +
    80 typedef KeyVector::const_iterator const_iterator;
    │ │ │ │
    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();
    │ │ │ │ +
    82 protected:
    │ │ │ │ +
    83
    │ │ │ │ + │ │ │ │
    86
    │ │ │ │ -
    87 static const size_t none = std::numeric_limits<size_t>::max();
    │ │ │ │ -
    88
    │ │ │ │ -
    89 // Allocate result parent vector and vector of last factor columns
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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); }
    │ │ │ │ +
    │ │ │ │ +
    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(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ +
    163
    │ │ │ │ +
    165 virtual void print(
    │ │ │ │ +
    166 const std::string& s = "Factor",
    │ │ │ │ +
    167 const KeyFormatter& formatter = DefaultKeyFormatter) const;
    │ │ │ │
    168
    │ │ │ │ -
    169 /* ************************************************************************* */
    │ │ │ │ -
    170 template<class BAYESNET, class GRAPH>
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    173 {
    │ │ │ │ -
    174 // Start by duplicating the tree.
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    182 KeyVector& keys() { return keys_; }
    │ │ │ │
    183
    │ │ │ │ -
    184 /* ************************************************************************* */
    │ │ │ │ -
    185 template<class BAYESNET, class GRAPH>
    │ │ │ │ -
    186 std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    284 roots_.swap(other.roots_);
    │ │ │ │ -
    285 remainingFactors_.swap(other.remainingFactors_);
    │ │ │ │ -
    286 }
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    287
    │ │ │ │ -
    288
    │ │ │ │ -
    289}
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Contains generic inference algorithms that convert between templated graphical models,...
    │ │ │ │ - │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ -
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    Test whether the tree is equal to another.
    Definition EliminationTree-inst.h:214
    │ │ │ │ -
    void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
    Print the tree to cout.
    Definition EliminationTree-inst.h:207
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    FastVector< sharedNode > roots_
    concept check
    Definition EliminationTree.h:86
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition EliminationTree.h:60
    │ │ │ │ -
    GRAPH FactorGraphType
    The factor graph type.
    Definition EliminationTree.h:58
    │ │ │ │ -
    void swap(This &other)
    Swap the data of this tree with another one, this operation is very fast.
    Definition EliminationTree-inst.h:283
    │ │ │ │ -
    EliminationTree()
    Protected default constructor.
    Definition EliminationTree.h:161
    │ │ │ │ -
    boost::shared_ptr< Node > sharedNode
    Shared pointer to Node.
    Definition EliminationTree.h:80
    │ │ │ │ -
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition EliminationTree.h:154
    │ │ │ │ -
    Key key
    key associated with root
    Definition EliminationTree.h:70
    │ │ │ │ -
    Children children
    sub-trees
    Definition EliminationTree.h:72
    │ │ │ │ -
    Factors factors
    factors associated with root
    Definition EliminationTree.h:71
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ +
    FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
    Definition FastSet.h:50
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    const_iterator find(Key key) const
    find
    Definition Factor.h:137
    │ │ │ │ +
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ +
    Factor()
    Default constructor for I/O.
    Definition Factor.h:91
    │ │ │ │ +
    iterator end()
    Iterator at end of involved variable keys.
    Definition Factor.h:188
    │ │ │ │ +
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │ +
    KeyVector & keys()
    Definition Factor.h:182
    │ │ │ │ +
    static Factor FromIterators(ITERATOR first, ITERATOR last)
    Construct factor from iterator keys.
    Definition Factor.h:112
    │ │ │ │ +
    static Factor FromKeys(const CONTAINER &keys)
    Construct factor from container of keys.
    Definition Factor.h:106
    │ │ │ │ +
    iterator begin()
    Iterator at beginning of involved variable keys.
    Definition Factor.h:185
    │ │ │ │ +
    virtual ~Factor()=default
    Default destructor.
    │ │ │ │ +
    Factor(ITERATOR first, ITERATOR last)
    Construct factor from iterator keys.
    Definition Factor.h:101
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ +
    const_iterator end() const
    Iterator at end of involved variable keys.
    Definition Factor.h:146
    │ │ │ │ +
    Factor(const CONTAINER &keys)
    Construct factor from container of keys.
    Definition Factor.h:96
    │ │ │ │ +
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ +
    Key back() const
    Last key.
    Definition Factor.h:134
    │ │ │ │ +
    Key front() const
    First key.
    Definition Factor.h:131
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,449 +1,260 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminationTree-inst.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) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * 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 │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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); │ │ │ │ │ +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 // 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(); │ │ │ │ │ +82 protected: │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _K_e_y_V_e_c_t_o_r _k_e_y_s__; │ │ │ │ │ 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 } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_1_8_2 _K_e_y_V_e_c_t_o_r& _k_e_y_s() { return keys_; } │ │ │ │ │ 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. │ │ │ │ │ -_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 │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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_:_:_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 │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_-_i_n_s_t_._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/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/Key.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,43 +95,96 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    ISAM.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    Key.h 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...
    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

    │ │ │ │ -

    Incremental update functionality (iSAM) for BayesTree.

    │ │ │ │ -
    Author
    Michael Kaess
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Alex Cunningham
    │ │ │ │ +
    Date
    Feb 20, 2012
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Feb 20, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -ISAM.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 | _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_:_:_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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ Author │ │ │ │ │ - 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 │ │ │ │ │ - * _I_S_A_M_._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/a00662.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,11 @@ │ │ │ │ │ var a00662 = [ │ │ │ │ │ - ["gtsam::ISAM< BAYESTREE >", "a03640.html", "a03640"] │ │ │ │ │ + ["gtsam::StreamedKey", "a03656.html", null], │ │ │ │ │ + ["gtsam::key_formatter", "a03660.html", null], │ │ │ │ │ + ["gtsam::traits< Key >", "a03664.html", null], │ │ │ │ │ + ["KeyFormatter", "a00662.html#ae4b4e8e7f0d745882c6a02b507d5bffe", null], │ │ │ │ │ + ["KeyVector", "a00662.html#a1c320e42b033352fbbd8266e3bd5faad", null], │ │ │ │ │ + ["PrintKey", "a00662.html#a4f9e295c4f7086ce05cbe37aeeca47fe", null], │ │ │ │ │ + ["PrintKeyList", "a00662.html#aab7ddea9b0bc36f296327e0ec08c4ef0", null], │ │ │ │ │ + ["PrintKeySet", "a00662.html#aac25d6c8afb07054416ba6727685bf55", null], │ │ │ │ │ + ["PrintKeyVector", "a00662.html#a4c14be406d50f65bd404429a7b1d3af0", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00662_source.html │ │ │ │ @@ -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/inference/Key.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,80 +98,148 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM.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
    │ │ │ │ -
    18// \callgraph
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/base/FastList.h>
    │ │ │ │ +
    21#include <gtsam/base/FastMap.h>
    │ │ │ │ +
    22#include <gtsam/base/FastSet.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
    │ │ │ │ -
    40 private:
    │ │ │ │ -
    41 typedef typename Base::Eliminate Eliminate;
    │ │ │ │ -
    42 typedef typename Base::EliminationTraitsType EliminationTraitsType;
    │ │ │ │ -
    43
    │ │ │ │ -
    44 public:
    │ │ │ │ +
    43static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
    │ │ │ │ +
    44
    │ │ │ │ +
    45// Helper function for Multi-robot Key Formatter
    │ │ │ │ +
    46GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
    │ │ │ │
    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
    │ │ │ │ -
    75};
    │ │ │ │ -
    │ │ │ │ -
    76
    │ │ │ │ -
    77} // namespace gtsam
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    54static const gtsam::KeyFormatter MultiRobotKeyFormatter =
    │ │ │ │ +
    55 &_multirobotKeyFormatter;
    │ │ │ │ +
    56
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    87
    │ │ │ │ +
    88// TODO(frank): Nothing fast about these :-(
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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} // namespace gtsam
    │ │ │ │ +
    127
    │ │ │ │ +
    128
    │ │ │ │ +
    129
    │ │ │ │ +
    130
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ -
    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
    │ │ │ │ -
    ISAM(const Base &bayesTree)
    Copy constructor.
    Definition ISAM.h:52
    │ │ │ │ -
    ISAM()
    Create an empty Bayes Tree.
    Definition ISAM.h:49
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    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
    │ │ │ │ +
    void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
    Utility function to print one key with optional prefix.
    Definition Key.cpp:40
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │ +
    To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
    Definition Key.h:58
    │ │ │ │ +
    Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter,...
    Definition Key.h:72
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,90 +1,187 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM.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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -40 private: │ │ │ │ │ -41 typedef typename Base::Eliminate Eliminate; │ │ │ │ │ -42 typedef typename Base::EliminationTraitsType EliminationTraitsType; │ │ │ │ │ -43 │ │ │ │ │ -44 public: │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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. │ │ │ │ │ +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); │ │ │ │ │ +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 │ │ │ │ │ +127 │ │ │ │ │ +128 │ │ │ │ │ +129 │ │ │ │ │ +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_S_e_t_._h │ │ │ │ │ +A thin wrapper around std::set 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. │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_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_:_:_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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_._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/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/Conditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,68 +95,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    LabeledSymbol.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    Conditional.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Base class for conditional densities. │ │ │ │ +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::Conditional< FACTOR, DERIVEDCONDITIONAL >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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
    │ │ │ │ +

    Base class for conditional densities.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +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 | _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 │ │ │ │ │ +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_:_:_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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ - Author │ │ │ │ │ - : Alex Cunningham │ │ │ │ │ - Date │ │ │ │ │ - Jan 12, 2010 │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ Author │ │ │ │ │ - : Alex Cunningham │ │ │ │ │ - : Frank Dellaert │ │ │ │ │ - : Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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::Conditional< FACTOR, DERIVEDCONDITIONAL >", "a03572.html", "a03572"] │ │ │ │ │ ]; │ │ │ ├── ./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/Conditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,146 +98,185 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    LabeledSymbol.h
    │ │ │ │ +
    Conditional.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>
    │ │ │ │ - │ │ │ │ +
    18// \callgraph
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <boost/range.hpp>
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/inference/Key.h>
    │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    │ │ │ │ -
    35class GTSAM_EXPORT LabeledSymbol {
    │ │ │ │ -
    36protected:
    │ │ │ │ -
    37 unsigned char c_, label_;
    │ │ │ │ -
    38 std::uint64_t j_;
    │ │ │ │ -
    39
    │ │ │ │ -
    40public:
    │ │ │ │ - │ │ │ │ -
    43
    │ │ │ │ -
    45 LabeledSymbol(const LabeledSymbol& key);
    │ │ │ │ -
    46
    │ │ │ │ -
    48 LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
    │ │ │ │ -
    49
    │ │ │ │ - │ │ │ │ -
    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_; }
    │ │ │ │ +
    27 class HybridValues; // forward declaration.
    │ │ │ │ +
    28
    │ │ │ │ +
    62 template<class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    64 {
    │ │ │ │ +
    65 protected:
    │ │ │ │ + │ │ │ │
    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_); }
    │ │ │ │ +
    69 private:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    86
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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:
    │ │ │ │ +
    107 virtual ~Conditional() {}
    │ │ │ │ +
    108
    │ │ │ │ +
    110 size_t nrFrontals() const { return nrFrontals_; }
    │ │ │ │ +
    111
    │ │ │ │ +
    113 size_t nrParents() const { return asFactor().size() - nrFrontals_; }
    │ │ │ │
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    unsigned char mrsymbolChr(Key key)
    Return the character portion of a symbol key.
    Definition LabeledSymbol.h:131
    │ │ │ │ -
    size_t mrsymbolIndex(Key key)
    Return the index portion of a symbol key.
    Definition LabeledSymbol.h:137
    │ │ │ │ -
    unsigned char mrsymbolLabel(Key key)
    Return the label portion of a symbol key.
    Definition LabeledSymbol.h:134
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    bool operator!=(const Matrix &A, const Matrix &B)
    inequality
    Definition Matrix.h:107
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    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
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Customized version of gtsam::Symbol for multi-robot use.
    Definition LabeledSymbol.h:35
    │ │ │ │ -
    unsigned char chr() const
    Retrieve key character.
    Definition LabeledSymbol.h:70
    │ │ │ │ -
    gtsam::Key key() const
    return the integer version
    Definition LabeledSymbol.h:64
    │ │ │ │ -
    size_t index() const
    Retrieve key index.
    Definition LabeledSymbol.h:73
    │ │ │ │ -
    unsigned char label() const
    Retrieve label character.
    Definition LabeledSymbol.h:67
    │ │ │ │ - │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
    Check invariants of this conditional, given the values x.
    Definition Conditional-inst.h:77
    │ │ │ │ +
    size_t nrFrontals_
    The first nrFrontal variables are frontal and the rest are parents.
    Definition Conditional.h:67
    │ │ │ │ +
    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
    │ │ │ │ +
    Key firstFrontalKey() const
    Convenience function to get the first frontal key.
    Definition Conditional.h:116
    │ │ │ │ +
    FACTOR::const_iterator endFrontals() const
    Iterator pointing past the last frontal key.
    Definition Conditional.h:163
    │ │ │ │ +
    FACTOR::iterator endParents()
    Mutable iterator pointing past the last parent key.
    Definition Conditional.h:184
    │ │ │ │ +
    FACTOR::iterator endFrontals()
    Mutable iterator pointing past the last frontal key.
    Definition Conditional.h:178
    │ │ │ │ +
    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
    │ │ │ │ +
    Parents parents() const
    return a view of the parent keys
    Definition Conditional.h:127
    │ │ │ │ +
    double operator()(const HybridValues &x) const
    Evaluate probability density, sugar.
    Definition Conditional.h:142
    │ │ │ │ +
    boost::iterator_range< typename FACTOR::const_iterator > Parents
    View of the separator keys (call parents())
    Definition Conditional.h:78
    │ │ │ │ +
    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
    │ │ │ │ +
    size_t nrFrontals() const
    return the number of frontals
    Definition Conditional.h:110
    │ │ │ │ +
    Conditional()
    Empty Constructor to make serialization possible.
    Definition Conditional.h:85
    │ │ │ │ +
    Conditional(size_t nrFrontals)
    Constructor.
    Definition Conditional.h:88
    │ │ │ │ +
    FACTOR::iterator beginParents()
    Mutable iterator pointing to the first parent key.
    Definition Conditional.h:181
    │ │ │ │ +
    size_t & nrFrontals()
    Mutable version of nrFrontals.
    Definition Conditional.h:172
    │ │ │ │ +
    FACTOR::iterator beginFrontals()
    Mutable iterator pointing to first frontal key.
    Definition Conditional.h:175
    │ │ │ │ +
    boost::iterator_range< typename FACTOR::const_iterator > Frontals
    View of the frontal keys (call frontals())
    Definition Conditional.h:75
    │ │ │ │ +
    Frontals frontals() const
    return a view of the frontal keys
    Definition Conditional.h:124
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition Conditional.h:217
    │ │ │ │ +
    FACTOR::const_iterator beginFrontals() const
    Iterator pointing to first frontal key.
    Definition Conditional.h:160
    │ │ │ │ +
    double normalizationConstant() const
    Non-virtual, exponentiate logNormalizationConstant.
    Definition Conditional-inst.h:70
    │ │ │ │ +
    size_t nrParents() const
    return the number of parents
    Definition Conditional.h:113
    │ │ │ │ +
    void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print with optional formatter
    Definition Conditional-inst.h:30
    │ │ │ │ +
    FACTOR::const_iterator endParents() const
    Iterator pointing past the last parent key.
    Definition Conditional.h:169
    │ │ │ │ +
    FACTOR::const_iterator beginParents() const
    Iterator pointing to the first parent key.
    Definition Conditional.h:166
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,189 +1,285 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LabeledSymbol.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 │ │ │ │ │ -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> │ │ │ │ │ +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 { │ │ │ │ │ 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_; } │ │ │ │ │ +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 │ │ │ │ │ -_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_); } │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ -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: │ │ │ │ │ +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 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 │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _L_a_b_e_l_e_d_S_y_m_b_o_l_._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/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/MetisIndex-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Ordering.h File Reference
    │ │ │ │ +
    MetisIndex-inl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Variable ordering for the elimination algorithm. │ │ │ │ -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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Variable ordering for the elimination algorithm.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Andrew Melim
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Sep 2, 2010
    │ │ │ │ +
    Author
    Andrew Melim
    │ │ │ │ +
    Date
    Oct. 10, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -Ordering.h File Reference │ │ │ │ │ -Variable ordering for the elimination algorithm. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +MetisIndex-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_:_:_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 ********** │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Andrew Melim │ │ │ │ │ - Frank Dellaert │ │ │ │ │ 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_._h │ │ │ │ │ + * _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/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/MetisIndex-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,255 +98,104 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Ordering.h
    │ │ │ │ +
    MetisIndex-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
    │ │ │ │ -
    21#pragma once
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <map>
    │ │ │ │ +
    21#include <vector>
    │ │ │ │
    22
    │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <gtsam/base/FastSet.h>
    │ │ │ │ -
    27
    │ │ │ │ -
    28#include <boost/assign/list_inserter.hpp>
    │ │ │ │ -
    29#include <algorithm>
    │ │ │ │ -
    30#include <vector>
    │ │ │ │ +
    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
    │ │ │ │ -
    32namespace gtsam {
    │ │ │ │ -
    33
    │ │ │ │ -
    │ │ │ │ -
    34class Ordering: public KeyVector {
    │ │ │ │ -
    35protected:
    │ │ │ │ -
    36 typedef KeyVector Base;
    │ │ │ │ -
    37
    │ │ │ │ -
    38public:
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    42 COLAMD, METIS, NATURAL, CUSTOM
    │ │ │ │ -
    43 };
    │ │ │ │ +
    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 // 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}
    │ │ │ │
    │ │ │ │ -
    44
    │ │ │ │ -
    45 typedef Ordering This;
    │ │ │ │ -
    46 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    47
    │ │ │ │ -
    49 GTSAM_EXPORT
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    76
    │ │ │ │ -
    78 bool contains(const Key& key) const;
    │ │ │ │ -
    79
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    83
    │ │ │ │ +
    84} // \ gtsam
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
    Definition MetisIndex.h:45
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    static Ordering Natural(const FACTOR_GRAPH &fg)
    Return a natural Ordering. Typically used by iterative solvers.
    Definition Ordering.h:190
    │ │ │ │ -
    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
    │ │ │ │ -
    Ordering(const KEYS &keys)
    Create from a container.
    Definition Ordering.h:57
    │ │ │ │ -
    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
    │ │ │ │ -
    OrderingType
    Type of ordering to use.
    Definition Ordering.h:41
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition Ordering.h:46
    │ │ │ │ -
    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
    │ │ │ │ -
    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_EXPORT Ordering()
    Create an empty ordering.
    Definition Ordering.h:50
    │ │ │ │ -
    static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
    Compute an ordering determined by METIS from a VariableIndex.
    Definition Ordering.cpp:212
    │ │ │ │ -
    FastMap< Key, size_t > invert() const
    Invert (not reverse) the ordering - returns a map from key to order position.
    Definition Ordering.cpp:36
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition Ordering.h:260
    │ │ │ │ -
    Ordering This
    Typedef to this class.
    Definition Ordering.h:45
    │ │ │ │ -
    bool contains(const Key &key) const
    Check if key exists in ordering.
    Definition Ordering.cpp:293
    │ │ │ │ -
    static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > &adj, const FACTOR_GRAPH &graph)
    METIS Formatting function.
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ +
    void augment(const FACTORGRAPH &factors)
    Augment the variable index with new factors.
    Definition MetisIndex-inl.h:27
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,312 +1,105 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Ordering.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 │ │ │ │ │ -21#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#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 │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +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 │ │ │ │ │ -_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() { │ │ │ │ │ +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 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. │ │ │ │ │ -_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 │ │ │ │ │ +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 // 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_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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _O_r_d_e_r_i_n_g_._h │ │ │ │ │ + * _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/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/BayesTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,151 +98,680 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BayesNet-inst.h
    │ │ │ │ +
    BayesTree-inst.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)
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24#include <boost/range/adaptor/reversed.hpp>
    │ │ │ │ -
    25#include <fstream>
    │ │ │ │ -
    26#include <string>
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26#include <gtsam/base/timing.h>
    │ │ │ │
    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);
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    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();
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    28#include <boost/optional.hpp>
    │ │ │ │ +
    29#include <fstream>
    │ │ │ │ +
    30
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │ +
    32
    │ │ │ │ +
    33 /* ************************************************************************* */
    │ │ │ │ +
    34 template<class CLIQUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    37 for (const sharedClique& root : roots_) getCliqueData(root, &stats);
    │ │ │ │ +
    38 return stats;
    │ │ │ │ +
    39 }
    │ │ │ │ +
    │ │ │ │ +
    40
    │ │ │ │ +
    41 /* ************************************************************************* */
    │ │ │ │ +
    42 template <class 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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    197 /* ************************************************************************* */
    │ │ │ │ +
    198 template<class CLIQUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    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)));
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    254 for(const sharedClique& child: subtree->children) {
    │ │ │ │ +
    255 fillNodesIndex(child); }
    │ │ │ │ + │ │ │ │ +
    257
    │ │ │ │ +
    258 /* ************************************************************************* */
    │ │ │ │ +
    259 template<class CLIQUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    409 // Remove all nodes and clear the root pointer
    │ │ │ │ +
    410 nodes_.clear();
    │ │ │ │ +
    411 roots_.clear();
    │ │ │ │ +
    412 }
    │ │ │ │ +
    │ │ │ │ +
    413
    │ │ │ │ +
    414 /* ************************************************************************* */
    │ │ │ │ +
    415 template<class CLIQUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    417 for(const sharedClique& root: roots_) {
    │ │ │ │ +
    418 root->deleteCachedShortcuts();
    │ │ │ │ +
    419 }
    │ │ │ │ +
    420 }
    │ │ │ │ +
    │ │ │ │ +
    421
    │ │ │ │ +
    422 /* ************************************************************************* */
    │ │ │ │ +
    423 template<class 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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │
    │ │ │ │
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Bayes network.
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ + │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ -
    void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print out graph
    Definition BayesNet-inst.h:32
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ -
    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
    │ │ │ │ -
    void digraphPreamble(std::ostream *os) const
    Write out preamble for digraph, including size.
    Definition DotWriter.cpp:36
    │ │ │ │ -
    boost::optional< Vector2 > variablePos(Key key) const
    Return variable position or none.
    Definition DotWriter.cpp:79
    │ │ │ │ -
    the error.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ + │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    store all the sizes
    Definition BayesTree.h:48
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    Nodes nodes_
    Map from indices to Clique.
    Definition BayesTree.h:100
    │ │ │ │ +
    void removeClique(sharedClique clique)
    remove a clique: warning, can result in a forest
    Definition BayesTree-inst.h:424
    │ │ │ │ +
    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
    │ │ │ │ +
    void fillNodesIndex(const sharedClique &subtree)
    Fill the nodes index for a subtree.
    Definition BayesTree-inst.h:246
    │ │ │ │ +
    void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output to graphviz format, stream version.
    Definition BayesTree-inst.h:64
    │ │ │ │ +
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition BayesTree-inst.h:230
    │ │ │ │ +
    This & operator=(const This &other)
    Assignment operator.
    Definition BayesTree-inst.h:199
    │ │ │ │ +
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ +
    BayesTree()
    Create an empty Bayes Tree.
    Definition BayesTree.h:109
    │ │ │ │ +
    void clear()
    Remove all nodes.
    Definition BayesTree-inst.h:408
    │ │ │ │ +
    void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
    add a clique (top down)
    Definition BayesTree-inst.h:142
    │ │ │ │ +
    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
    │ │ │ │ +
    Key findParentClique(const CONTAINER &parents) const
    Find parent clique of a conditional.
    Definition BayesTree-inst.h:238
    │ │ │ │ +
    size_t size() const
    number of cliques
    Definition BayesTree-inst.h:133
    │ │ │ │ +
    void deleteCachedShortcuts()
    Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
    Definition BayesTree-inst.h:416
    │ │ │ │ +
    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
    │ │ │ │ +
    sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
    Return marginal on any variable.
    Definition BayesTree-inst.h:270
    │ │ │ │ +
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTree-inst.h:55
    │ │ │ │ +
    BayesTreeCliqueData getCliqueData() const
    Gather data on all cliques.
    Definition BayesTree-inst.h:35
    │ │ │ │ +
    Cliques removeSubtree(const sharedClique &subtree)
    Remove the requested subtree.
    Definition BayesTree-inst.h:496
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ +
    void insertRoot(const sharedClique &subtree)
    Insert a new subtree with known parent clique.
    Definition BayesTree-inst.h:260
    │ │ │ │ +
    void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    output to file with graphviz format.
    Definition BayesTree-inst.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,179 +1,757 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesNet-inst.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_/_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 │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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_:_:_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 DotWriter &writer=DotWriter()) const │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) 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 │ │ │ │ │ +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 DotWriter &writer=DotWriter()) const │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) 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. │ │ │ │ │ +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 │ │ │ │ │ - * BBaayyeessNNeett--iinnsstt..hh │ │ │ │ │ + * _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/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/BayesTreeCliqueBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,42 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    inferenceExceptions.h File Reference
    │ │ │ │ +
    BayesTreeCliqueBase.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Exceptions that may be thrown by inference algorithms. │ │ │ │ +

    Base class for cliques of a BayesTree. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::InconsistentEliminationRequested
     An inference algorithm was called with inconsistent arguments. 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

    │ │ │ │ -

    Exceptions that may be thrown by inference algorithms.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Apr 25, 2013
    │ │ │ │ +

    Base class for cliques of a BayesTree.

    │ │ │ │ +
    Author
    Richard Roberts and Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -inferenceExceptions.h File Reference │ │ │ │ │ -Exceptions that may be thrown by inference algorithms. _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 │ │ │ │ │ -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_:_:_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 ********** │ │ │ │ │ -Exceptions that may be thrown by inference algorithms. │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Apr 25, 2013 │ │ │ │ │ + Richard Roberts and 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 │ │ │ │ │ + * _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/a00674.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00674 = [ │ │ │ │ │ - ["gtsam::InconsistentEliminationRequested", "a03636.html", null] │ │ │ │ │ + ["gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >", "a03552.html", "a03552"] │ │ │ │ │ ]; │ │ │ ├── ./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/BayesTreeCliqueBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    inferenceExceptions.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
    │ │ │ │ @@ -114,43 +114,210 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    18#pragma once
    │ │ │ │
    19
    │ │ │ │ - │ │ │ │ -
    21#include <boost/lexical_cast.hpp>
    │ │ │ │ -
    22#include <exception>
    │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ +
    20#include <gtsam/inference/Key.h>
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/base/types.h>
    │ │ │ │ + │ │ │ │ +
    24#include <boost/optional.hpp>
    │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ -
    29 class InconsistentEliminationRequested : public std::exception {
    │ │ │ │ -
    30 public:
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    26#include <string>
    │ │ │ │ +
    27#include <mutex>
    │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    31 // Forward declarations
    │ │ │ │ +
    32 template<class CLIQUE> class BayesTree;
    │ │ │ │ +
    33 template<class GRAPH> struct EliminationTraits;
    │ │ │ │ +
    34
    │ │ │ │ +
    48 template<class DERIVED, class FACTORGRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    50 {
    │ │ │ │ +
    51 private:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    83 : conditional_(c.conditional_),
    │ │ │ │ +
    84 parent_(c.parent_),
    │ │ │ │ +
    85 children(c.children),
    │ │ │ │ +
    86 problemSize_(c.problemSize_),
    │ │ │ │ +
    87 is_root(c.is_root) {}
    │ │ │ │ +
    │ │ │ │ +
    88
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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_;
    │ │ │ │ + │ │ │ │ +
    111
    │ │ │ │ +
    112 public:
    │ │ │ │ +
    113 sharedConditional conditional_;
    │ │ │ │ +
    114 derived_weak_ptr parent_;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    182
    │ │ │ │ +
    183 const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
    │ │ │ │ +
    184 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    202 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
    │ │ │ │ +
    203 cachedSeparatorMarginal_ = boost::none;
    │ │ │ │ +
    204 }
    │ │ │ │ +
    │ │ │ │ +
    205
    │ │ │ │ +
    206 private:
    │ │ │ │ +
    207
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    An inference algorithm was called with inconsistent arguments.
    Definition inferenceExceptions.h:29
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ +
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ +
    const derived_ptr operator[](size_t i) const
    Return the child at index i.
    Definition BayesTreeCliqueBase.h:150
    │ │ │ │ +
    BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c)
    Shallow copy assignment constructor.
    Definition BayesTreeCliqueBase.h:90
    │ │ │ │ +
    void deleteCachedShortcutsNonRecursive()
    Non-recursive delete cached shortcuts and marginals - internal only.
    Definition BayesTreeCliqueBase.h:201
    │ │ │ │ +
    size_t nrChildren() const
    Return the number of children.
    Definition BayesTreeCliqueBase.h:147
    │ │ │ │ +
    int problemSize() const
    Problem size (used for parallel traversal)
    Definition BayesTreeCliqueBase.h:162
    │ │ │ │ +
    BayesTreeCliqueBase()
    Default constructor.
    Definition BayesTreeCliqueBase.h:74
    │ │ │ │ +
    derived_ptr parent() const
    return a shared_ptr to the parent clique
    Definition BayesTreeCliqueBase.h:159
    │ │ │ │ +
    size_t treeSize() const
    The size of subtree rooted at this clique, i.e., nr of Cliques.
    Definition BayesTreeCliqueBase-inst.h:84
    │ │ │ │ +
    BayesTreeCliqueBase(const sharedConditional &conditional)
    Construct from a conditional, leaving parent and child pointers uninitialized.
    Definition BayesTreeCliqueBase.h:78
    │ │ │ │ +
    std::mutex cachedSeparatorMarginalMutex_
    This protects Cached seperator marginal P(S) from concurrent read/writes as many the functions which ...
    Definition BayesTreeCliqueBase.h:110
    │ │ │ │ +
    bool isRoot() const
    Return true if this clique is the root of a Bayes tree.
    Definition BayesTreeCliqueBase.h:144
    │ │ │ │ +
    BayesTreeCliqueBase(const BayesTreeCliqueBase &c)
    Shallow copy constructor.
    Definition BayesTreeCliqueBase.h:82
    │ │ │ │ +
    FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
    return the marginal P(C) of the clique, using marginal caching
    Definition BayesTreeCliqueBase-inst.h:195
    │ │ │ │ +
    FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
    return the marginal P(S) on the separator
    Definition BayesTreeCliqueBase-inst.h:147
    │ │ │ │ +
    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
    │ │ │ │ +
    void deleteCachedShortcuts()
    This deletes the cached shortcuts of all cliques (subtree) below this clique.
    Definition BayesTreeCliqueBase-inst.h:207
    │ │ │ │ +
    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
    │ │ │ │ +
    const sharedConditional & conditional() const
    Access the conditional.
    Definition BayesTreeCliqueBase.h:141
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BayesTreeCliqueBase.h:209
    │ │ │ │ +
    KeyVector separator_setminus_B(const derived_ptr &B) const
    Calculate set for shortcut calculations.
    Definition BayesTreeCliqueBase-inst.h:45
    │ │ │ │ +
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition BayesTreeCliqueBase-inst.h:76
    │ │ │ │ +
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTreeCliqueBase-inst.h:93
    │ │ │ │ +
    boost::optional< FactorGraphType > cachedSeparatorMarginal_
    This stores the Cached separator marginal P(S)
    Definition BayesTreeCliqueBase.h:105
    │ │ │ │ +
    void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
    Fill the elimination result produced during elimination.
    Definition BayesTreeCliqueBase-inst.h:27
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -inferenceExceptions.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 │ │ │ │ │ @@ -16,39 +16,300 @@ │ │ │ │ │ 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> │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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_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_:_:_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 │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._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/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/LabeledSymbol.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,69 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Symbol.cpp File Reference
    │ │ │ │ +
    LabeledSymbol.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ Functions

    │ │ │ │ -GTSAM_EXPORT std::ostream & gtsam::operator<< (std::ostream &os, const Symbol &symbol)
     
    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
    June 9, 2012
    │ │ │ │ -
    Author
    : Frank Dellaert
    │ │ │ │ +
    Date
    Jan 12, 2010
    │ │ │ │ +
    Author
    : Alex Cunningham
    │ │ │ │ +
    Date
    Jan 12, 2010
    │ │ │ │ +
    Author
    : Alex Cunningham
    │ │ │ │ +
    │ │ │ │ +: Frank Dellaert
    │ │ │ │
    │ │ │ │ : Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -Symbol.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 │ │ │ │ │ +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 │ │ │ │ │ + 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_._._. │ │ │ │ │ +  │ │ │ │ │ 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) │ │ │ │ │ + _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 │ │ │ │ │ - June 9, 2012 │ │ │ │ │ + Jan 12, 2010 │ │ │ │ │ + Author │ │ │ │ │ + : Alex Cunningham │ │ │ │ │ + Date │ │ │ │ │ + Jan 12, 2010 │ │ │ │ │ Author │ │ │ │ │ + : Alex Cunningham │ │ │ │ │ : Frank Dellaert │ │ │ │ │ : Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _S_y_m_b_o_l_._c_p_p │ │ │ │ │ + * _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/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/graph-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,226 +98,338 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BayesTreeCliqueBase.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 <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/types.h>
    │ │ │ │ - │ │ │ │ -
    24#include <boost/optional.hpp>
    │ │ │ │ -
    25
    │ │ │ │ -
    26#include <string>
    │ │ │ │ -
    27#include <mutex>
    │ │ │ │ -
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    31 // Forward declarations
    │ │ │ │ -
    32 template<class CLIQUE> class BayesTree;
    │ │ │ │ -
    33 template<class GRAPH> struct EliminationTraits;
    │ │ │ │ -
    34
    │ │ │ │ -
    48 template<class DERIVED, class FACTORGRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    50 {
    │ │ │ │ -
    51 private:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    83 : conditional_(c.conditional_),
    │ │ │ │ -
    84 parent_(c.parent_),
    │ │ │ │ -
    85 children(c.children),
    │ │ │ │ -
    86 problemSize_(c.problemSize_),
    │ │ │ │ -
    87 is_root(c.is_root) {}
    │ │ │ │ -
    │ │ │ │ -
    88
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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 public:
    │ │ │ │ -
    113 sharedConditional conditional_;
    │ │ │ │ -
    114 derived_weak_ptr parent_;
    │ │ │ │ - │ │ │ │ -
    116 int problemSize_;
    │ │ │ │ -
    117
    │ │ │ │ -
    118 bool is_root = false;
    │ │ │ │ +
    112 return g;
    │ │ │ │ +
    113}
    │ │ │ │ +
    │ │ │ │ +
    114
    │ │ │ │ +
    115/* ************************************************************************* */
    │ │ │ │ +
    116template<class G, class V, class KEY>
    │ │ │ │ +
    117boost::tuple<G, V, std::map<KEY,V> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    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(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    147 size_t nrChildren() const { return children.size(); }
    │ │ │ │ -
    148
    │ │ │ │ -
    150 const derived_ptr operator[](size_t i) const { return children[i]; }
    │ │ │ │ +
    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
    │ │ │ │ -
    153 size_t treeSize() const;
    │ │ │ │ -
    154
    │ │ │ │ -
    156 size_t numCachedSeparatorMarginals() const;
    │ │ │ │ -
    157
    │ │ │ │ -
    159 derived_ptr parent() const { return parent_.lock(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    182
    │ │ │ │ -
    183 const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
    │ │ │ │ -
    184 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    202 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
    │ │ │ │ -
    203 cachedSeparatorMarginal_ = boost::none;
    │ │ │ │ -
    204 }
    │ │ │ │ -
    │ │ │ │ +
    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 private:
    │ │ │ │ -
    207
    │ │ │ │ - │ │ │ │ -
    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 };
    │ │ │ │ -
    │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    225 boost::breadth_first_search(g, root, boost::visitor(vis));
    │ │ │ │
    226
    │ │ │ │ -
    227}
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ - │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    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
    │ │ │ │ +
    227 return config;
    │ │ │ │ +
    228}
    │ │ │ │ +
    │ │ │ │ +
    229
    │ │ │ │ +
    230/* ************************************************************************* */
    │ │ │ │ +
    231template<class G, class KEY, class FACTOR2>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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 algorithm using boost library.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ -
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ -
    const derived_ptr operator[](size_t i) const
    Return the child at index i.
    Definition BayesTreeCliqueBase.h:150
    │ │ │ │ -
    BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c)
    Shallow copy assignment constructor.
    Definition BayesTreeCliqueBase.h:90
    │ │ │ │ -
    void deleteCachedShortcutsNonRecursive()
    Non-recursive delete cached shortcuts and marginals - internal only.
    Definition BayesTreeCliqueBase.h:201
    │ │ │ │ -
    size_t nrChildren() const
    Return the number of children.
    Definition BayesTreeCliqueBase.h:147
    │ │ │ │ -
    int problemSize() const
    Problem size (used for parallel traversal)
    Definition BayesTreeCliqueBase.h:162
    │ │ │ │ -
    BayesTreeCliqueBase()
    Default constructor.
    Definition BayesTreeCliqueBase.h:74
    │ │ │ │ -
    derived_ptr parent() const
    return a shared_ptr to the parent clique
    Definition BayesTreeCliqueBase.h:159
    │ │ │ │ -
    size_t treeSize() const
    The size of subtree rooted at this clique, i.e., nr of Cliques.
    Definition BayesTreeCliqueBase-inst.h:84
    │ │ │ │ -
    BayesTreeCliqueBase(const sharedConditional &conditional)
    Construct from a conditional, leaving parent and child pointers uninitialized.
    Definition BayesTreeCliqueBase.h:78
    │ │ │ │ -
    std::mutex cachedSeparatorMarginalMutex_
    This protects Cached seperator marginal P(S) from concurrent read/writes as many the functions which ...
    Definition BayesTreeCliqueBase.h:110
    │ │ │ │ -
    bool isRoot() const
    Return true if this clique is the root of a Bayes tree.
    Definition BayesTreeCliqueBase.h:144
    │ │ │ │ -
    BayesTreeCliqueBase(const BayesTreeCliqueBase &c)
    Shallow copy constructor.
    Definition BayesTreeCliqueBase.h:82
    │ │ │ │ -
    FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
    return the marginal P(C) of the clique, using marginal caching
    Definition BayesTreeCliqueBase-inst.h:195
    │ │ │ │ -
    FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
    return the marginal P(S) on the separator
    Definition BayesTreeCliqueBase-inst.h:147
    │ │ │ │ -
    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
    │ │ │ │ -
    void deleteCachedShortcuts()
    This deletes the cached shortcuts of all cliques (subtree) below this clique.
    Definition BayesTreeCliqueBase-inst.h:207
    │ │ │ │ -
    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
    │ │ │ │ -
    const sharedConditional & conditional() const
    Access the conditional.
    Definition BayesTreeCliqueBase.h:141
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BayesTreeCliqueBase.h:209
    │ │ │ │ -
    KeyVector separator_setminus_B(const derived_ptr &B) const
    Calculate set for shortcut calculations.
    Definition BayesTreeCliqueBase-inst.h:45
    │ │ │ │ -
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition BayesTreeCliqueBase-inst.h:76
    │ │ │ │ -
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTreeCliqueBase-inst.h:93
    │ │ │ │ -
    boost::optional< FactorGraphType > cachedSeparatorMarginal_
    This stores the Cached separator marginal P(S)
    Definition BayesTreeCliqueBase.h:105
    │ │ │ │ -
    void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
    Fill the elimination result produced during elimination.
    Definition BayesTreeCliqueBase-inst.h:27
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
    find the minimum spanning tree using boost graph library
    Definition graph-inl.h:232
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition graph-inl.h:38
    │ │ │ │ +
    Definition graph-inl.h:154
    │ │ │ │ +
    SDGraph is undirected graph with variable keys and double edge weights.
    Definition graph.h:40
    │ │ │ │ +
    Definition graph.h:47
    │ │ │ │ +
    Map from variable key to parent key.
    Definition graph.h:58
    │ │ │ │ +
    void insert(const KEY &key, const KEY &parent)
    convenience insert so we can pass ints for TypedSymbol keys
    Definition graph.h:61
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,315 +1,377 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 <_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 │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -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 │ │ │ │ │ -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 } │ │ │ │ │ -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__; │ │ │ │ │ +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 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; │ │ │ │ │ +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 │ │ │ │ │ -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(); } │ │ │ │ │ +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 │ │ │ │ │ -_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]; } │ │ │ │ │ +146 if (!foundRoot) │ │ │ │ │ +147 throw std::invalid_argument("predecessorMap2Graph: invalid predecessor │ │ │ │ │ +map!"); │ │ │ │ │ +148 else │ │ │ │ │ +149 return boost::tuple >(g, root, key2vertex); │ │ │ │ │ +150} │ │ │ │ │ 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(); } │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 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 }; │ │ │ │ │ +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} │ │ │ │ │ -_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 │ │ │ │ │ +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_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_:_:_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_:_:_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 │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._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/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/BayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    VariableIndex.h File Reference
    │ │ │ │ +
    BayesNet.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Bayes network. │ │ │ │ +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...
    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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    March 26, 2013
    │ │ │ │ +

    Bayes network.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -VariableIndex.h File Reference │ │ │ │ │ +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 │ │ │ │ │ - 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ +Bayes network. │ │ │ │ │ Author │ │ │ │ │ + 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_._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/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/BayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,199 +98,115 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    VariableIndex.h
    │ │ │ │ +
    BayesNet.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
    │ │ │ │ - │ │ │ │ -
    21#include <gtsam/inference/Key.h>
    │ │ │ │ -
    22#include <gtsam/base/FastMap.h>
    │ │ │ │ - │ │ │ │ -
    24#include <gtsam/dllexport.h>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    24#include <string>
    │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ -
    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.
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    28class HybridValues;
    │ │ │ │ +
    29
    │ │ │ │ +
    34template <class CONDITIONAL>
    │ │ │ │ +
    │ │ │ │ +
    35class BayesNet : public FactorGraph<CONDITIONAL> {
    │ │ │ │ +
    36 private:
    │ │ │ │ + │ │ │ │ +
    38
    │ │ │ │ +
    39 public:
    │ │ │ │ +
    40 typedef typename boost::shared_ptr<CONDITIONAL>
    │ │ │ │ + │ │ │ │ +
    42
    │ │ │ │ +
    43 protected:
    │ │ │ │ +
    46
    │ │ │ │ + │ │ │ │ +
    49
    │ │ │ │ +
    51 template <typename ITERATOR>
    │ │ │ │ +
    │ │ │ │ +
    52 BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    │ │ │ │ +
    53 : Base(firstConditional, lastConditional) {}
    │ │ │ │ +
    │ │ │ │
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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;
    │ │ │ │ +
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ - │ │ │ │ -
    The base class for all factors.
    │ │ │ │ - │ │ │ │ +
    103};
    │ │ │ │ +
    │ │ │ │ +
    104
    │ │ │ │ +
    105} // namespace gtsam
    │ │ │ │ +
    106
    │ │ │ │ +
    107#include <gtsam/inference/BayesNet-inst.h>
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t FactorIndex
    Integer nonlinear factor index type.
    Definition types.h:103
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ -
    const FactorIndices & internalAt(Key variable) const
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:172
    │ │ │ │ -
    size_t nEntries() const
    The number of nonzero blocks, i.e. the number of variable-factor entries.
    Definition VariableIndex.h:86
    │ │ │ │ -
    const_iterator begin() const
    Iterator to the first variable entry.
    Definition VariableIndex.h:156
    │ │ │ │ -
    const_iterator find(Key key) const
    Find the iterator for the requested variable entry.
    Definition VariableIndex.h:162
    │ │ │ │ -
    const FactorIndices & operator[](Key variable) const
    Access a list of factors by variable.
    Definition VariableIndex.h:89
    │ │ │ │ -
    VariableIndex(const FG &factorGraph)
    Create a VariableIndex that computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:71
    │ │ │ │ -
    bool empty(Key variable) const
    Return true if no factors associated with a variable.
    Definition VariableIndex.h:98
    │ │ │ │ -
    size_t size() const
    The number of variable entries. This is equal to the number of unique variable Keys.
    Definition VariableIndex.h:80
    │ │ │ │ -
    FactorIndices & internalAt(Key variable)
    Internal version of 'at' that asserts existence.
    Definition VariableIndex.h:179
    │ │ │ │ -
    const_iterator end() const
    Iterator to the first variable entry.
    Definition VariableIndex.h:159
    │ │ │ │ -
    VariableIndex()
    Default constructor, creates an empty VariableIndex.
    Definition VariableIndex.h:64
    │ │ │ │ -
    size_t nFactors() const
    The number of factors in the original factor graph.
    Definition VariableIndex.h:83
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    BayesNet(std::initializer_list< sharedConditional > conditionals)
    Constructor that takes an initializer list of shared pointers.
    Definition BayesNet.h:60
    │ │ │ │ +
    void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print out graph
    Definition BayesNet-inst.h:32
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< CONDITIONAL > sharedConditional
    A shared pointer to a conditional.
    Definition BayesNet.h:41
    │ │ │ │ +
    BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    Construct from iterator over conditionals.
    Definition BayesNet.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    BayesNet()
    Default constructor as an empty BayesNet.
    Definition BayesNet.h:48
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,255 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VariableIndex.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, Georgia Tech 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_/_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 │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -55 public: │ │ │ │ │ -56 typedef KeyMap::const_iterator const_iterator; │ │ │ │ │ -57 typedef KeyMap::const_iterator iterator; │ │ │ │ │ -58 typedef KeyMap::value_type value_type; │ │ │ │ │ -59 │ │ │ │ │ +_6_0 _B_a_y_e_s_N_e_t(std::initializer_list conditionals) │ │ │ │ │ +61 : _B_a_s_e(conditionals) {} │ │ │ │ │ 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 } │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +103}; │ │ │ │ │ +104 │ │ │ │ │ +105} // namespace gtsam │ │ │ │ │ +106 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_._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/a00695.html │ │ │ │ @@ -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/VariableSlots.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ClusterTree-inst.h File Reference
    │ │ │ │ +
    VariableSlots.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ +

    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::EliminationData< CLUSTERTREE >
    class  gtsam::VariableSlots
     A combined factor is assembled as one block of rows for each component factor. More...
     
    class  gtsam::EliminationData< CLUSTERTREE >::EliminationPostOrderVisitor
    struct  gtsam::traits< VariableSlots >
     traits 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
    │ │ │ │ -
    │ │ │ │ -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,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 │ │ │ │ │ -ClusterTree-inst.h File Reference │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. _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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ - 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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ - Date │ │ │ │ │ - Oct 8, 2013 │ │ │ │ │ +VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ +each block comes from in the source factors. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Oct 4, 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_-_i_n_s_t_._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/a00695.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00695 = [ │ │ │ │ │ - ["gtsam::EliminationData< CLUSTERTREE >", "a03556.html", "a03556"], │ │ │ │ │ - ["gtsam::EliminationData< CLUSTERTREE >::EliminationPostOrderVisitor", "a03560.html", null] │ │ │ │ │ + ["gtsam::VariableSlots", "a03708.html", "a03708"], │ │ │ │ │ + ["gtsam::traits< VariableSlots >", "a03712.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/VariableSlots.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,357 +98,126 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ClusterTree-inst.h
    │ │ │ │ +
    VariableSlots.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    15#include <gtsam/base/timing.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_ << ")";
    │ │ │ │ - │ │ │ │ -
    30}
    │ │ │ │ -
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/base/FastMap.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>
    │ │ │ │
    31
    │ │ │ │ -
    32/* ************************************************************************* */
    │ │ │ │ -
    33template <class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ +
    52class VariableSlots : public FastMap<Key, FastVector<size_t> > {
    │ │ │ │
    53
    │ │ │ │ -
    54/* ************************************************************************* */
    │ │ │ │ -
    55template<class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    54public:
    │ │ │ │ +
    55
    │ │ │ │ + │ │ │ │ +
    57 GTSAM_EXPORT static const size_t Empty;
    │ │ │ │ +
    58
    │ │ │ │ +
    61
    │ │ │ │ +
    67 template<class FG>
    │ │ │ │ +
    68 VariableSlots(const FG& factorGraph);
    │ │ │ │ +
    69
    │ │ │ │ +
    71
    │ │ │ │ +
    74
    │ │ │ │ +
    76 GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
    │ │ │ │
    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);
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    101
    │ │ │ │ -
    102/* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    103template <class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    105 // Start by duplicating the tree.
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ -
    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.
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    232 const This& 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> >
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 utilities.
    │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    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
    │ │ │ │ +
    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}
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
    Definition types.h:192
    │ │ │ │ -
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:197
    │ │ │ │ -
    GRAPH::Eliminate Eliminate
    Typedef for an eliminate subroutine.
    Definition ClusterTree.h:195
    │ │ │ │ -
    Definition BayesTree.h:276
    │ │ │ │ -
    Definition ClusterTree-inst.h:115
    │ │ │ │ - │ │ │ │ -
    A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
    Definition ClusterTree.h:25
    │ │ │ │ -
    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
    │ │ │ │ -
    FastVector< sharedNode > roots_
    concept check
    Definition ClusterTree.h:116
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Print the cluster tree.
    Definition ClusterTree-inst.h:98
    │ │ │ │ -
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:32
    │ │ │ │ -
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition ClusterTree-inst.h:26
    │ │ │ │ -
    Keys orderedFrontalKeys
    Frontal keys of this node.
    Definition ClusterTree.h:41
    │ │ │ │ -
    void mergeChildren(const std::vector< bool > &merge)
    Merge all children for which bit is set into this node.
    Definition ClusterTree-inst.h:56
    │ │ │ │ -
    void merge(const boost::shared_ptr< Cluster > &cluster)
    Merge in given cluster.
    Definition ClusterTree-inst.h:44
    │ │ │ │ -
    std::vector< size_t > nrFrontalsOfChildren() const
    Return a vector with nrFrontal keys for each child.
    Definition ClusterTree-inst.h:34
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A combined factor is assembled as one block of rows for each component factor.
    Definition VariableSlots.h:52
    │ │ │ │ +
    GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
    print
    Definition VariableSlots.cpp:29
    │ │ │ │ +
    VariableSlots(const FG &factorGraph)
    Constructor from a set of factors to be combined.
    Definition VariableSlots.h:89
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,439 +1,149 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ClusterTree-inst.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 │ │ │ │ │ -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_/_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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ 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} │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ +74 │ │ │ │ │ +76 GTSAM_EXPORT void _p_r_i_n_t(const std::string& str = "VariableSlots: ") const; │ │ │ │ │ 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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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} │ │ │ │ │ _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. │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ -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 │ │ │ │ │ +_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_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/ISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    BayesTree.cpp File Reference
    │ │ │ │ +
    ISAM.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ +

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Bayes Tree is a tree of cliques of a Bayes Chain.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Michael Kaess
    │ │ │ │ -
    │ │ │ │ -Viorela Ila
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +

    Incremental update functionality (iSAM) for BayesTree.

    │ │ │ │ +
    Author
    Michael Kaess
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,26 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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. │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ 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_T_r_e_e_._c_p_p │ │ │ │ │ + * _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/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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <gtsam/base/timing.h>
    │ │ │ │ -
    27
    │ │ │ │ -
    28#include <boost/optional.hpp>
    │ │ │ │ -
    29#include <fstream>
    │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ -
    32
    │ │ │ │ -
    33 /* ************************************************************************* */
    │ │ │ │ -
    34 template<class CLIQUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    37 for (const sharedClique& root : roots_) getCliqueData(root, &stats);
    │ │ │ │ -
    38 return stats;
    │ │ │ │ -
    39 }
    │ │ │ │ -
    │ │ │ │ -
    40
    │ │ │ │ -
    41 /* ************************************************************************* */
    │ │ │ │ -
    42 template <class 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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    15#include <gtsam/base/timing.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_ << ")";
    │ │ │ │ + │ │ │ │ +
    30}
    │ │ │ │ +
    │ │ │ │ +
    31
    │ │ │ │ +
    32/* ************************************************************************* */
    │ │ │ │ +
    33template <class GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    101
    │ │ │ │ +
    102/* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    103template <class GRAPH>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    105 // Start by duplicating the tree.
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    197 /* ************************************************************************* */
    │ │ │ │ -
    198 template<class CLIQUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    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)));
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    232 const This& 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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    254 for(const sharedClique& child: subtree->children) {
    │ │ │ │ -
    255 fillNodesIndex(child); }
    │ │ │ │ - │ │ │ │ -
    257
    │ │ │ │ -
    258 /* ************************************************************************* */
    │ │ │ │ -
    259 template<class CLIQUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    409 // Remove all nodes and clear the root pointer
    │ │ │ │ -
    410 nodes_.clear();
    │ │ │ │ -
    411 roots_.clear();
    │ │ │ │ -
    412 }
    │ │ │ │ -
    │ │ │ │ -
    413
    │ │ │ │ -
    414 /* ************************************************************************* */
    │ │ │ │ -
    415 template<class CLIQUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    417 for(const sharedClique& root: roots_) {
    │ │ │ │ -
    418 root->deleteCachedShortcuts();
    │ │ │ │ -
    419 }
    │ │ │ │ -
    420 }
    │ │ │ │ -
    │ │ │ │ -
    421
    │ │ │ │ -
    422 /* ************************************************************************* */
    │ │ │ │ -
    423 template<class 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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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> >
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 utilities.
    │ │ │ │ │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │
    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
    │ │ │ │ -
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ - │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    store all the sizes
    Definition BayesTree.h:48
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    Nodes nodes_
    Map from indices to Clique.
    Definition BayesTree.h:100
    │ │ │ │ -
    void removeClique(sharedClique clique)
    remove a clique: warning, can result in a forest
    Definition BayesTree-inst.h:424
    │ │ │ │ -
    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
    │ │ │ │ -
    void fillNodesIndex(const sharedClique &subtree)
    Fill the nodes index for a subtree.
    Definition BayesTree-inst.h:246
    │ │ │ │ -
    void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output to graphviz format, stream version.
    Definition BayesTree-inst.h:64
    │ │ │ │ -
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition BayesTree-inst.h:230
    │ │ │ │ -
    This & operator=(const This &other)
    Assignment operator.
    Definition BayesTree-inst.h:199
    │ │ │ │ -
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ -
    BayesTree()
    Create an empty Bayes Tree.
    Definition BayesTree.h:109
    │ │ │ │ -
    void clear()
    Remove all nodes.
    Definition BayesTree-inst.h:408
    │ │ │ │ -
    void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
    add a clique (top down)
    Definition BayesTree-inst.h:142
    │ │ │ │ -
    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
    │ │ │ │ -
    Key findParentClique(const CONTAINER &parents) const
    Find parent clique of a conditional.
    Definition BayesTree-inst.h:238
    │ │ │ │ -
    size_t size() const
    number of cliques
    Definition BayesTree-inst.h:133
    │ │ │ │ -
    void deleteCachedShortcuts()
    Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
    Definition BayesTree-inst.h:416
    │ │ │ │ -
    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
    │ │ │ │ -
    sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
    Return marginal on any variable.
    Definition BayesTree-inst.h:270
    │ │ │ │ -
    size_t numCachedSeparatorMarginals() const
    Collect number of cliques with cached separator marginals.
    Definition BayesTree-inst.h:55
    │ │ │ │ -
    BayesTreeCliqueData getCliqueData() const
    Gather data on all cliques.
    Definition BayesTree-inst.h:35
    │ │ │ │ -
    Cliques removeSubtree(const sharedClique &subtree)
    Remove the requested subtree.
    Definition BayesTree-inst.h:496
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ -
    void insertRoot(const sharedClique &subtree)
    Insert a new subtree with known parent clique.
    Definition BayesTree-inst.h:260
    │ │ │ │ -
    void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    output to file with graphviz format.
    Definition BayesTree-inst.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
    Definition types.h:192
    │ │ │ │ +
    A cluster-tree that eliminates to a Bayes tree.
    Definition ClusterTree.h:184
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:197
    │ │ │ │ +
    GRAPH::Eliminate Eliminate
    Typedef for an eliminate subroutine.
    Definition ClusterTree.h:195
    │ │ │ │ +
    Definition BayesTree.h:276
    │ │ │ │ +
    Definition ClusterTree-inst.h:115
    │ │ │ │ + │ │ │ │ +
    A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
    Definition ClusterTree.h:25
    │ │ │ │ +
    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
    │ │ │ │ +
    FastVector< sharedNode > roots_
    concept check
    Definition ClusterTree.h:116
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Print the cluster tree.
    Definition ClusterTree-inst.h:98
    │ │ │ │ +
    boost::shared_ptr< FactorType > sharedFactor
    Shared pointer to a factor.
    Definition ClusterTree.h:32
    │ │ │ │ +
    virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print this node
    Definition ClusterTree-inst.h:26
    │ │ │ │ +
    Keys orderedFrontalKeys
    Frontal keys of this node.
    Definition ClusterTree.h:41
    │ │ │ │ +
    void mergeChildren(const std::vector< bool > &merge)
    Merge all children for which bit is set into this node.
    Definition ClusterTree-inst.h:56
    │ │ │ │ +
    void merge(const boost::shared_ptr< Cluster > &cluster)
    Merge in given cluster.
    Definition ClusterTree-inst.h:44
    │ │ │ │ +
    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/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_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/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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 } │ │ │ │ │ +_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} │ │ │ │ │ -_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +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_i_m_i_n_g_._h │ │ │ │ │ Timing utilities. │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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/ConjugateGradientSolver.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,55 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    GaussianBayesTree-inl.h File Reference
    │ │ │ │ +
    ConjugateGradientSolver.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ +

    Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::ConjugateGradientParameters
     parameters for the conjugate gradient method More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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)
     
    │ │ │ │ +template<class S , class V >
    gtsam::preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters &parameters)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

    │ │ │ │ -

    GaussianBayesTree

    Author
    Frank Dellaert
    │ │ │ │ +

    Implementation of Conjugate Gradient solver for a linear system.

    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ +Sungtae An
    │ │ │ │ +
    Date
    Nov 6, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -GaussianBayesTree-inl.h 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 | _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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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) │ │ │ │ │ +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 ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Sungtae An │ │ │ │ │ + Date │ │ │ │ │ + Nov 6, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00704_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianBayesTree-inl.h
    │ │ │ │ +
    ConjugateGradientSolver.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,57 +114,173 @@ │ │ │ │
    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
    │ │ │ │ + │ │ │ │
    23
    │ │ │ │ -
    24#include <stdarg.h>
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28/* ************************************************************************* */
    │ │ │ │ -
    29namespace internal {
    │ │ │ │ -
    30template<class BAYESTREE>
    │ │ │ │ -
    31void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
    │ │ │ │ -
    32 // parents are assumed to already be solved and available in result
    │ │ │ │ -
    33 clique->conditional()->solveInPlace(result);
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    30
    │ │ │ │ +
    31public:
    │ │ │ │ + │ │ │ │ +
    33 typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
    │ │ │ │
    34
    │ │ │ │ -
    35 // starting from the root, call optimize on each conditional
    │ │ │ │ -
    36 for(const typename BAYESTREE::sharedClique& child: clique->children_)
    │ │ │ │ -
    37 optimizeInPlace<BAYESTREE>(child, result);
    │ │ │ │ -
    38}
    │ │ │ │ -
    39
    │ │ │ │ -
    40/* ************************************************************************* */
    │ │ │ │ -
    41template<class BAYESTREE>
    │ │ │ │ -
    42double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
    │ │ │ │ -
    43 double result = 0.0;
    │ │ │ │ -
    44
    │ │ │ │ -
    45 // this clique
    │ │ │ │ -
    46 result += clique->conditional()->logDeterminant();
    │ │ │ │ -
    47
    │ │ │ │ -
    48 // sum of children
    │ │ │ │ -
    49 for(const typename BAYESTREE::sharedClique& child: clique->children_)
    │ │ │ │ -
    50 result += logDeterminant<BAYESTREE>(child);
    │ │ │ │ -
    51
    │ │ │ │ -
    52 return result;
    │ │ │ │ -
    53}
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    37 size_t reset_;
    │ │ │ │ +
    38 double epsilon_rel_;
    │ │ │ │ +
    39 double epsilon_abs_;
    │ │ │ │ +
    40
    │ │ │ │ +
    41 /* Matrix Operation Kernel */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    43 GTSAM = 0,
    │ │ │ │ +
    44 } blas_kernel_ ;
    │ │ │ │ +
    │ │ │ │ +
    45
    │ │ │ │ + │ │ │ │ +
    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/* ************************************************************************* */
    │ │ │ │ -
    56} // \namespace internal
    │ │ │ │ -
    57} // \namespace gtsam
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    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 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
    │ │ │ │ +
    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}
    │ │ │ │ +
    Some support classes for iterative solvers.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ +
    size_t minIterations_
    minimum number of cg iterations
    Definition ConjugateGradientSolver.h:35
    │ │ │ │ +
    size_t reset_
    number of iterations before reset
    Definition ConjugateGradientSolver.h:37
    │ │ │ │ +
    BLASKernel
    Definition ConjugateGradientSolver.h:42
    │ │ │ │ +
    double epsilon_rel_
    threshold for relative error decrease
    Definition ConjugateGradientSolver.h:38
    │ │ │ │ +
    size_t maxIterations_
    maximum number of cg iterations
    Definition ConjugateGradientSolver.h:36
    │ │ │ │ +
    double epsilon_abs_
    threshold for absolute error decrease
    Definition ConjugateGradientSolver.h:39
    │ │ │ │ +
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianBayesTree-inl.h │ │ │ │ │ +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 │ │ │ │ │ @@ -16,58 +16,201 @@ │ │ │ │ │ 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_B_a_y_e_s_T_r_e_e_._h> // Only to help Eclipse │ │ │ │ │ +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 │ │ │ │ │ -24#include │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -29namespace internal { │ │ │ │ │ -30template │ │ │ │ │ -31void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s& result) { │ │ │ │ │ -32 // parents are assumed to already be solved and available in result │ │ │ │ │ -33 clique->conditional()->solveInPlace(result); │ │ │ │ │ +_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 │ │ │ │ │ -35 // starting from the root, call optimize on each conditional │ │ │ │ │ -36 for(const typename BAYESTREE::sharedClique& child: clique->children_) │ │ │ │ │ -37 optimizeInPlace(child, result); │ │ │ │ │ -38} │ │ │ │ │ -39 │ │ │ │ │ -40/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -41template │ │ │ │ │ -42double logDeterminant(const typename BAYESTREE::sharedClique& clique) { │ │ │ │ │ -43 double result = 0.0; │ │ │ │ │ -44 │ │ │ │ │ -45 // this clique │ │ │ │ │ -46 result += clique->conditional()->logDeterminant(); │ │ │ │ │ -47 │ │ │ │ │ -48 // sum of children │ │ │ │ │ -49 for(const typename BAYESTREE::sharedClique& child: clique->children_) │ │ │ │ │ -50 result += logDeterminant(child); │ │ │ │ │ -51 │ │ │ │ │ -52 return result; │ │ │ │ │ -53} │ │ │ │ │ +_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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -56} // \namespace internal │ │ │ │ │ -57} // \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. │ │ │ │ │ +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 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 │ │ │ │ │ +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. │ │ │ │ │ _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. │ │ │ │ │ +_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/SubgraphSolver.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    VectorValues.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SubgraphSolver.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementations for VectorValues. │ │ │ │ +

    Subgraph Solver from IROS 2010. │ │ │ │ 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
    │ │ │ │ +

    Subgraph Solver from IROS 2010.

    │ │ │ │ +
    Date
    2010
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Alex Cunningham
    │ │ │ │ +Yong Dian Jian
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ + Date │ │ │ │ │ + 2010 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,40 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianEliminationTree.h File Reference
    │ │ │ │ +
    IterativeSolver.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Some support classes for iterative solvers. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::GaussianEliminationTree
    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.
     
    │ │ │ │

    Detailed Description

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

    Some support classes for iterative solvers.

    │ │ │ │ +
    Date
    2010
    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -GaussianEliminationTree.h File Reference │ │ │ │ │ +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_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + 2010 │ │ │ │ │ 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_._h │ │ │ │ │ * Generated on Tue Jan 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.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00713 = [ │ │ │ │ │ - ["gtsam::GaussianEliminationTree", "a03776.html", "a03776"] │ │ │ │ │ + ["gtsam::IterativeOptimizationParameters", "a03828.html", null], │ │ │ │ │ + ["gtsam::IterativeSolver", "a03832.html", null], │ │ │ │ │ + ["gtsam::KeyInfoEntry", "a03836.html", null], │ │ │ │ │ + ["gtsam::KeyInfo", "a03840.html", "a03840"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00713_source.html │ │ │ │ @@ -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/linear/IterativeSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianEliminationTree.h
    │ │ │ │ +
    IterativeSolver.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,182 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ -
    27 class GTSAM_EXPORT GaussianEliminationTree :
    │ │ │ │ -
    28 public EliminationTree<GaussianBayesNet, GaussianFactorGraph>
    │ │ │ │ -
    29 {
    │ │ │ │ -
    30 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    33 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    34
    │ │ │ │ - │ │ │ │ -
    44 const VariableIndex& structure, const Ordering& order);
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    45
    │ │ │ │ - │ │ │ │ -
    52 const Ordering& order);
    │ │ │ │ -
    53
    │ │ │ │ -
    55 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ -
    56
    │ │ │ │ -
    57 private:
    │ │ │ │ +
    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 friend class ::EliminationTreeTester;
    │ │ │ │ -
    60
    │ │ │ │ -
    61 };
    │ │ │ │ -
    │ │ │ │ -
    62
    │ │ │ │ -
    63}
    │ │ │ │ - │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    78
    │ │ │ │ +
    79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s);
    │ │ │ │ +
    80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v);
    │ │ │ │ +
    81};
    │ │ │ │ +
    │ │ │ │ +
    82
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    87public:
    │ │ │ │ +
    88 typedef boost::shared_ptr<IterativeSolver> shared_ptr;
    │ │ │ │ + │ │ │ │ +
    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_;
    │ │ │ │ +
    136
    │ │ │ │ +
    137 void initialize(const GaussianFactorGraph &fg);
    │ │ │ │ +
    138
    │ │ │ │ +
    139public:
    │ │ │ │ +
    140
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    143 numCols_(0) {
    │ │ │ │ +
    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
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ -
    Definition GaussianEliminationTree.h:29
    │ │ │ │ -
    GaussianEliminationTree This
    This class.
    Definition GaussianEliminationTree.h:32
    │ │ │ │ -
    EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base
    Base class.
    Definition GaussianEliminationTree.h:31
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition GaussianEliminationTree.h:33
    │ │ │ │
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │ +
    Base class for Iterative Solvers like SubgraphSolver.
    Definition IterativeSolver.h:86
    │ │ │ │ +
    Handy data structure for iterative solvers key to (index, dimension, start)
    Definition IterativeSolver.h:114
    │ │ │ │ +
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ +
    const Ordering & ordering() const
    Return the ordering.
    Definition IterativeSolver.h:158
    │ │ │ │ +
    size_t numCols() const
    Return the total number of columns (scalar variables = sum of dimensions)
    Definition IterativeSolver.h:153
    │ │ │ │ +
    KeyInfo()
    Default Constructor.
    Definition IterativeSolver.h:142
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    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 │ │ │ │ │ -GaussianEliminationTree.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,77 +16,204 @@ │ │ │ │ │ 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); │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +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 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. │ │ │ │ │ +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 │ │ │ │ │ +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() { │ │ │ │ │ +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_; │ │ │ │ │ +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 } │ │ │ │ │ +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. │ │ │ │ │ _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_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_:_:_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_:_:_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 │ │ │ │ │ - * _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_._h │ │ │ │ │ * Generated on Tue Jan 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/GaussianConditional-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,34 +95,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianConditional.cpp File Reference
    │ │ │ │ +
    GaussianConditional-inl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    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

    │ │ │ │

    Conditional Gaussian Base class.

    │ │ │ │ -
    Author
    Christian Potthast, Frank Dellaert
    │ │ │ │ +
    Author
    Christian Potthast
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianConditional.cpp File Reference │ │ │ │ │ +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 ********** │ │ │ │ │ Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast, Frank Dellaert │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/BinaryJacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,53 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    JacobianFactor.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    BinaryJacobianFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ +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

    │ │ │ │ -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_ptrgtsam::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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Dec 8, 2010
    │ │ │ │ +

    A binary JacobianFactor specialization that uses fixed matrix math for speed.

    │ │ │ │ +
    Date
    June 2015
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,31 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_:_:_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 │ │ │ │ │ -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). │ │ │ │ │ -  │ │ │ │ │ ********** 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. │ │ │ │ │ + Date │ │ │ │ │ + June 2015 │ │ │ │ │ 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 │ │ │ │ │ + * _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/a00719.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00719 = [ │ │ │ │ │ - ["EliminateQR", "a00719.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ + ["gtsam::BinaryJacobianFactor< M, N1, N2 >", "a03720.html", "a03720"], │ │ │ │ │ + ["gtsam::traits< BinaryJacobianFactor< M, N1, N2 > >", "a03724.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/linearExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,51 +95,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    iterative-inl.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    linearExceptions.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Iterative methods, template implementation. │ │ │ │ +

    Exceptions that may be thrown by linear solver components. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::CGState< S, V, E >
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Functions

    template<class S , class V , class E >
    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
    │ │ │ │ +

    Exceptions that may be thrown by linear solver components.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Aug 17, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 | _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 │ │ │ │ │ +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 │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_G_S_t_a_t_e_<_ _S_,_ _V_,_ _E_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Exceptions that may be thrown by linear solver components. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ + Aug 17, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ + * _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/a00722.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,6 @@ │ │ │ │ │ var a00722 = [ │ │ │ │ │ - ["gtsam::CGState< S, V, E >", "a03820.html", "a03820"], │ │ │ │ │ - ["conjugateGradients", "a00722.html#a6516ea957c3a22ddf429cefef1fe9486", null] │ │ │ │ │ + ["gtsam::IndeterminantLinearSystemException", "a03864.html", null], │ │ │ │ │ + ["gtsam::InvalidNoiseModel", "a03868.html", "a03868"], │ │ │ │ │ + ["gtsam::InvalidMatrixBlock", "a03872.html", "a03872"], │ │ │ │ │ + ["gtsam::InvalidDenseElimination", "a03876.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/linearExceptions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,179 +98,109 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    iterative-inl.h
    │ │ │ │ +
    linearExceptions.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    21#include <gtsam/base/types.h>
    │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    27 /* ************************************************************************* */
    │ │ │ │ -
    28 // state for CG method
    │ │ │ │ -
    29 template<class S, class V, class E>
    │ │ │ │ -
    │ │ │ │ -
    30 struct CGState {
    │ │ │ │ -
    31
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    94 class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
    │ │ │ │ +
    95 Key j_;
    │ │ │ │ +
    96 public:
    │ │ │ │ +
    97 IndeterminantLinearSystemException(Key j) noexcept : j_(j) {}
    │ │ │ │ +
    98 ~IndeterminantLinearSystemException() noexcept override {}
    │ │ │ │ +
    99 Key nearbyVariable() const { return j_; }
    │ │ │ │ +
    100 const char* what() const noexcept override;
    │ │ │ │ +
    101 };
    │ │ │ │ +
    │ │ │ │
    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
    │ │ │ │ +
    103 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    106 class GTSAM_EXPORT InvalidNoiseModel : public ThreadsafeException<InvalidNoiseModel> {
    │ │ │ │ +
    107 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    110
    │ │ │ │ +
    111 InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) :
    │ │ │ │ +
    112 factorDims(factorDims), noiseModelDims(noiseModelDims) {}
    │ │ │ │ +
    113 ~InvalidNoiseModel() noexcept override {}
    │ │ │ │ +
    114
    │ │ │ │ +
    115 const char* what() const noexcept override;
    │ │ │ │ +
    116
    │ │ │ │ +
    117 private:
    │ │ │ │ +
    118 mutable std::string description_;
    │ │ │ │ +
    119 };
    │ │ │ │
    │ │ │ │
    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);
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    125 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ -
    Iterative methods, implementation.
    │ │ │ │ +
    129 InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
    │ │ │ │ +
    130 factorRows(factorRows), blockRows(blockRows) {}
    │ │ │ │ +
    131 ~InvalidMatrixBlock() noexcept override {}
    │ │ │ │ +
    132
    │ │ │ │ +
    133 const char* what() const noexcept override;
    │ │ │ │ +
    134
    │ │ │ │ +
    135 private:
    │ │ │ │ +
    136 mutable std::string description_;
    │ │ │ │ +
    137 };
    │ │ │ │ +
    │ │ │ │ +
    138
    │ │ │ │ +
    139 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    141 public:
    │ │ │ │ + │ │ │ │ +
    143 };
    │ │ │ │ +
    │ │ │ │ +
    144
    │ │ │ │ +
    145 }
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ -
    Definition iterative-inl.h:30
    │ │ │ │ -
    bool steepest
    flag to indicate we are doing steepest descent
    Definition iterative-inl.h:36
    │ │ │ │ -
    double threshold
    gamma (squared L2 norm of g) and convergence threshold
    Definition iterative-inl.h:38
    │ │ │ │ -
    int k
    iteration
    Definition iterative-inl.h:35
    │ │ │ │ -
    V d
    gradient g and search direction d for CG
    Definition iterative-inl.h:37
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    Definition ThreadsafeException.h:42
    │ │ │ │ +
    Thrown when a linear system is ill-posed.
    Definition linearExceptions.h:94
    │ │ │ │ +
    An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
    Definition linearExceptions.h:106
    │ │ │ │ +
    const DenseIndex factorDims
    The dimensionality of the factor.
    Definition linearExceptions.h:108
    │ │ │ │ +
    const DenseIndex noiseModelDims
    The dimensionality of the noise model.
    Definition linearExceptions.h:109
    │ │ │ │ +
    An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
    Definition linearExceptions.h:124
    │ │ │ │ +
    const DenseIndex factorRows
    The dimensionality of the factor.
    Definition linearExceptions.h:126
    │ │ │ │ +
    const DenseIndex blockRows
    The dimensionality of the noise model.
    Definition linearExceptions.h:127
    │ │ │ │ +
    Definition linearExceptions.h:140
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,206 +1,134 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -iterative-inl.h │ │ │ │ │ +linearExceptions.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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> │ │ │ │ │ -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 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#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#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ 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_; │ │ │ │ │ -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; │ │ │ │ │ +_9_4 class GTSAM_EXPORT _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 : public │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ +95 _K_e_y j_; │ │ │ │ │ +96 public: │ │ │ │ │ +97 _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(_K_e_y j) noexcept : j_(j) {} │ │ │ │ │ +98 _~_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() noexcept override {} │ │ │ │ │ +99 _K_e_y nearbyVariable() const { return j_; } │ │ │ │ │ +100 const char* what() const noexcept override; │ │ │ │ │ +101 }; │ │ │ │ │ 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 │ │ │ │ │ +103 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_1_0_6 class GTSAM_EXPORT _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l : public │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ +107 public: │ │ │ │ │ +_1_0_8 const _D_e_n_s_e_I_n_d_e_x _f_a_c_t_o_r_D_i_m_s; │ │ │ │ │ +_1_0_9 const _D_e_n_s_e_I_n_d_e_x _n_o_i_s_e_M_o_d_e_l_D_i_m_s; │ │ │ │ │ +110 │ │ │ │ │ +111 _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(_D_e_n_s_e_I_n_d_e_x factorDims, _D_e_n_s_e_I_n_d_e_x noiseModelDims) : │ │ │ │ │ +112 factorDims(factorDims), noiseModelDims(noiseModelDims) {} │ │ │ │ │ +113 _~_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l() noexcept override {} │ │ │ │ │ +114 │ │ │ │ │ +115 const char* what() const noexcept override; │ │ │ │ │ +116 │ │ │ │ │ +117 private: │ │ │ │ │ +118 mutable std::string description_; │ │ │ │ │ +119 }; │ │ │ │ │ 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); │ │ │ │ │ +_1_2_4 class GTSAM_EXPORT _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k : public │ │ │ │ │ +_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_M_a_t_r_i_x_B_l_o_c_k> { │ │ │ │ │ +125 public: │ │ │ │ │ +_1_2_6 const _D_e_n_s_e_I_n_d_e_x _f_a_c_t_o_r_R_o_w_s; │ │ │ │ │ +_1_2_7 const _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_R_o_w_s; │ │ │ │ │ 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. │ │ │ │ │ +129 _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x factorRows, _D_e_n_s_e_I_n_d_e_x blockRows) : │ │ │ │ │ +130 factorRows(factorRows), blockRows(blockRows) {} │ │ │ │ │ +131 _~_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k() noexcept override {} │ │ │ │ │ +132 │ │ │ │ │ +133 const char* what() const noexcept override; │ │ │ │ │ +134 │ │ │ │ │ +135 private: │ │ │ │ │ +136 mutable std::string description_; │ │ │ │ │ +137 }; │ │ │ │ │ +138 │ │ │ │ │ +139 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_1_4_0 class _I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n : public │ │ │ │ │ +_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_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n> { │ │ │ │ │ +141 public: │ │ │ │ │ +142 _I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n(const char *message) : │ │ │ │ │ +_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_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n_>(message) {} │ │ │ │ │ +143 }; │ │ │ │ │ +144 │ │ │ │ │ +145 } │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_:_:_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_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_:_:_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. │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:94 │ │ │ │ │ +_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_N_o_i_s_e_M_o_d_e_l_:_:_f_a_c_t_o_r_D_i_m_s │ │ │ │ │ +const DenseIndex factorDims │ │ │ │ │ +The dimensionality of the factor. │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l_:_:_n_o_i_s_e_M_o_d_e_l_D_i_m_s │ │ │ │ │ +const DenseIndex noiseModelDims │ │ │ │ │ +The dimensionality of the noise model. │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:109 │ │ │ │ │ +_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_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k_:_:_f_a_c_t_o_r_R_o_w_s │ │ │ │ │ +const DenseIndex factorRows │ │ │ │ │ +The dimensionality of the factor. │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k_:_:_b_l_o_c_k_R_o_w_s │ │ │ │ │ +const DenseIndex blockRows │ │ │ │ │ +The dimensionality of the noise model. │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:127 │ │ │ │ │ +_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 │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:140 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ + * _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/a00725.html │ │ │ │ @@ -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/linear/GaussianFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,63 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    RegularJacobianFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    GaussianFactorGraph.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    JacobianFactor class with fixed sized blcoks. │ │ │ │ +

    Linear Factor Graph where all factors are Gaussians. │ │ │ │ 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...
    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Functions

    bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
     Evaluates whether linear factors have any constrained noise models.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    JacobianFactor class with fixed sized blcoks.

    │ │ │ │ -
    Author
    Sungtae An
    │ │ │ │ -
    Date
    Nov 11, 2014
    │ │ │ │ +

    Linear Factor Graph where all factors are Gaussians.

    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_._._. │ │ │ │ │ +_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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ +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 ********** │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ Author │ │ │ │ │ - Sungtae An │ │ │ │ │ - Date │ │ │ │ │ - Nov 11, 2014 │ │ │ │ │ + Kai Ni │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00725.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00725 = [ │ │ │ │ │ - ["gtsam::RegularJacobianFactor< D >", "a04016.html", "a04016"] │ │ │ │ │ + ["gtsam::EliminationTraits< GaussianFactorGraph >", "a03788.html", "a03788"], │ │ │ │ │ + ["gtsam::GaussianFactorGraph", "a03792.html", "a03792"], │ │ │ │ │ + ["gtsam::traits< GaussianFactorGraph >", "a03796.html", null], │ │ │ │ │ + ["hasConstraints", "a00725.html#a35c269c3243cab16a7475239a9c91021", null] │ │ │ │ │ ]; │ │ │ ├── ./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/GaussianFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,215 +98,338 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    RegularJacobianFactor.h
    │ │ │ │ +
    GaussianFactorGraph.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    22#pragma once
    │ │ │ │
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    31template<size_t D>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    31
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │
    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:
    │ │ │ │ +
    34 // Forward declarations
    │ │ │ │ +
    35 class GaussianFactorGraph;
    │ │ │ │ +
    36 class GaussianFactor;
    │ │ │ │ + │ │ │ │ +
    38 class GaussianBayesNet;
    │ │ │ │ +
    39 class GaussianEliminationTree;
    │ │ │ │ +
    40 class GaussianBayesTree;
    │ │ │ │ +
    41 class GaussianJunctionTree;
    │ │ │ │
    42
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    65 const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
    │ │ │ │ -
    66 SharedDiagonal()) :
    │ │ │ │ -
    67 JacobianFactor(keys, augmentedMatrix, sigmas) {
    │ │ │ │ -
    68 }
    │ │ │ │ -
    │ │ │ │ -
    69
    │ │ │ │ - │ │ │ │ -
    71
    │ │ │ │ -
    │ │ │ │ -
    73 void multiplyHessianAdd(double alpha, const VectorValues& x,
    │ │ │ │ -
    74 VectorValues& y) const override {
    │ │ │ │ - │ │ │ │ -
    76 }
    │ │ │ │ -
    │ │ │ │ +
    43 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    45 {
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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); }
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    81 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    82
    │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    88
    │ │ │ │ +
    94 GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
    │ │ │ │ +
    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 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    106 template<class DERIVEDFACTOR>
    │ │ │ │ + │ │ │ │ +
    108
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ -
    │ │ │ │ -
    126
    │ │ │ │ -
    │ │ │ │ -
    128 VectorValues gradientAtZero() const override {
    │ │ │ │ - │ │ │ │ -
    130 }
    │ │ │ │
    │ │ │ │ +
    125
    │ │ │ │ +
    127 void add(const GaussianFactor& factor) { push_back(factor.clone()); }
    │ │ │ │ +
    128
    │ │ │ │ +
    130 void add(const sharedFactor& factor) { push_back(factor); }
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
    │ │ │ │ +
    313
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Variable elimination algorithms for factor graphs.
    │ │ │ │ +
    vector of errors
    │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ -
    VectorValues hessianDiagonal() const
    Return the diagonal of the Hessian for this factor.
    Definition GaussianFactor.cpp:35
    │ │ │ │ +
    bool hasConstraints(const GaussianFactorGraph &factors)
    Evaluates whether linear factors have any constrained noise models.
    Definition GaussianFactorGraph.cpp:442
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    bool isEqual(const FactorGraph &other) const
    Check exact equality of the factor pointers. Useful for derived ==.
    Definition FactorGraph.h:134
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ +
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ +
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ +
    Definition GaussianEliminationTree.h:29
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    virtual GaussianFactor::shared_ptr clone() const =0
    Clone a factor (make a deep copy)
    │ │ │ │ +
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition GaussianFactorGraph.h:58
    │ │ │ │ +
    GaussianBayesTree BayesTreeType
    Type of Bayes tree.
    Definition GaussianFactorGraph.h:51
    │ │ │ │ +
    GaussianConditional ConditionalType
    Type of conditionals from elimination.
    Definition GaussianFactorGraph.h:48
    │ │ │ │ +
    GaussianFactor FactorType
    Type of factors in factor graph.
    Definition GaussianFactorGraph.h:46
    │ │ │ │ +
    GaussianEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition GaussianFactorGraph.h:50
    │ │ │ │ +
    GaussianFactorGraph FactorGraphType
    Type of the factor graph (e.g. GaussianFactorGraph)
    Definition GaussianFactorGraph.h:47
    │ │ │ │ +
    GaussianBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition GaussianFactorGraph.h:49
    │ │ │ │ +
    GaussianJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition GaussianFactorGraph.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    EliminateableFactorGraph< This > BaseEliminateable
    Typedef to base elimination class.
    Definition GaussianFactorGraph.h:80
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Add an n-ary factor.
    Definition GaussianFactorGraph.h:156
    │ │ │ │ +
    GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
    Construct from an initializer lists of GaussianFactor shared pointers.
    Definition GaussianFactorGraph.h:94
    │ │ │ │ +
    GaussianFactorGraph()
    Default constructor.
    Definition GaussianFactorGraph.h:87
    │ │ │ │ +
    void add(const GaussianFactor &factor)
    Add a factor by value - makes a copy.
    Definition GaussianFactorGraph.h:127
    │ │ │ │ +
    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
    │ │ │ │ +
    void add(const sharedFactor &factor)
    Add a factor by pointer - stores pointer without copying the factor.
    Definition GaussianFactorGraph.h:130
    │ │ │ │ +
    friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
    Check exact equality.
    Definition GaussianFactorGraph.h:121
    │ │ │ │ +
    GaussianFactorGraph This
    Typedef to this class.
    Definition GaussianFactorGraph.h:78
    │ │ │ │ +
    KeySet Keys
    Return the set of variables involved in the factors (computes a set union).
    Definition GaussianFactorGraph.h:163
    │ │ │ │ +
    void add(const Vector &b)
    Add a null factor.
    Definition GaussianFactorGraph.h:133
    │ │ │ │ +
    void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Add a unary factor.
    Definition GaussianFactorGraph.h:137
    │ │ │ │ +
    virtual ~GaussianFactorGraph()
    Virtual destructor.
    Definition GaussianFactorGraph.h:110
    │ │ │ │ +
    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
    │ │ │ │ +
    GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition GaussianFactorGraph.h:99
    │ │ │ │ +
    FactorGraph< GaussianFactor > Base
    Typedef to base factor graph type.
    Definition GaussianFactorGraph.h:79
    │ │ │ │ +
    GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition GaussianFactorGraph.h:107
    │ │ │ │ +
    GaussianFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition GaussianFactorGraph.h:103
    │ │ │ │ +
    A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
    Definition GaussianJunctionTree.h:39
    │ │ │ │
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition JacobianFactor.cpp:649
    │ │ │ │ -
    VectorValues gradientAtZero() const override
    A'*b for Jacobian.
    Definition JacobianFactor.cpp:701
    │ │ │ │ -
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ -
    Vector operator*(const double *x) const
    double* Matrix-vector multiply, i.e.
    Definition RegularJacobianFactor.h:172
    │ │ │ │ -
    void hessianDiagonal(double *d) const override
    Raw memory access version of hessianDiagonal.
    Definition RegularJacobianFactor.h:109
    │ │ │ │ -
    void gradientAtZero(double *d) const override
    Raw memory access version of gradientAtZero.
    Definition RegularJacobianFactor.h:133
    │ │ │ │ -
    void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
    double* Transpose Matrix-vector multiply, i.e.
    Definition RegularJacobianFactor.h:161
    │ │ │ │ -
    VectorValues gradientAtZero() const override
    Expose base class gradientAtZero.
    Definition RegularJacobianFactor.h:128
    │ │ │ │ -
    RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Construct an n-ary factor.
    Definition RegularJacobianFactor.h:52
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition RegularJacobianFactor.h:73
    │ │ │ │ -
    RegularJacobianFactor()
    Default constructor.
    Definition RegularJacobianFactor.h:44
    │ │ │ │ -
    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
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const double *x, double *y) const
    double* Hessian-vector multiply, i.e.
    Definition RegularJacobianFactor.h:82
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    is the normalization constant.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,269 +1,493 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularJacobianFactor.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_/_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> │ │ │ │ │ +22#pragma once │ │ │ │ │ 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 { │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +_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 │ │ │ │ │ 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 } │ │ │ │ │ +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 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; │ │ │ │ │ +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 } │ │ │ │ │ -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 } │ │ │ │ │ +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 _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 │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ +_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. │ │ │ │ │ +_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. │ │ │ │ │ _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. │ │ │ │ │ +_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Errors.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,212 +98,95 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Preconditioner.h
    │ │ │ │ +
    Errors.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
    │ │ │ │ +
    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 typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
    │ │ │ │ +
    26#include <string>
    │ │ │ │
    27
    │ │ │ │ -
    28 enum Kernel { /* Preconditioner Kernel */
    │ │ │ │ -
    29 GTSAM = 0,
    │ │ │ │ -
    30 CHOLMOD /* experimental */
    │ │ │ │ -
    31 } kernel_ ;
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    30// Forward declarations
    │ │ │ │ +
    31class VectorValues;
    │ │ │ │
    32
    │ │ │ │ -
    33 enum Verbosity {
    │ │ │ │ -
    34 SILENT = 0,
    │ │ │ │ -
    35 COMPLEXITY = 1,
    │ │ │ │ -
    36 ERROR = 2
    │ │ │ │ -
    37 } verbosity_ ;
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ +
    37GTSAM_EXPORT Errors createErrors(const VectorValues& V);
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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 };
    │ │ │ │ +
    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};
    │ │ │ │
    │ │ │ │ -
    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 */
    │ │ │ │ - │ │ │ │ -
    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/*******************************************************************************************/
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    97 typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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/*******************************************************************************************/
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    128};
    │ │ │ │ -
    │ │ │ │ -
    129
    │ │ │ │ -
    130/*******************************************************************************************/
    │ │ │ │ -
    │ │ │ │ -
    131class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
    │ │ │ │ -
    132public:
    │ │ │ │ -
    133 typedef Preconditioner Base;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    73} // namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ -
    Definition Preconditioner.h:24
    │ │ │ │ -
    Definition Preconditioner.h:64
    │ │ │ │ -
    virtual void solve(const Vector &y, Vector &x) const =0
    implement x = L^{-1} y
    │ │ │ │ -
    virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda)=0
    build/factorize the preconditioner
    │ │ │ │ -
    virtual void transposeSolve(const Vector &y, Vector &x) const =0
    implement x = L^{-T} y
    │ │ │ │ -
    Definition Preconditioner.h:95
    │ │ │ │ -
    Definition Preconditioner.h:103
    │ │ │ │ -
    void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
    build/factorize the preconditioner
    Definition Preconditioner.h:116
    │ │ │ │ -
    void solve(const Vector &y, Vector &x) const override
    implement x = L^{-1} y
    Definition Preconditioner.h:114
    │ │ │ │ -
    void transposeSolve(const Vector &y, Vector &x) const override
    implement x = L^{-T} y
    Definition Preconditioner.h:115
    │ │ │ │ -
    Definition Preconditioner.h:124
    │ │ │ │ -
    Definition Preconditioner.h:131
    │ │ │ │ +
    Errors operator+(const Errors &a, const Errors &b)
    Addition.
    Definition Errors.cpp:60
    │ │ │ │ +
    void axpy(double alpha, const Errors &x, Errors &y)
    BLAS level 2 style AXPY, y := alpha*x + y
    Definition Errors.cpp:111
    │ │ │ │ +
    Errors createErrors(const VectorValues &V)
    Break V into pieces according to its start indices.
    Definition Errors.cpp:29
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    Errors operator-(const Errors &a, const Errors &b)
    Subtraction.
    Definition Errors.cpp:75
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ + │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,115 @@ │ │ │ │ │ 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 { │ │ │ │ │ +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, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +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_/_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 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +26#include │ │ │ │ │ 27 │ │ │ │ │ -28 enum Kernel { /* Preconditioner Kernel */ │ │ │ │ │ -29 GTSAM = 0, │ │ │ │ │ -30 CHOLMOD /* experimental */ │ │ │ │ │ -31 } kernel_ ; │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Forward declarations │ │ │ │ │ +31class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ 32 │ │ │ │ │ -33 enum Verbosity { │ │ │ │ │ -34 SILENT = 0, │ │ │ │ │ -35 COMPLEXITY = 1, │ │ │ │ │ -36 ERROR = 2 │ │ │ │ │ -37 } verbosity_ ; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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() {} │ │ │ │ │ +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 /* │ │ │ │ │ -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: │ │ │ │ │ -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; │ │ │ │ │ -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); │ │ │ │ │ -159 │ │ │ │ │ -160} │ │ │ │ │ -161 │ │ │ │ │ -162 │ │ │ │ │ +73} // namespace gtsam │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * PPrreeccoonnddiittiioonneerr..hh │ │ │ │ │ + * _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/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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    27 size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) :
    │ │ │ │ -
    28 BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
    │ │ │ │ -
    │ │ │ │ -
    29
    │ │ │ │ -
    30 /* ************************************************************************* */
    │ │ │ │ -
    31 template<typename KEYS>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    GaussianConditional()
    default constructor needed for serialization
    Definition GaussianConditional.h:54
    │ │ │ │ -
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition GaussianConditional.h:48
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Pure virtual class for all robust error function classes.
    Definition LossFunctions.h:63
    │ │ │ │ +
    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
    │ │ │ │ +
    ReweightScheme reweight_
    Strategy for reweighting.
    Definition LossFunctions.h:72
    │ │ │ │ +
    ReweightScheme reweightScheme() const
    Returns the reweight scheme, as explained in ReweightScheme.
    Definition LossFunctions.h:79
    │ │ │ │ +
    virtual double weight(double distance) const =0
    This method is responsible for returning the weight function for a given amount of error.
    │ │ │ │ +
    ReweightScheme
    the rows can be weighted independently according to the error or uniformly with the norm of the right...
    Definition LossFunctions.h:67
    │ │ │ │ +
    "Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain least-squares (non-ro...
    Definition LossFunctions.h:146
    │ │ │ │ +
    double weight(double) const override
    This method is responsible for returning the weight function for a given amount of error.
    Definition LossFunctions.h:152
    │ │ │ │ +
    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
    │ │ │ │ +
    Implementation of the "Fair" robust error model (Zhang97ivc)
    Definition LossFunctions.h:175
    │ │ │ │ +
    The "Huber" robust error model (Zhang97ivc).
    Definition LossFunctions.h:208
    │ │ │ │ +
    Implementation of the "Cauchy" robust error model (Lee2013IROS).
    Definition LossFunctions.h:246
    │ │ │ │ +
    Implementation of the "Tukey" robust error model (Zhang97ivc).
    Definition LossFunctions.h:280
    │ │ │ │ +
    Implementation of the "Welsch" robust error model (Zhang97ivc).
    Definition LossFunctions.h:313
    │ │ │ │ +
    Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
    Definition LossFunctions.h:349
    │ │ │ │ +
    DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
    Definition LossFunctions.h:387
    │ │ │ │ +
    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/GaussianEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,42 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    NoiseModel.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianEliminationTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::GaussianEliminationTree
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

    │ │ │ │ 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
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +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 │ │ │ │ │ -NoiseModel.cpp File Reference │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._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/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/SubgraphBuilder.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianBayesNet.cpp File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    SubgraphBuilder.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ -More...

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

    │ │ │ │ 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, GaussianFactorGraphgtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
     Split the graph into a subgraph and the remaining edges.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Chordal Bayes Net, the result of eliminating a factor graph.

    │ │ │ │ -
    Author
    Frank Dellaert, Varun Agrawal
    │ │ │ │ +
    Date
    Dec 31, 2009
    │ │ │ │ +
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,42 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 ********** │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ + Date │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Varun Agrawal │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00749.html │ │ │ │ @@ -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/linear/Scatter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,51 +96,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    linearExceptions.h File Reference
    │ │ │ │ +
    Scatter.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Exceptions that may be thrown by linear solver components. │ │ │ │ +

    Maps global variable indices to slot indices. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::IndeterminantLinearSystemException
     Thrown when a linear system is ill-posed. More...
    struct  gtsam::SlotEntry
     One SlotEntry stores the slot index for a variable, as well its dim. 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
    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

    │ │ │ │ -

    Exceptions that may be thrown by linear solver components.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Aug 17, 2012
    │ │ │ │ +

    Maps global variable indices to slot indices.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    June 2015
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +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 │ │ │ │ │ -linearExceptions.h File Reference │ │ │ │ │ -Exceptions that may be thrown by linear solver components. _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_:_:_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_._._. │ │ │ │ │ +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_:_:_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 │ │ │ │ │ + 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 ********** │ │ │ │ │ -Exceptions that may be thrown by linear solver components. │ │ │ │ │ +Maps global variable indices to slot indices. │ │ │ │ │ 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_._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/a00749.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,4 @@ │ │ │ │ │ var a00749 = [ │ │ │ │ │ - ["gtsam::IndeterminantLinearSystemException", "a03864.html", null], │ │ │ │ │ - ["gtsam::InvalidNoiseModel", "a03868.html", "a03868"], │ │ │ │ │ - ["gtsam::InvalidMatrixBlock", "a03872.html", "a03872"], │ │ │ │ │ - ["gtsam::InvalidDenseElimination", "a03876.html", null] │ │ │ │ │ + ["gtsam::SlotEntry", "a04024.html", null], │ │ │ │ │ + ["gtsam::Scatter", "a04028.html", "a04028"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00749_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearExceptions.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,109 +98,84 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    linearExceptions.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ -
    21#include <gtsam/base/types.h>
    │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    │ │ │ │ -
    94 class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
    │ │ │ │ -
    95 Key j_;
    │ │ │ │ -
    96 public:
    │ │ │ │ -
    97 IndeterminantLinearSystemException(Key j) noexcept : j_(j) {}
    │ │ │ │ -
    98 ~IndeterminantLinearSystemException() noexcept override {}
    │ │ │ │ -
    99 Key nearbyVariable() const { return j_; }
    │ │ │ │ -
    100 const char* what() const noexcept override;
    │ │ │ │ -
    101 };
    │ │ │ │ -
    │ │ │ │ -
    102
    │ │ │ │ -
    103 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    106 class GTSAM_EXPORT InvalidNoiseModel : public ThreadsafeException<InvalidNoiseModel> {
    │ │ │ │ -
    107 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    110
    │ │ │ │ -
    111 InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) :
    │ │ │ │ -
    112 factorDims(factorDims), noiseModelDims(noiseModelDims) {}
    │ │ │ │ -
    113 ~InvalidNoiseModel() noexcept override {}
    │ │ │ │ -
    114
    │ │ │ │ -
    115 const char* what() const noexcept override;
    │ │ │ │ -
    116
    │ │ │ │ -
    117 private:
    │ │ │ │ -
    118 mutable std::string description_;
    │ │ │ │ -
    119 };
    │ │ │ │ -
    │ │ │ │ -
    120
    │ │ │ │ -
    121 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    125 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    128
    │ │ │ │ -
    129 InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
    │ │ │ │ -
    130 factorRows(factorRows), blockRows(blockRows) {}
    │ │ │ │ -
    131 ~InvalidMatrixBlock() noexcept override {}
    │ │ │ │ -
    132
    │ │ │ │ -
    133 const char* what() const noexcept override;
    │ │ │ │ -
    134
    │ │ │ │ -
    135 private:
    │ │ │ │ -
    136 mutable std::string description_;
    │ │ │ │ -
    137 };
    │ │ │ │ -
    │ │ │ │ -
    138
    │ │ │ │ -
    139 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    141 public:
    │ │ │ │ - │ │ │ │ -
    143 };
    │ │ │ │ -
    │ │ │ │ -
    144
    │ │ │ │ -
    145 }
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ +
    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
    │ │ │ │ +
    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} // \ namespace gtsam
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    Definition ThreadsafeException.h:42
    │ │ │ │ -
    Thrown when a linear system is ill-posed.
    Definition linearExceptions.h:94
    │ │ │ │ -
    An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
    Definition linearExceptions.h:106
    │ │ │ │ -
    const DenseIndex factorDims
    The dimensionality of the factor.
    Definition linearExceptions.h:108
    │ │ │ │ -
    const DenseIndex noiseModelDims
    The dimensionality of the noise model.
    Definition linearExceptions.h:109
    │ │ │ │ -
    An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
    Definition linearExceptions.h:124
    │ │ │ │ -
    const DenseIndex factorRows
    The dimensionality of the factor.
    Definition linearExceptions.h:126
    │ │ │ │ -
    const DenseIndex blockRows
    The dimensionality of the noise model.
    Definition linearExceptions.h:127
    │ │ │ │ -
    Definition linearExceptions.h:140
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    One SlotEntry stores the slot index for a variable, as well its dim.
    Definition Scatter.h:32
    │ │ │ │ +
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ +
    GTSAM_EXPORT void add(Key key, size_t dim)
    Add a key/dim pair.
    Definition Scatter.cpp:76
    │ │ │ │ +
    GTSAM_EXPORT Scatter()
    Default Constructor.
    Definition Scatter.h:52
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,134 +1,97 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -linearExceptions.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#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#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -_9_4 class GTSAM_EXPORT _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 : public │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ -95 _K_e_y j_; │ │ │ │ │ -96 public: │ │ │ │ │ -97 _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(_K_e_y j) noexcept : j_(j) {} │ │ │ │ │ -98 _~_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() noexcept override {} │ │ │ │ │ -99 _K_e_y nearbyVariable() const { return j_; } │ │ │ │ │ -100 const char* what() const noexcept override; │ │ │ │ │ -101 }; │ │ │ │ │ -102 │ │ │ │ │ -103 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_0_6 class GTSAM_EXPORT _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l : public │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ -107 public: │ │ │ │ │ -_1_0_8 const _D_e_n_s_e_I_n_d_e_x _f_a_c_t_o_r_D_i_m_s; │ │ │ │ │ -_1_0_9 const _D_e_n_s_e_I_n_d_e_x _n_o_i_s_e_M_o_d_e_l_D_i_m_s; │ │ │ │ │ -110 │ │ │ │ │ -111 _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(_D_e_n_s_e_I_n_d_e_x factorDims, _D_e_n_s_e_I_n_d_e_x noiseModelDims) : │ │ │ │ │ -112 factorDims(factorDims), noiseModelDims(noiseModelDims) {} │ │ │ │ │ -113 _~_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l() noexcept override {} │ │ │ │ │ -114 │ │ │ │ │ -115 const char* what() const noexcept override; │ │ │ │ │ -116 │ │ │ │ │ -117 private: │ │ │ │ │ -118 mutable std::string description_; │ │ │ │ │ -119 }; │ │ │ │ │ -120 │ │ │ │ │ -121 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_2_4 class GTSAM_EXPORT _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k : public │ │ │ │ │ -_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_M_a_t_r_i_x_B_l_o_c_k> { │ │ │ │ │ -125 public: │ │ │ │ │ -_1_2_6 const _D_e_n_s_e_I_n_d_e_x _f_a_c_t_o_r_R_o_w_s; │ │ │ │ │ -_1_2_7 const _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_R_o_w_s; │ │ │ │ │ -128 │ │ │ │ │ -129 _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x factorRows, _D_e_n_s_e_I_n_d_e_x blockRows) : │ │ │ │ │ -130 factorRows(factorRows), blockRows(blockRows) {} │ │ │ │ │ -131 _~_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k() noexcept override {} │ │ │ │ │ -132 │ │ │ │ │ -133 const char* what() const noexcept override; │ │ │ │ │ -134 │ │ │ │ │ -135 private: │ │ │ │ │ -136 mutable std::string description_; │ │ │ │ │ -137 }; │ │ │ │ │ -138 │ │ │ │ │ -139 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_4_0 class _I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n : public │ │ │ │ │ -_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_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n> { │ │ │ │ │ -141 public: │ │ │ │ │ -142 _I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n(const char *message) : │ │ │ │ │ -_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_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n_>(message) {} │ │ │ │ │ -143 }; │ │ │ │ │ -144 │ │ │ │ │ -145 } │ │ │ │ │ -_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. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +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 │ │ │ │ │ +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} // \ 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_:_:_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_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_:_:_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. │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:94 │ │ │ │ │ -_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_N_o_i_s_e_M_o_d_e_l_:_:_f_a_c_t_o_r_D_i_m_s │ │ │ │ │ -const DenseIndex factorDims │ │ │ │ │ -The dimensionality of the factor. │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l_:_:_n_o_i_s_e_M_o_d_e_l_D_i_m_s │ │ │ │ │ -const DenseIndex noiseModelDims │ │ │ │ │ -The dimensionality of the noise model. │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:109 │ │ │ │ │ -_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_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k_:_:_f_a_c_t_o_r_R_o_w_s │ │ │ │ │ -const DenseIndex factorRows │ │ │ │ │ -The dimensionality of the factor. │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k_:_:_b_l_o_c_k_R_o_w_s │ │ │ │ │ -const DenseIndex blockRows │ │ │ │ │ -The dimensionality of the noise model. │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:127 │ │ │ │ │ -_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 │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:140 │ │ │ │ │ +_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 │ │ │ │ │ - * _l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._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/a00752.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,57 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    HessianFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianDensity.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Contains the HessianFactor class, a general quadratic factor. │ │ │ │ +

    A Gaussian Density. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::HessianFactor
     A Gaussian factor using the canonical parameters (information form) More...
     
    struct  gtsam::traits< HessianFactor >
     traits More...
    class  gtsam::GaussianDensity
     A GaussianDensity is a GaussianConditional without parents. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ Namespaces

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

    │ │ │ │ -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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Contains the HessianFactor class, a general quadratic factor.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Dec 8, 2010
    │ │ │ │ +

    A Gaussian Density.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Jan 21, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,44 +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 │ │ │ │ │ -HessianFactor.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 │ │ │ │ │ +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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -  A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_e_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_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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ -  │ │ │ │ │ - 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 ********** │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +A Gaussian Density. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ + Jan 21, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00752_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,283 +98,92 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HessianFactor.h
    │ │ │ │ +
    GaussianDensity.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    19// \callgraph
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │ -
    26#include <boost/make_shared.hpp>
    │ │ │ │ -
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ -
    29
    │ │ │ │ -
    30 // Forward declarations
    │ │ │ │ -
    31 class Ordering;
    │ │ │ │ -
    32 class JacobianFactor;
    │ │ │ │ -
    33 class HessianFactor;
    │ │ │ │ - │ │ │ │ -
    35 class GaussianBayesNet;
    │ │ │ │ -
    36 class GaussianFactorGraph;
    │ │ │ │ +
    │ │ │ │ +
    32 class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
    │ │ │ │ +
    33
    │ │ │ │ +
    34 public:
    │ │ │ │ +
    35
    │ │ │ │ +
    36 typedef boost::shared_ptr<GaussianDensity> shared_ptr;
    │ │ │ │
    37
    │ │ │ │ -
    │ │ │ │ -
    101 class GTSAM_EXPORT HessianFactor : public GaussianFactor {
    │ │ │ │ -
    102 protected:
    │ │ │ │ -
    103
    │ │ │ │ - │ │ │ │ -
    105
    │ │ │ │ -
    106 public:
    │ │ │ │ -
    107
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    110 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    111 typedef SymmetricBlockMatrix::Block Block;
    │ │ │ │ -
    112 typedef SymmetricBlockMatrix::constBlock constBlock;
    │ │ │ │ -
    113
    │ │ │ │ -
    114
    │ │ │ │ - │ │ │ │ -
    117
    │ │ │ │ -
    123 HessianFactor(Key j, const Matrix& G, const Vector& g, double f);
    │ │ │ │ -
    124
    │ │ │ │ -
    128 HessianFactor(Key j, const Vector& mu, const Matrix& Sigma);
    │ │ │ │ -
    129
    │ │ │ │ -
    145 HessianFactor(Key j1, Key j2,
    │ │ │ │ -
    146 const Matrix& G11, const Matrix& G12, const Vector& g1,
    │ │ │ │ -
    147 const Matrix& G22, const Vector& g2, double f);
    │ │ │ │ -
    148
    │ │ │ │ -
    153 HessianFactor(Key j1, Key j2, Key j3,
    │ │ │ │ -
    154 const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
    │ │ │ │ -
    155 const Matrix& G22, const Matrix& G23, const Vector& g2,
    │ │ │ │ -
    156 const Matrix& G33, const Vector& g3, double f);
    │ │ │ │ -
    157
    │ │ │ │ -
    162 HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
    │ │ │ │ -
    163 const std::vector<Vector>& gs, double f);
    │ │ │ │ -
    164
    │ │ │ │ -
    167 template<typename KEYS>
    │ │ │ │ -
    168 HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation);
    │ │ │ │ -
    169
    │ │ │ │ -
    171 explicit HessianFactor(const JacobianFactor& cg);
    │ │ │ │ -
    172
    │ │ │ │ -
    175 explicit HessianFactor(const GaussianFactor& factor);
    │ │ │ │ -
    176
    │ │ │ │ -
    178 explicit HessianFactor(const GaussianFactorGraph& factors,
    │ │ │ │ -
    179 const Scatter& scatter);
    │ │ │ │ -
    180
    │ │ │ │ -
    │ │ │ │ -
    182 explicit HessianFactor(const GaussianFactorGraph& factors)
    │ │ │ │ -
    183 : HessianFactor(factors, Scatter(factors)) {}
    │ │ │ │ -
    │ │ │ │ -
    184
    │ │ │ │ -
    186 ~HessianFactor() override {}
    │ │ │ │ -
    187
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    190 return boost::make_shared<HessianFactor>(*this); }
    │ │ │ │ -
    │ │ │ │ -
    191
    │ │ │ │ -
    193 void print(const std::string& s = "",
    │ │ │ │ -
    194 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
    │ │ │ │ -
    195
    │ │ │ │ -
    197 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
    │ │ │ │ -
    198
    │ │ │ │ -
    200 using GaussianFactor::error;
    │ │ │ │ -
    201
    │ │ │ │ -
    206 double error(const VectorValues& c) const override;
    │ │ │ │ -
    207
    │ │ │ │ -
    │ │ │ │ -
    213 DenseIndex getDim(const_iterator variable) const override {
    │ │ │ │ -
    214 return info_.getDim(std::distance(begin(), variable));
    │ │ │ │ -
    215 }
    │ │ │ │ -
    │ │ │ │ -
    216
    │ │ │ │ -
    218 size_t rows() const { return info_.rows(); }
    │ │ │ │ -
    219
    │ │ │ │ -
    225 GaussianFactor::shared_ptr negate() const override;
    │ │ │ │ -
    226
    │ │ │ │ -
    │ │ │ │ -
    230 double constantTerm() const {
    │ │ │ │ -
    231 const auto view = info_.diagonalBlock(size());
    │ │ │ │ -
    232 return view(0, 0);
    │ │ │ │ -
    233 }
    │ │ │ │ -
    │ │ │ │ -
    234
    │ │ │ │ -
    238 double& constantTerm() { return info_.diagonalBlock(size())(0, 0); }
    │ │ │ │ -
    239
    │ │ │ │ -
    │ │ │ │ -
    244 SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
    │ │ │ │ -
    245 assert(!empty());
    │ │ │ │ -
    246 return info_.aboveDiagonalBlock(j - begin(), size());
    │ │ │ │ -
    247 }
    │ │ │ │ -
    │ │ │ │ -
    248
    │ │ │ │ -
    │ │ │ │ -
    251 SymmetricBlockMatrix::constBlock linearTerm() const {
    │ │ │ │ -
    252 assert(!empty());
    │ │ │ │ -
    253 // get the last column (except the bottom right block)
    │ │ │ │ -
    254 return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    │ │ │ │ -
    255 }
    │ │ │ │ -
    │ │ │ │ -
    256
    │ │ │ │ -
    │ │ │ │ -
    259 SymmetricBlockMatrix::Block linearTerm() {
    │ │ │ │ -
    260 assert(!empty());
    │ │ │ │ -
    261 return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    │ │ │ │ -
    262 }
    │ │ │ │ -
    │ │ │ │ -
    263
    │ │ │ │ -
    265 const SymmetricBlockMatrix& info() const { return info_; }
    │ │ │ │ -
    266
    │ │ │ │ -
    269 SymmetricBlockMatrix& info() { return info_; }
    │ │ │ │ -
    270
    │ │ │ │ -
    286 Matrix augmentedInformation() const override;
    │ │ │ │ -
    287
    │ │ │ │ -
    289 Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper> informationView() const;
    │ │ │ │ -
    290
    │ │ │ │ -
    294 Matrix information() const override;
    │ │ │ │ -
    295
    │ │ │ │ -
    297 void hessianDiagonalAdd(VectorValues& d) const override;
    │ │ │ │ -
    298
    │ │ │ │ -
    300 using Base::hessianDiagonal;
    │ │ │ │ -
    301
    │ │ │ │ -
    303 void hessianDiagonal(double* d) const override;
    │ │ │ │ -
    304
    │ │ │ │ -
    306 std::map<Key,Matrix> hessianBlockDiagonal() const override;
    │ │ │ │ -
    307
    │ │ │ │ -
    309 std::pair<Matrix, Vector> jacobian() const override;
    │ │ │ │ -
    310
    │ │ │ │ -
    316 Matrix augmentedJacobian() const override;
    │ │ │ │ -
    317
    │ │ │ │ -
    323 void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
    │ │ │ │ -
    324
    │ │ │ │ -
    │ │ │ │ -
    328 void updateHessian(HessianFactor* other) const {
    │ │ │ │ -
    329 assert(other);
    │ │ │ │ -
    330 updateHessian(other->keys_, &other->info_);
    │ │ │ │ -
    331 }
    │ │ │ │ -
    │ │ │ │ -
    332
    │ │ │ │ -
    334 void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const override;
    │ │ │ │ -
    335
    │ │ │ │ -
    337 VectorValues gradientAtZero() const override;
    │ │ │ │ -
    338
    │ │ │ │ -
    340 void gradientAtZero(double* d) const override;
    │ │ │ │ -
    341
    │ │ │ │ -
    346 Vector gradient(Key key, const VectorValues& x) const override;
    │ │ │ │ -
    347
    │ │ │ │ -
    352 boost::shared_ptr<GaussianConditional> eliminateCholesky(const Ordering& keys);
    │ │ │ │ -
    353
    │ │ │ │ -
    355 VectorValues solve();
    │ │ │ │ -
    356
    │ │ │ │ -
    357 private:
    │ │ │ │ -
    359 void Allocate(const Scatter& scatter);
    │ │ │ │ -
    360
    │ │ │ │ -
    362 HessianFactor(const Scatter& scatter);
    │ │ │ │ -
    363
    │ │ │ │ -
    364 friend class NonlinearFactorGraph;
    │ │ │ │ -
    365 friend class NonlinearClusterTree;
    │ │ │ │ -
    366
    │ │ │ │ -
    368 friend class boost::serialization::access;
    │ │ │ │ -
    369 template<class ARCHIVE>
    │ │ │ │ -
    370 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    371 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
    │ │ │ │ -
    372 ar & BOOST_SERIALIZATION_NVP(info_);
    │ │ │ │ -
    373 }
    │ │ │ │ -
    374 };
    │ │ │ │ -
    │ │ │ │ -
    375
    │ │ │ │ -
    392GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
    │ │ │ │ -
    393 EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
    │ │ │ │ -
    394
    │ │ │ │ -
    410GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
    │ │ │ │ -
    411 EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
    │ │ │ │ -
    412
    │ │ │ │ -
    414template<>
    │ │ │ │ -
    415struct traits<HessianFactor> : public Testable<HessianFactor> {};
    │ │ │ │ -
    416
    │ │ │ │ -
    417} // \ namespace gtsam
    │ │ │ │ -
    418
    │ │ │ │ -
    419
    │ │ │ │ - │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ -
    Maps global variable indices to slot indices.
    │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    41 }
    │ │ │ │ +
    │ │ │ │ +
    42
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    Point3 mean(const CONTAINER &points)
    mean
    Definition Point3.h:68
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    DenseIndex getDim(DenseIndex block) const
    Number of dimensions for variable on this diagonal block.
    Definition SymmetricBlockMatrix.h:123
    │ │ │ │ -
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition SymmetricBlockMatrix.h:114
    │ │ │ │ -
    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
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ -
    HessianFactor This
    Typedef to this class.
    Definition HessianFactor.h:109
    │ │ │ │ -
    void updateHessian(HessianFactor *other) const
    Update another Hessian factor.
    Definition HessianFactor.h:328
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    A shared_ptr to this class.
    Definition HessianFactor.h:110
    │ │ │ │ -
    GaussianFactor::shared_ptr clone() const override
    Clone this HessianFactor.
    Definition HessianFactor.h:189
    │ │ │ │ -
    const SymmetricBlockMatrix & info() const
    Return underlying information matrix.
    Definition HessianFactor.h:265
    │ │ │ │ -
    SymmetricBlockMatrix::Block linearTerm()
    Return the complete linear term as described above.
    Definition HessianFactor.h:259
    │ │ │ │ -
    double constantTerm() const
    Return the constant term as described above.
    Definition HessianFactor.h:230
    │ │ │ │ -
    SymmetricBlockMatrix::constBlock linearTerm() const
    Return the complete linear term as described above.
    Definition HessianFactor.h:251
    │ │ │ │ -
    ~HessianFactor() override
    Destructor.
    Definition HessianFactor.h:186
    │ │ │ │ -
    HessianFactor(const GaussianFactorGraph &factors)
    Combine a set of factors into a single dense HessianFactor.
    Definition HessianFactor.h:182
    │ │ │ │ -
    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 HessianFactor.h:213
    │ │ │ │ -
    SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const
    Return the part of linear term as described above corresponding to the requested variable.
    Definition HessianFactor.h:244
    │ │ │ │ -
    SymmetricBlockMatrix info_
    The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
    Definition HessianFactor.h:104
    │ │ │ │ -
    GaussianFactor Base
    Typedef to base class.
    Definition HessianFactor.h:108
    │ │ │ │ -
    size_t rows() const
    Return the number of columns and rows of the Hessian matrix, including the information vector.
    Definition HessianFactor.h:218
    │ │ │ │ -
    SymmetricBlockMatrix::constBlock constBlock
    A block from the Hessian matrix (const version)
    Definition HessianFactor.h:112
    │ │ │ │ -
    SymmetricBlockMatrix & info()
    Return non-const information matrix.
    Definition HessianFactor.h:269
    │ │ │ │ -
    SymmetricBlockMatrix::Block Block
    A block from the Hessian matrix.
    Definition HessianFactor.h:111
    │ │ │ │ -
    double & constantTerm()
    Return the constant term as described above.
    Definition HessianFactor.h:238
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ -
    is the normalization constant.
    │ │ │ │ +
    size_t nrParents() const
    return the number of parents
    Definition Conditional.h:113
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ +
    A GaussianDensity is a GaussianConditional without parents.
    Definition GaussianDensity.h:32
    │ │ │ │ +
    GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal &noiseModel=SharedDiagonal())
    constructor using d, R
    Definition GaussianDensity.h:51
    │ │ │ │ +
    GaussianDensity()
    default constructor needed for serialization
    Definition GaussianDensity.h:39
    │ │ │ │ +
    GaussianDensity(const GaussianConditional &conditional)
    Copy constructor from GaussianConditional.
    Definition GaussianDensity.h:44
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,413 +1,110 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HessianFactor.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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_/_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_/_S_c_a_t_t_e_r_._h> │ │ │ │ │ -23#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> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +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 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30 // Forward declarations │ │ │ │ │ -31 class Ordering; │ │ │ │ │ -32 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ -33 class HessianFactor; │ │ │ │ │ -34 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -35 class GaussianBayesNet; │ │ │ │ │ -36 class GaussianFactorGraph; │ │ │ │ │ +_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 │ │ │ │ │ -_1_0_1 class GTSAM_EXPORT _H_e_s_s_i_a_n_F_a_c_t_o_r : public _G_a_u_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -102 protected: │ │ │ │ │ -103 │ │ │ │ │ -_1_0_4 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _i_n_f_o__; │ │ │ │ │ -105 │ │ │ │ │ -106 public: │ │ │ │ │ -107 │ │ │ │ │ -_1_0_8 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -_1_0_9 typedef _H_e_s_s_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_1_1_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_1_1_1 typedef SymmetricBlockMatrix::Block _B_l_o_c_k; │ │ │ │ │ -_1_1_2 typedef SymmetricBlockMatrix::constBlock _c_o_n_s_t_B_l_o_c_k; │ │ │ │ │ -113 │ │ │ │ │ -114 │ │ │ │ │ -116 _H_e_s_s_i_a_n_F_a_c_t_o_r(); │ │ │ │ │ -117 │ │ │ │ │ -123 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j, const Matrix& G, const Vector& g, double f); │ │ │ │ │ -124 │ │ │ │ │ -128 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j, const Vector& mu, const Matrix& Sigma); │ │ │ │ │ -129 │ │ │ │ │ -145 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, │ │ │ │ │ -146 const Matrix& G11, const Matrix& G12, const Vector& g1, │ │ │ │ │ -147 const Matrix& G22, const Vector& g2, double f); │ │ │ │ │ -148 │ │ │ │ │ -153 _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, │ │ │ │ │ -154 const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, │ │ │ │ │ -155 const Matrix& G22, const Matrix& G23, const Vector& g2, │ │ │ │ │ -156 const Matrix& G33, const Vector& g3, double f); │ │ │ │ │ -157 │ │ │ │ │ -162 _H_e_s_s_i_a_n_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& js, const std::vector& Gs, │ │ │ │ │ -163 const std::vector& gs, double f); │ │ │ │ │ -164 │ │ │ │ │ -167 template │ │ │ │ │ -168 _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); │ │ │ │ │ -169 │ │ │ │ │ -171 explicit _H_e_s_s_i_a_n_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& cg); │ │ │ │ │ -172 │ │ │ │ │ -175 explicit _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& factor); │ │ │ │ │ -176 │ │ │ │ │ -178 explicit _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, │ │ │ │ │ -179 const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 explicit _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) │ │ │ │ │ -183 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors, _S_c_a_t_t_e_r(factors)) {} │ │ │ │ │ -184 │ │ │ │ │ -_1_8_6 _~_H_e_s_s_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::make_shared(*this); } │ │ │ │ │ -191 │ │ │ │ │ -193 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -194 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -195 │ │ │ │ │ -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 │ │ │ │ │ -200 using GaussianFactor::error; │ │ │ │ │ -201 │ │ │ │ │ -206 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const override; │ │ │ │ │ -207 │ │ │ │ │ -_2_1_3 _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 { │ │ │ │ │ -214 return info_._g_e_t_D_i_m(std::distance(begin(), variable)); │ │ │ │ │ -215 } │ │ │ │ │ -216 │ │ │ │ │ -_2_1_8 size_t _r_o_w_s() const { return info_._r_o_w_s(); } │ │ │ │ │ -219 │ │ │ │ │ -225 _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; │ │ │ │ │ -226 │ │ │ │ │ -_2_3_0 double _c_o_n_s_t_a_n_t_T_e_r_m() const { │ │ │ │ │ -231 const auto view = info_._d_i_a_g_o_n_a_l_B_l_o_c_k(size()); │ │ │ │ │ -232 return view(0, 0); │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -_2_3_8 double& _c_o_n_s_t_a_n_t_T_e_r_m() { return info_._d_i_a_g_o_n_a_l_B_l_o_c_k(size())(0, 0); } │ │ │ │ │ -239 │ │ │ │ │ -_2_4_4 SymmetricBlockMatrix::constBlock _l_i_n_e_a_r_T_e_r_m(_c_o_n_s_t___i_t_e_r_a_t_o_r j) const { │ │ │ │ │ -245 assert(!empty()); │ │ │ │ │ -246 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j - begin(), size()); │ │ │ │ │ -247 } │ │ │ │ │ -248 │ │ │ │ │ -_2_5_1 SymmetricBlockMatrix::constBlock _l_i_n_e_a_r_T_e_r_m() const { │ │ │ │ │ -252 assert(!empty()); │ │ │ │ │ -253 // get the last column (except the bottom right block) │ │ │ │ │ -254 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(0, size(), size(), size() + 1); │ │ │ │ │ -255 } │ │ │ │ │ -256 │ │ │ │ │ -_2_5_9 SymmetricBlockMatrix::Block _l_i_n_e_a_r_T_e_r_m() { │ │ │ │ │ -260 assert(!empty()); │ │ │ │ │ -261 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(0, size(), size(), size() + 1); │ │ │ │ │ -262 } │ │ │ │ │ -263 │ │ │ │ │ -_2_6_5 const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _i_n_f_o() const { return info_; } │ │ │ │ │ -266 │ │ │ │ │ -_2_6_9 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _i_n_f_o() { return info_; } │ │ │ │ │ -270 │ │ │ │ │ -286 Matrix augmentedInformation() const override; │ │ │ │ │ -287 │ │ │ │ │ -289 Eigen::SelfAdjointView │ │ │ │ │ -informationView() const; │ │ │ │ │ -290 │ │ │ │ │ -294 Matrix information() const override; │ │ │ │ │ -295 │ │ │ │ │ -297 void hessianDiagonalAdd(_V_e_c_t_o_r_V_a_l_u_e_s& d) const override; │ │ │ │ │ -298 │ │ │ │ │ -300 using Base::hessianDiagonal; │ │ │ │ │ -301 │ │ │ │ │ -303 void hessianDiagonal(double* d) const override; │ │ │ │ │ -304 │ │ │ │ │ -306 std::map hessianBlockDiagonal() const override; │ │ │ │ │ -307 │ │ │ │ │ -309 std::pair jacobian() const override; │ │ │ │ │ -310 │ │ │ │ │ -316 Matrix augmentedJacobian() const override; │ │ │ │ │ -317 │ │ │ │ │ -323 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; │ │ │ │ │ -324 │ │ │ │ │ -_3_2_8 void _u_p_d_a_t_e_H_e_s_s_i_a_n(_H_e_s_s_i_a_n_F_a_c_t_o_r* other) const { │ │ │ │ │ -329 assert(other); │ │ │ │ │ -330 updateHessian(other->_k_e_y_s__, &other->_i_n_f_o__); │ │ │ │ │ -331 } │ │ │ │ │ -332 │ │ │ │ │ -334 void multiplyHessianAdd(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 override; │ │ │ │ │ -335 │ │ │ │ │ -337 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const override; │ │ │ │ │ -338 │ │ │ │ │ -340 void gradientAtZero(double* d) const override; │ │ │ │ │ -341 │ │ │ │ │ -346 Vector gradient(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override; │ │ │ │ │ -347 │ │ │ │ │ -352 boost::shared_ptr eliminateCholesky(const _O_r_d_e_r_i_n_g& │ │ │ │ │ -keys); │ │ │ │ │ -353 │ │ │ │ │ -355 _V_e_c_t_o_r_V_a_l_u_e_s solve(); │ │ │ │ │ -356 │ │ │ │ │ -357 private: │ │ │ │ │ -359 void Allocate(const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ -360 │ │ │ │ │ -362 _H_e_s_s_i_a_n_F_a_c_t_o_r(const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ -363 │ │ │ │ │ -364 friend class _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -365 friend class NonlinearClusterTree; │ │ │ │ │ -366 │ │ │ │ │ -_3_6_8 friend class boost::serialization::access; │ │ │ │ │ -369 template │ │ │ │ │ -370 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -371 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_G_a_u_s_s_i_a_n_F_a_c_t_o_r); │ │ │ │ │ -372 ar & BOOST_SERIALIZATION_NVP(info_); │ │ │ │ │ -373 } │ │ │ │ │ -374 }; │ │ │ │ │ -375 │ │ │ │ │ -392GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -393 _E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y(const GaussianFactorGraph& factors, const Ordering& │ │ │ │ │ -keys); │ │ │ │ │ -394 │ │ │ │ │ -410GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -411 _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y(const GaussianFactorGraph& factors, const Ordering& │ │ │ │ │ -keys); │ │ │ │ │ -412 │ │ │ │ │ -414template<> │ │ │ │ │ -_4_1_5struct _t_r_a_i_t_s<_H_e_s_s_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -416 │ │ │ │ │ -417} // \ namespace gtsam │ │ │ │ │ -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. │ │ │ │ │ -_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 │ │ │ │ │ -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 │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -HessianFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ -void updateHessian(HessianFactor *other) const │ │ │ │ │ -Update another Hessian factor. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:328 │ │ │ │ │ -_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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -GaussianFactor::shared_ptr clone() const override │ │ │ │ │ -Clone this HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o │ │ │ │ │ -const SymmetricBlockMatrix & info() const │ │ │ │ │ -Return underlying information matrix. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:265 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ -SymmetricBlockMatrix::Block linearTerm() │ │ │ │ │ -Return the complete linear term as described above. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:259 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_a_n_t_T_e_r_m │ │ │ │ │ -double constantTerm() const │ │ │ │ │ -Return the constant term as described above. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ -SymmetricBlockMatrix::constBlock linearTerm() const │ │ │ │ │ -Return the complete linear term as described above. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:251 │ │ │ │ │ -_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() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:186 │ │ │ │ │ -_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(const GaussianFactorGraph &factors) │ │ │ │ │ -Combine a set of factors into a single dense HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_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 HessianFactor.h:213 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ -SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const │ │ │ │ │ -Return the part of linear term as described above corresponding to the │ │ │ │ │ -requested variable. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:244 │ │ │ │ │ -_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_:_:_B_a_s_e │ │ │ │ │ -GaussianFactor Base │ │ │ │ │ -Typedef to base class. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_r_o_w_s │ │ │ │ │ -size_t rows() const │ │ │ │ │ -Return the number of columns and rows of the Hessian matrix, including the │ │ │ │ │ -information vector. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_B_l_o_c_k │ │ │ │ │ -SymmetricBlockMatrix::constBlock constBlock │ │ │ │ │ -A block from the Hessian matrix (const version) │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o │ │ │ │ │ -SymmetricBlockMatrix & info() │ │ │ │ │ -Return non-const information matrix. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:269 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_B_l_o_c_k │ │ │ │ │ -SymmetricBlockMatrix::Block Block │ │ │ │ │ -A block from the Hessian matrix. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_a_n_t_T_e_r_m │ │ │ │ │ -double & constantTerm() │ │ │ │ │ -Return the constant term as described above. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:238 │ │ │ │ │ -_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_:_:_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_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_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. │ │ │ │ │ +_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 │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_._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/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/Scatter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,101 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    NoiseModel.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    Scatter.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -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
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │ +
    Date
    June 2015
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,83 +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 │ │ │ │ │ -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 │ │ │ │ │ +Scatter.cpp File Reference │ │ │ │ │ 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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 13, 2010 │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + June 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ + * _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/a00758_source.html │ │ │ │ @@ -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/linear/Preconditioner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,137 +98,212 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PowerMethod.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
    │ │ │ │ -
    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
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    31
    │ │ │ │ -
    32using Sparse = Eigen::SparseMatrix<double>;
    │ │ │ │ -
    33
    │ │ │ │ -
    56template <class Operator>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    58 protected:
    │ │ │ │ -
    63 const Operator &A_;
    │ │ │ │ -
    64
    │ │ │ │ -
    65 const int dim_; // dimension of Matrix A
    │ │ │ │ -
    66
    │ │ │ │ -
    67 size_t nrIterations_; // number of iterations
    │ │ │ │ +
    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 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 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
    │ │ │ │ -
    │ │ │ │ -
    95 Vector powerIteration(const Vector &x) const {
    │ │ │ │ -
    96 Vector y = A_ * x;
    │ │ │ │ -
    97 y.normalize();
    │ │ │ │ -
    98 return y;
    │ │ │ │ -
    99 }
    │ │ │ │ -
    │ │ │ │ -
    100
    │ │ │ │ -
    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 }
    │ │ │ │ +
    69 /* Generic Constructor and Destructor */
    │ │ │ │ + │ │ │ │ +
    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/*******************************************************************************************/
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    97 typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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 {}
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ +
    121};
    │ │ │ │
    │ │ │ │ -
    144
    │ │ │ │ -
    146 double eigenvalue() const { return ritzValue_; }
    │ │ │ │ +
    122
    │ │ │ │ +
    123/*******************************************************************************************/
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    128};
    │ │ │ │ +
    │ │ │ │ +
    129
    │ │ │ │ +
    130/*******************************************************************************************/
    │ │ │ │ +
    │ │ │ │ +
    131class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
    │ │ │ │ +
    132public:
    │ │ │ │ +
    133 typedef Preconditioner Base;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    146protected:
    │ │ │ │
    147
    │ │ │ │ -
    149 Vector eigenvector() const { return ritzVector_; }
    │ │ │ │ -
    150};
    │ │ │ │ -
    │ │ │ │ -
    151
    │ │ │ │ -
    152} // namespace gtsam
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    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
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Compute maximum Eigenpair with power method.
    Definition PowerMethod.h:57
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector eigenvector() const
    Return the eigenvector.
    Definition PowerMethod.h:149
    │ │ │ │ -
    const Operator & A_
    Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
    Definition PowerMethod.h:63
    │ │ │ │ -
    double eigenvalue() const
    Return the eigenvalue.
    Definition PowerMethod.h:146
    │ │ │ │ -
    PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none)
    Construct from the aim matrix and intial ritz vector.
    Definition PowerMethod.h:77
    │ │ │ │ -
    Vector powerIteration() const
    Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
    Definition PowerMethod.h:105
    │ │ │ │ -
    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
    │ │ │ │ -
    bool compute(size_t maxIterations, double tol)
    Start the power/accelerated iteration, after performing the power/accelerated iteration,...
    Definition PowerMethod.h:129
    │ │ │ │ -
    size_t nrIterations() const
    Return the number of iterations.
    Definition PowerMethod.h:121
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ +
    Definition Preconditioner.h:24
    │ │ │ │ +
    Definition Preconditioner.h:64
    │ │ │ │ +
    virtual void solve(const Vector &y, Vector &x) const =0
    implement x = L^{-1} y
    │ │ │ │ +
    virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda)=0
    build/factorize the preconditioner
    │ │ │ │ +
    virtual void transposeSolve(const Vector &y, Vector &x) const =0
    implement x = L^{-T} y
    │ │ │ │ +
    Definition Preconditioner.h:95
    │ │ │ │ +
    Definition Preconditioner.h:103
    │ │ │ │ +
    void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
    build/factorize the preconditioner
    Definition Preconditioner.h:116
    │ │ │ │ +
    void solve(const Vector &y, Vector &x) const override
    implement x = L^{-1} y
    Definition Preconditioner.h:114
    │ │ │ │ +
    void transposeSolve(const Vector &y, Vector &x) const override
    implement x = L^{-T} y
    Definition Preconditioner.h:115
    │ │ │ │ +
    Definition Preconditioner.h:124
    │ │ │ │ +
    Definition Preconditioner.h:131
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,158 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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-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_/_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 │ │ │ │ │ -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 │ │ │ │ │ +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 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 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 │ │ │ │ │ -_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_); } │ │ │ │ │ -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_; } │ │ │ │ │ +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: │ │ │ │ │ +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 │ │ │ │ │ -_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_; } │ │ │ │ │ +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 │ │ │ │ │ +146protected: │ │ │ │ │ 147 │ │ │ │ │ -_1_4_9 Vector _e_i_g_e_n_v_e_c_t_o_r() const { return ritzVector_; } │ │ │ │ │ -150}; │ │ │ │ │ -151 │ │ │ │ │ -152} // namespace gtsam │ │ │ │ │ +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 │ │ │ │ │ -_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_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_:_:_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 │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _P_o_w_e_r_M_e_t_h_o_d_._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/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/Errors.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,72 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    JacobianFactor.h File Reference
    │ │ │ │ +
    Errors.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Factor Graph Values. │ │ │ │ +More...

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

    │ │ │ │ -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.
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ Functions

    std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptrgtsam::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).
     
    │ │ │ │ +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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ +

    Factor Graph Values.

    │ │ │ │ +

    Errors

    Author
    Carlos Nieto
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    Dec 8, 2010
    │ │ │ │ +Christian Potthast
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 | _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 | _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_._._. │ │ │ │ │ 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). │ │ │ │ │ +_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 ********** │ │ │ │ │ +Factor Graph _V_a_l_u_e_s. │ │ │ │ │ +Errors │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Carlos Nieto │ │ │ │ │ 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 │ │ │ │ │ + * _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/a00761.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,9 @@ │ │ │ │ │ var a00761 = [ │ │ │ │ │ - ["gtsam::JacobianFactor", "a03844.html", "a03844"], │ │ │ │ │ - ["gtsam::traits< JacobianFactor >", "a03848.html", null], │ │ │ │ │ - ["EliminateQR", "a00761.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ + ["axpy", "a00761.html#a5644e12e89afaac433f5d61c3d8e9003", null], │ │ │ │ │ + ["createErrors", "a00761.html#aa84fa88db91d429bec261c655fc54bd6", null], │ │ │ │ │ + ["dot", "a00761.html#a106ec458ddb425604809da93346af55d", null], │ │ │ │ │ + ["operator+", "a00761.html#a552de56fca49d1b4942dd9b8e8357b0a", null], │ │ │ │ │ + ["operator-", "a00761.html#aa91c0485819ec2bfd249a922c81e3eb4", null], │ │ │ │ │ + ["operator-", "a00761.html#af2dc48a6a2e9abf4d0636578847a6b5a", null], │ │ │ │ │ + ["print", "a00761.html#a6abcac97e441f1454790ff53caa1b229", 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/RegularHessianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Scatter.h File Reference
    │ │ │ │ +
    RegularHessianFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Maps global variable indices to slot indices. │ │ │ │ +

    HessianFactor class with constant sized blocks. │ │ │ │ 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::RegularHessianFactor< D >
     
    class  gtsam::Scatter
     Scatter is an intermediate data structure used when building a HessianFactor incrementally, to get the keys in the right order. More...
    struct  gtsam::traits< RegularHessianFactor< D > >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Maps global variable indices to slot indices.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    June 2015
    │ │ │ │ +

    HessianFactor class with constant sized blocks.

    │ │ │ │ +
    Author
    Sungtae An
    │ │ │ │ +
    Date
    March 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -Scatter.h File Reference │ │ │ │ │ -Maps global variable indices to slot indices. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_<_ _D_ _> │ │ │ │ │   │ │ │ │ │ - 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Maps global variable indices to slot indices. │ │ │ │ │ +HessianFactor class with constant sized blocks. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Sungtae An │ │ │ │ │ Date │ │ │ │ │ - June 2015 │ │ │ │ │ + March 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_c_a_t_t_e_r_._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/a00764.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00764 = [ │ │ │ │ │ - ["gtsam::SlotEntry", "a04024.html", null], │ │ │ │ │ - ["gtsam::Scatter", "a04028.html", "a04028"] │ │ │ │ │ + ["gtsam::RegularHessianFactor< D >", "a04008.html", "a04008"], │ │ │ │ │ + ["gtsam::traits< RegularHessianFactor< D > >", "a04012.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/RegularHessianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,84 +98,274 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Scatter.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
    │ │ │ │
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <vector>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    27template<size_t D>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30public:
    │ │ │ │ +
    31
    │ │ │ │ +
    32 typedef Eigen::Matrix<double, D, 1> VectorD;
    │ │ │ │ +
    33 typedef Eigen::Matrix<double, D, D> MatrixD;
    │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │
    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};
    │ │ │ │ -
    │ │ │ │ -
    67
    │ │ │ │ -
    68} // \ namespace gtsam
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    71 checkInvariants();
    │ │ │ │ +
    72 }
    │ │ │ │ +
    │ │ │ │ +
    73
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    76 : HessianFactor(jf) {}
    │ │ │ │ +
    │ │ │ │ +
    77
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    80 const Scatter& scatter)
    │ │ │ │ +
    81 : HessianFactor(factors, scatter) {
    │ │ │ │ +
    82 checkInvariants();
    │ │ │ │ +
    83 }
    │ │ │ │ +
    │ │ │ │ +
    84
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ +
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    Vector diagonal(DenseIndex J) const
    Get the diagonal of the J'th diagonal block.
    Definition SymmetricBlockMatrix.h:145
    │ │ │ │ +
    DenseIndex cols() const
    Column size.
    Definition SymmetricBlockMatrix.h:117
    │ │ │ │ +
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ +
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ +
    DenseIndex nBlocks() const
    Block count.
    Definition SymmetricBlockMatrix.h:120
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    One SlotEntry stores the slot index for a variable, as well its dim.
    Definition Scatter.h:32
    │ │ │ │ +
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ +
    Matrix augmentedInformation() const override
    Return the augmented information matrix represented by this GaussianFactor.
    Definition HessianFactor.cpp:282
    │ │ │ │ +
    SymmetricBlockMatrix info_
    The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
    Definition HessianFactor.h:104
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition HessianFactor.cpp:391
    │ │ │ │ +
    Definition RegularHessianFactor.h:28
    │ │ │ │ +
    RegularHessianFactor(const GaussianFactorGraph &factors)
    Construct from a GaussianFactorGraph.
    Definition RegularHessianFactor.h:86
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition RegularHessianFactor.h:112
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
    y += alpha * A'*A*x
    Definition RegularHessianFactor.h:118
    │ │ │ │ +
    void hessianDiagonal(double *d) const override
    Return the diagonal of the Hessian for this factor (raw memory version)
    Definition RegularHessianFactor.h:185
    │ │ │ │ +
    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
    │ │ │ │ +
    void gradientAtZero(double *d) const override
    Add gradient at zero to d TODO: is it really the goal to add ??
    Definition RegularHessianFactor.h:196
    │ │ │ │ +
    RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
    Construct from a GaussianFactorGraph.
    Definition RegularHessianFactor.h:79
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    RegularHessianFactor(const RegularJacobianFactor< D > &jf)
    Construct from RegularJacobianFactor.
    Definition RegularHessianFactor.h:75
    │ │ │ │ +
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ -
    GTSAM_EXPORT void add(Key key, size_t dim)
    Add a key/dim pair.
    Definition Scatter.cpp:76
    │ │ │ │ -
    GTSAM_EXPORT Scatter()
    Default Constructor.
    Definition Scatter.h:52
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,97 +1,356 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Scatter.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 │ │ │ │ │ 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_/_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 │ │ │ │ │ -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} // \ 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 │ │ │ │ │ +_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_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _S_c_a_t_t_e_r_._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/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/GaussianEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    SparseEigen.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianEliminationTree.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Utilities for creating Eigen sparse matrices (gtsam::SparseEigen) │ │ │ │ -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
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,21 @@ │ │ │ │ │ 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 │ │ │ │ │ +GaussianEliminationTree.cpp File Reference │ │ │ │ │ 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 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Mandy Xie │ │ │ │ │ - Fan Jiang │ │ │ │ │ - Gerry Chen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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_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/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/SubgraphPreconditioner.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianBayesTree.h File Reference
    │ │ │ │ +
    SubgraphPreconditioner.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    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

    │ │ │ │ -

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

    │ │ │ │ -

    GaussianBayesTree

    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Date
    Dec 31, 2009
    │ │ │ │ +
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +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 │ │ │ │ │ -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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ + Date │ │ │ │ │ + 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_B_a_y_e_s_T_r_e_e_._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/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/NoiseModel.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,46 +95,100 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    NoiseModel.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...
    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< GaussianFactor >
     traits 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

    │ │ │ │ -

    A factor with a quadratic error function - a Gaussian.

    │ │ │ │ -

    GaussianFactor

    Author
    Richard Roberts, Christian Potthast
    │ │ │ │ +
    Date
    Jan 13, 2010
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,83 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_:_:_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. │ │ │ │ │ + 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_._._. │ │ │ │ │   │ │ │ │ │ -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_ _> │ │ │ │ │ + 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_ _> │ │ │ │ │ +  │ │ │ │ │ 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 ********** │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -GaussianFactor │ │ │ │ │ + Date │ │ │ │ │ + Jan 13, 2010 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts, Christian Potthast │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00788.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,15 @@ │ │ │ │ │ var a00788 = [ │ │ │ │ │ - ["gtsam::GaussianFactor", "a03780.html", "a03780"], │ │ │ │ │ - ["gtsam::traits< GaussianFactor >", "a03784.html", null] │ │ │ │ │ + ["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", "a00788.html#ab6e5a4884342656e0837ef07008ec03f", null] │ │ │ │ │ ]; │ │ │ ├── ./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/NoiseModel.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,170 +98,631 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianFactor.h
    │ │ │ │ +
    NoiseModel.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
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    38 class GTSAM_EXPORT GaussianFactor : public Factor
    │ │ │ │ -
    39 {
    │ │ │ │ -
    40 public:
    │ │ │ │ - │ │ │ │ -
    42 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    43 typedef Factor Base;
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    50 template<typename CONTAINER>
    │ │ │ │ -
    51 GaussianFactor(const CONTAINER& keys) : Base(keys) {}
    │ │ │ │ -
    52
    │ │ │ │ -
    54 virtual ~GaussianFactor() {}
    │ │ │ │ -
    55
    │ │ │ │ -
    56 // Implementing Testable interface
    │ │ │ │ +
    45 //---------------------------------------------------------------------------------------
    │ │ │ │ +
    46
    │ │ │ │ +
    │ │ │ │ +
    53 class GTSAM_EXPORT Base {
    │ │ │ │ +
    54
    │ │ │ │ +
    55 public:
    │ │ │ │ +
    56 typedef boost::shared_ptr<Base> shared_ptr;
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    82 virtual DenseIndex getDim(const_iterator variable) const = 0;
    │ │ │ │ +
    82 virtual Vector sigmas() const;
    │ │ │ │
    83
    │ │ │ │ -
    91 virtual Matrix augmentedJacobian() const = 0;
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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 std::map<Key,Matrix> hessianBlockDiagonal() const = 0;
    │ │ │ │ -
    128
    │ │ │ │ - │ │ │ │ -
    131
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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};
    │ │ │ │ +
    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} // \ namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    The base class for all factors.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    virtual DenseIndex getDim(const_iterator variable) const =0
    Return the dimension of the variable pointed to by the given key iterator.
    │ │ │ │ -
    virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0
    Return the block diagonal of the Hessian for this factor.
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    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.
    │ │ │ │ -
    virtual Vector gradient(Key key, const VectorValues &x) const =0
    Gradient wrt a key at any values.
    │ │ │ │ -
    virtual GaussianFactor::shared_ptr clone() const =0
    Clone a factor (make a deep copy)
    │ │ │ │ -
    virtual Matrix information() const =0
    Return the non-augmented information matrix represented by this GaussianFactor.
    │ │ │ │ -
    Factor Base
    Our base class.
    Definition GaussianFactor.h:43
    │ │ │ │ -
    virtual ~GaussianFactor()
    Destructor.
    Definition GaussianFactor.h:54
    │ │ │ │ -
    virtual VectorValues gradientAtZero() const =0
    A'*b for Jacobian, eta for Hessian.
    │ │ │ │ -
    GaussianFactor(const CONTAINER &keys)
    Construct from container of keys.
    Definition GaussianFactor.h:51
    │ │ │ │ -
    virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const =0
    y += alpha * A'*A*x
    │ │ │ │ -
    virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0
    Equals for testable.
    │ │ │ │ -
    virtual Matrix augmentedInformation() const =0
    Return the augmented information matrix represented by this GaussianFactor.
    │ │ │ │ -
    virtual Matrix augmentedJacobian() const =0
    Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
    │ │ │ │ -
    GaussianFactor()
    Default constructor creates empty factor.
    Definition GaussianFactor.h:46
    │ │ │ │ -
    virtual GaussianFactor::shared_ptr negate() const =0
    Construct the corresponding anti-factor to negate information stored stored in this factor.
    │ │ │ │ -
    virtual void hessianDiagonal(double *d) const =0
    Raw memory access version of hessianDiagonal.
    │ │ │ │ -
    GaussianFactor This
    This class.
    Definition GaussianFactor.h:41
    │ │ │ │ -
    virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const =0
    Update an information matrix by adding the information corresponding to this factor (used internally ...
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override=0
    print
    │ │ │ │ -
    virtual void hessianDiagonalAdd(VectorValues &d) const =0
    Add the current diagonal to a VectorValues instance.
    │ │ │ │ -
    virtual void gradientAtZero(double *d) const =0
    Raw memory access version of gradientAtZero.
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    Pure virtual class for all robust error function classes.
    Definition LossFunctions.h:63
    │ │ │ │ +
    noiseModel::Base is the abstract base class for all noise models.
    Definition NoiseModel.h:53
    │ │ │ │ +
    virtual bool isConstrained() const
    true if a constrained noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:69
    │ │ │ │ +
    virtual void whitenInPlace(Vector &v) const
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:112
    │ │ │ │ +
    size_t dim() const
    Dimensionality.
    Definition NoiseModel.h:75
    │ │ │ │ +
    virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:127
    │ │ │ │ +
    virtual void whitenInPlace(Eigen::Block< Vector > &v) const
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:122
    │ │ │ │ +
    virtual Vector whiten(const Vector &v) const =0
    Whiten an error vector.
    │ │ │ │ +
    virtual double mahalanobisDistance(const Vector &v) const
    Mahalanobis distance.
    Definition NoiseModel.h:97
    │ │ │ │ +
    virtual bool isUnit() const
    true if a unit noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:72
    │ │ │ │ +
    virtual double weight(const Vector &v) const
    get the weight from the effective loss function on residual vector v
    Definition NoiseModel.h:137
    │ │ │ │ +
    virtual Vector unweightedWhiten(const Vector &v) const
    Useful function for robust noise models to get the unweighted but whitened error.
    Definition NoiseModel.h:132
    │ │ │ │ +
    virtual Vector unwhiten(const Vector &v) const =0
    Unwhiten an error vector.
    │ │ │ │ +
    virtual double loss(const double squared_distance) const
    loss function, input is Mahalanobis distance
    Definition NoiseModel.h:102
    │ │ │ │ +
    virtual void unwhitenInPlace(Vector &v) const
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:117
    │ │ │ │ +
    virtual Matrix Whiten(const Matrix &H) const =0
    Whiten a matrix.
    │ │ │ │ +
    Base(size_t dim=1)
    primary constructor
    Definition NoiseModel.h:65
    │ │ │ │ +
    Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
    Definition NoiseModel.h:162
    │ │ │ │ +
    virtual Matrix R() const
    Return R itself, but note that Whiten(H) is cheaper than R*H.
    Definition NoiseModel.h:255
    │ │ │ │ +
    Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
    constructor takes square root information matrix
    Definition NoiseModel.h:186
    │ │ │ │ +
    boost::optional< Matrix > sqrt_information_
    Matrix square root of information matrix (R)
    Definition NoiseModel.h:167
    │ │ │ │ +
    A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
    Definition NoiseModel.h:281
    │ │ │ │ +
    Matrix R() const override
    Return R itself, but note that Whiten(H) is cheaper than R*H.
    Definition NoiseModel.h:352
    │ │ │ │ +
    Vector sigmas_
    Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
    Definition NoiseModel.h:289
    │ │ │ │ +
    double sigma(size_t i) const
    Return standard deviations (sqrt of diagonal)
    Definition NoiseModel.h:335
    │ │ │ │ +
    const Vector & invsigmas() const
    Return sqrt precisions.
    Definition NoiseModel.h:340
    │ │ │ │ +
    Vector sigmas() const override
    Calculate standard deviations.
    Definition NoiseModel.h:325
    │ │ │ │ +
    const Vector & precisions() const
    Return precisions.
    Definition NoiseModel.h:346
    │ │ │ │ +
    A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
    Definition NoiseModel.h:381
    │ │ │ │ +
    bool isConstrained() const override
    true if a constrained noise mode, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:409
    │ │ │ │ +
    static shared_ptr All(size_t dim, const Vector &mu)
    Fully constrained variations.
    Definition NoiseModel.h:462
    │ │ │ │ +
    static shared_ptr All(size_t dim, double mu)
    Fully constrained variations with a mu parameter.
    Definition NoiseModel.h:467
    │ │ │ │ +
    static shared_ptr All(size_t dim)
    Fully constrained variations.
    Definition NoiseModel.h:457
    │ │ │ │ +
    const Vector & mu() const
    Access mu as a vector.
    Definition NoiseModel.h:415
    │ │ │ │ +
    Vector mu_
    Penalty function weight - needs to be large enough to dominate soft constraints.
    Definition NoiseModel.h:385
    │ │ │ │ +
    An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
    Definition NoiseModel.h:516
    │ │ │ │ +
    double sigma() const
    Return standard deviation.
    Definition NoiseModel.h:565
    │ │ │ │ +
    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
    │ │ │ │ +
    Isotropic(size_t dim, double sigma)
    protected constructor takes sigma
    Definition NoiseModel.h:521
    │ │ │ │ +
    Unit: i.i.d.
    Definition NoiseModel.h:584
    │ │ │ │ +
    void WhitenInPlace(Eigen::Block< Matrix >) const override
    In-place version.
    Definition NoiseModel.h:610
    │ │ │ │ +
    Vector unwhiten(const Vector &v) const override
    Unwhiten an error vector.
    Definition NoiseModel.h:607
    │ │ │ │ +
    void whitenInPlace(Eigen::Block< Vector > &) const override
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:613
    │ │ │ │ +
    bool isUnit() const override
    true if a unit noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:602
    │ │ │ │ +
    Unit(size_t dim=1)
    constructor for serialization
    Definition NoiseModel.h:590
    │ │ │ │ +
    Vector whiten(const Vector &v) const override
    Whiten an error vector.
    Definition NoiseModel.h:606
    │ │ │ │ +
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    void unwhitenInPlace(Vector &) const override
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:612
    │ │ │ │ +
    void unwhitenInPlace(Eigen::Block< Vector > &) const override
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:614
    │ │ │ │ +
    void whitenInPlace(Vector &) const override
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:611
    │ │ │ │ +
    void WhitenInPlace(Matrix &) const override
    In-place version.
    Definition NoiseModel.h:609
    │ │ │ │ +
    Matrix Whiten(const Matrix &H) const override
    Whiten a matrix.
    Definition NoiseModel.h:608
    │ │ │ │ +
    Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
    Definition NoiseModel.h:642
    │ │ │ │ +
    const RobustModel::shared_ptr & robust() const
    Return the contained robust error function.
    Definition NoiseModel.h:669
    │ │ │ │ +
    Robust()
    Default Constructor for serialization.
    Definition NoiseModel.h:656
    │ │ │ │ +
    Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
    Constructor.
    Definition NoiseModel.h:659
    │ │ │ │ +
    double squaredMahalanobisDistance(const Vector &v) const override
    Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
    Definition NoiseModel.h:688
    │ │ │ │ +
    const NoiseModel::shared_ptr noise_
    noise model used
    Definition NoiseModel.h:651
    │ │ │ │ +
    double loss(const double squared_distance) const override
    Compute loss from the m-estimator using the Mahalanobis distance.
    Definition NoiseModel.h:682
    │ │ │ │ +
    Vector unwhiten(const Vector &) const override
    Unwhiten an error vector.
    Definition NoiseModel.h:679
    │ │ │ │ +
    Matrix Whiten(const Matrix &A) const override
    Whiten a matrix.
    Definition NoiseModel.h:677
    │ │ │ │ +
    const RobustModel::shared_ptr robust_
    robust error function used
    Definition NoiseModel.h:650
    │ │ │ │ +
    ~Robust() override
    Destructor.
    Definition NoiseModel.h:663
    │ │ │ │ +
    const NoiseModel::shared_ptr & noise() const
    Return the contained noise model.
    Definition NoiseModel.h:672
    │ │ │ │ +
    Vector whiten(const Vector &v) const override
    Whiten an error vector.
    Definition NoiseModel.h:675
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,255 +1,805 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianFactor.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) │ │ │ │ │ 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> │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +45 //-------------------------------------------------------------------------- │ │ │ │ │ +------------- │ │ │ │ │ +46 │ │ │ │ │ +_5_3 class GTSAM_EXPORT _B_a_s_e { │ │ │ │ │ +54 │ │ │ │ │ +55 public: │ │ │ │ │ +56 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +82 virtual Vector sigmas() const; │ │ │ │ │ 83 │ │ │ │ │ -_9_1 virtual Matrix _a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n() const = 0; │ │ │ │ │ +_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 │ │ │ │ │ -_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; │ │ │ │ │ +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 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; │ │ │ │ │ +_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 │ │ │ │ │ -_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; │ │ │ │ │ +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_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}; │ │ │ │ │ +_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} // \ namespace gtsam │ │ │ │ │ +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 │ │ │ │ │ -_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_:_:_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_:_:_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_:_:_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_:_:_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() │ │ │ │ │ +_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 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. │ │ │ │ │ +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 │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/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/NoiseModel.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,70 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Errors.cpp File Reference
    │ │ │ │ +
    NoiseModel.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Factor Graph Values. │ │ │ │ -More...

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

    │ │ │ │ Namespaces

    namespace  gtsam
     Global functions in a separate testing namespace.
     
    namespace  gtsam::noiseModel
     All noise models live in the noiseModel 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
     
    │ │ │ │ +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

    │ │ │ │ -

    Factor Graph Values.

    │ │ │ │ -

    Errors

    Author
    Carlos Nieto
    │ │ │ │ +
    Date
    Jan 13, 2010
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │ -Christian Potthast
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +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 │ │ │ │ │ -Errors.cpp File Reference │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +NoiseModel.cpp File Reference │ │ │ │ │ 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. │ │ │ │ │ -  │ │ │ │ │ - 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. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -_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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. │ │ │ │ │ -Errors │ │ │ │ │ + Date │ │ │ │ │ + Jan 13, 2010 │ │ │ │ │ Author │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _E_r_r_o_r_s_._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/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/Sampler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SubgraphSolver.h File Reference
    │ │ │ │ +
    Sampler.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Subgraph Solver from IROS 2010. │ │ │ │ +

    sampling from a NoiseModel │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::SubgraphSolverParameters
     
    class  gtsam::SubgraphSolver
     This class implements the linear SPCG solver presented in Dellaert et al in IROS'10. 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

    │ │ │ │ -

    Subgraph Solver from IROS 2010.

    │ │ │ │ -
    Date
    2010
    │ │ │ │ +

    sampling from a NoiseModel

    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Yong Dian Jian
    │ │ │ │ +Alex Cunningham │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -SubgraphSolver.h File Reference │ │ │ │ │ -Subgraph Solver from IROS 2010. _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_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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Subgraph Solver from IROS 2010. │ │ │ │ │ - Date │ │ │ │ │ - 2010 │ │ │ │ │ +sampling from a NoiseModel │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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_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/a00794.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00794 = [ │ │ │ │ │ - ["gtsam::SubgraphSolverParameters", "a04056.html", null], │ │ │ │ │ - ["gtsam::SubgraphSolver", "a04060.html", "a04060"] │ │ │ │ │ + ["gtsam::Sampler", "a04020.html", "a04020"] │ │ │ │ │ ]; │ │ │ ├── ./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/Sampler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,114 +98,79 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SubgraphSolver.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23#include <random>
    │ │ │ │
    24
    │ │ │ │ -
    25#include <map>
    │ │ │ │ -
    26#include <utility> // pair
    │ │ │ │ -
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ -
    29
    │ │ │ │ -
    30// Forward declarations
    │ │ │ │ -
    31class GaussianFactorGraph;
    │ │ │ │ -
    32class GaussianBayesNet;
    │ │ │ │ -
    33class SubgraphPreconditioner;
    │ │ │ │ -
    34
    │ │ │ │ -
    │ │ │ │ -
    35struct GTSAM_EXPORT SubgraphSolverParameters
    │ │ │ │ - │ │ │ │ -
    37 SubgraphBuilderParameters builderParams;
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    79
    │ │ │ │ -
    80 protected:
    │ │ │ │ -
    81 Parameters parameters_;
    │ │ │ │ -
    82 boost::shared_ptr<SubgraphPreconditioner> pc_;
    │ │ │ │ -
    83
    │ │ │ │ -
    84 public:
    │ │ │ │ +
    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
    │ │ │ │ +
    60 explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
    │ │ │ │ +
    61
    │ │ │ │ +
    65
    │ │ │ │ +
    66 size_t dim() const { return model_->dim(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    93 SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
    │ │ │ │ -
    94 const Ordering &ordering);
    │ │ │ │ -
    95
    │ │ │ │ - │ │ │ │ -
    103 const Parameters &parameters, const Ordering &ordering);
    │ │ │ │ - │ │ │ │ -
    109 const Parameters &parameters);
    │ │ │ │ -
    110
    │ │ │ │ -
    112 ~SubgraphSolver() override {}
    │ │ │ │ -
    113
    │ │ │ │ -
    117
    │ │ │ │ -
    119 VectorValues optimize() const;
    │ │ │ │ -
    120
    │ │ │ │ - │ │ │ │ -
    123 const KeyInfo &keyInfo,
    │ │ │ │ -
    124 const std::map<Key, Vector> &lambda,
    │ │ │ │ -
    125 const VectorValues &initial) override;
    │ │ │ │ -
    126
    │ │ │ │ -
    130
    │ │ │ │ -
    132 std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
    │ │ │ │ -
    133 const GaussianFactorGraph &gfg);
    │ │ │ │ -
    134
    │ │ │ │ -
    136};
    │ │ │ │ -
    │ │ │ │ -
    137
    │ │ │ │ -
    138} // namespace gtsam
    │ │ │ │ -
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ - │ │ │ │ +
    88} // namespace gtsam
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ -
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Base class for Iterative Solvers like SubgraphSolver.
    Definition IterativeSolver.h:86
    │ │ │ │ -
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ -
    Definition SubgraphBuilder.h:96
    │ │ │ │ -
    Definition SubgraphSolver.h:36
    │ │ │ │ -
    This class implements the linear SPCG solver presented in Dellaert et al in IROS'10.
    Definition SubgraphSolver.h:76
    │ │ │ │ -
    ~SubgraphSolver() override
    Destructor.
    Definition SubgraphSolver.h:112
    │ │ │ │ -
    boost::shared_ptr< SubgraphPreconditioner > pc_
    preconditioner object
    Definition SubgraphSolver.h:82
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Sampling structure that keeps internal random number generators for diagonal distributions specified ...
    Definition Sampler.h:31
    │ │ │ │ +
    noiseModel::Diagonal::shared_ptr model_
    noiseModel created at generation
    Definition Sampler.h:34
    │ │ │ │ +
    std::mt19937_64 generator_
    generator
    Definition Sampler.h:37
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,139 +1,80 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SubgraphSolver.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_/_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> │ │ │ │ │ +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 │ │ │ │ │ -25#include │ │ │ │ │ -26#include // pair │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Forward declarations │ │ │ │ │ -31class GaussianFactorGraph; │ │ │ │ │ -32class GaussianBayesNet; │ │ │ │ │ -33class SubgraphPreconditioner; │ │ │ │ │ -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 = │ │ │ │ │ -_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: │ │ │ │ │ +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 │ │ │ │ │ +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 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 │ │ │ │ │ -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 │ │ │ │ │ -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. │ │ │ │ │ -_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_u_b_g_r_a_p_h_S_o_l_v_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/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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    IterativeSolver.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    iterative.cpp 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ 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

    │ │ │ │ -

    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,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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -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_._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/a00797.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,7 @@ │ │ │ │ │ var a00797 = [ │ │ │ │ │ - ["gtsam::IterativeOptimizationParameters", "a03828.html", null], │ │ │ │ │ - ["gtsam::IterativeSolver", "a03832.html", null], │ │ │ │ │ - ["gtsam::KeyInfoEntry", "a03836.html", null], │ │ │ │ │ - ["gtsam::KeyInfo", "a03840.html", "a03840"] │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#aa84114ad1593684ad739088a2898afdb", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#af55440f741d8b2f706101f7a79ba7111", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#ad2b587fde5d35a27d6c88feb4432e785", null], │ │ │ │ │ + ["steepestDescent", "a00797.html#a81b7af7638d28555c3f404318cc67b22", null], │ │ │ │ │ + ["steepestDescent", "a00797.html#aed4b2ca704ddd6530ea70122f883ac88", null] │ │ │ │ │ ]; │ │ │ ├── ./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/PowerMethod.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Sampler.cpp File Reference
    │ │ │ │ +
    PowerMethod.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    sampling from a diagonal NoiseModel │ │ │ │ +

    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

    │ │ │ │ -

    sampling from a diagonal NoiseModel

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

    Power method for fast eigenvalue and eigenvector computation.

    │ │ │ │ +
    Date
    Sept 2020
    │ │ │ │ +
    Author
    Jing Wu
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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_._._. │ │ │ │ │ +_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 ********** │ │ │ │ │ -sampling from a diagonal NoiseModel │ │ │ │ │ +Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ + Date │ │ │ │ │ + Sept 2020 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_a_m_p_l_e_r_._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/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/GaussianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,45 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    BinaryJacobianFactor.h File Reference
    │ │ │ │ +
    GaussianFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ +

    A factor with a quadratic error function - a Gaussian. │ │ │ │ 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...
    class  gtsam::GaussianFactor
     An abstract virtual base class for JacobianFactor and HessianFactor. More...
     
    struct  gtsam::traits< BinaryJacobianFactor< M, N1, N2 > >
    struct  gtsam::traits< GaussianFactor >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    A binary JacobianFactor specialization that uses fixed matrix math for speed.

    │ │ │ │ -
    Date
    June 2015
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +

    A factor with a quadratic error function - a Gaussian.

    │ │ │ │ +

    GaussianFactor

    Author
    Richard Roberts, Christian Potthast
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ + 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_<_ _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_ _>_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** 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. │ │ │ │ │ - Date │ │ │ │ │ - June 2015 │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +GaussianFactor │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts, Christian Potthast │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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] │ │ │ │ │ + ["gtsam::GaussianFactor", "a03780.html", "a03780"], │ │ │ │ │ + ["gtsam::traits< GaussianFactor >", "a03784.html", 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/GaussianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,132 +98,170 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BinaryJacobianFactor.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
    │ │ │ │ +
    19// \callgraph
    │ │ │ │ +
    20
    │ │ │ │
    21#pragma once
    │ │ │ │
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/base/timing.h>
    │ │ │ │ + │ │ │ │ +
    24#include <gtsam/base/Matrix.h>
    │ │ │ │ +
    25#include <gtsam/base/Testable.h>
    │ │ │ │
    26
    │ │ │ │
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │ -
    32template<int M, int N1, int N2>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    29 // Forward declarations
    │ │ │ │ +
    30 class VectorValues;
    │ │ │ │ +
    31 class Scatter;
    │ │ │ │ +
    32 class SymmetricBlockMatrix;
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ +
    38 class GTSAM_EXPORT GaussianFactor : public Factor
    │ │ │ │ +
    39 {
    │ │ │ │ +
    40 public:
    │ │ │ │ + │ │ │ │ +
    42 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    43 typedef Factor Base;
    │ │ │ │ +
    44
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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 utilities.
    │ │ │ │ -
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    131
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
    Update an off diagonal block.
    Definition SymmetricBlockMatrix.h:228
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex nBlocks() const
    Block count.
    Definition SymmetricBlockMatrix.h:120
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    const Matrix & matrix() const
    Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
    Definition VerticalBlockMatrix.h:188
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ -
    const_iterator end() const
    Iterator at end of involved variable keys.
    Definition Factor.h:146
    │ │ │ │ -
    A binary JacobianFactor specialization that uses fixed matrix math for speed.
    Definition BinaryJacobianFactor.h:33
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ -
    const SharedDiagonal & get_model() const
    get a copy of model
    Definition JacobianFactor.h:291
    │ │ │ │ -
    constABlock getA() const
    Get a view of the A matrix, not weighted by noise.
    Definition JacobianFactor.h:303
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    virtual DenseIndex getDim(const_iterator variable) const =0
    Return the dimension of the variable pointed to by the given key iterator.
    │ │ │ │ +
    virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0
    Return the block diagonal of the Hessian for this factor.
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    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.
    │ │ │ │ +
    virtual Vector gradient(Key key, const VectorValues &x) const =0
    Gradient wrt a key at any values.
    │ │ │ │ +
    virtual GaussianFactor::shared_ptr clone() const =0
    Clone a factor (make a deep copy)
    │ │ │ │ +
    virtual Matrix information() const =0
    Return the non-augmented information matrix represented by this GaussianFactor.
    │ │ │ │ +
    Factor Base
    Our base class.
    Definition GaussianFactor.h:43
    │ │ │ │ +
    virtual ~GaussianFactor()
    Destructor.
    Definition GaussianFactor.h:54
    │ │ │ │ +
    virtual VectorValues gradientAtZero() const =0
    A'*b for Jacobian, eta for Hessian.
    │ │ │ │ +
    GaussianFactor(const CONTAINER &keys)
    Construct from container of keys.
    Definition GaussianFactor.h:51
    │ │ │ │ +
    virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const =0
    y += alpha * A'*A*x
    │ │ │ │ +
    virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0
    Equals for testable.
    │ │ │ │ +
    virtual Matrix augmentedInformation() const =0
    Return the augmented information matrix represented by this GaussianFactor.
    │ │ │ │ +
    virtual Matrix augmentedJacobian() const =0
    Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
    │ │ │ │ +
    GaussianFactor()
    Default constructor creates empty factor.
    Definition GaussianFactor.h:46
    │ │ │ │ +
    virtual GaussianFactor::shared_ptr negate() const =0
    Construct the corresponding anti-factor to negate information stored stored in this factor.
    │ │ │ │ +
    virtual void hessianDiagonal(double *d) const =0
    Raw memory access version of hessianDiagonal.
    │ │ │ │ +
    GaussianFactor This
    This class.
    Definition GaussianFactor.h:41
    │ │ │ │ +
    virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const =0
    Update an information matrix by adding the information corresponding to this factor (used internally ...
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override=0
    print
    │ │ │ │ +
    virtual void hessianDiagonalAdd(VectorValues &d) const =0
    Add the current diagonal to a VectorValues instance.
    │ │ │ │ +
    virtual void gradientAtZero(double *d) const =0
    Raw memory access version of gradientAtZero.
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,99 +1,128 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BinaryJacobianFactor.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 │ │ │ │ │ +19// \callgraph │ │ │ │ │ +20 │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ _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 │ │ │ │ │ @@ -101,86 +130,126 @@ │ │ │ │ │ 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_:_:_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 │ │ │ │ │ +_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 ... │ │ │ │ │ -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_:_:_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 │ │ │ │ │ - * _B_i_n_a_r_y_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 │ │ │ │ │ * Generated on Tue Jan 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/linearAlgorithms-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianDensity.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    linearAlgorithms-inst.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    A Gaussian Density. │ │ │ │ +

    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Functions

    │ │ │ │ +template<class BAYESTREE >
    VectorValues gtsam::internal::linearAlgorithms::optimizeBayesTree (const BAYESTREE &bayesTree)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A Gaussian Density.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Jan 21, 2012
    │ │ │ │ +

    Templated algorithms that are used in multiple places in linear.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,32 @@ │ │ │ │ │ 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 | _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. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::iinntteerrnnaall::::lliinneeaarrAAllggoorriitthhmmss::::ooppttiimmiizzeeBBaayyeessTTrreeee (const │ │ │ │ │ + BAYESTREE &bayesTree) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Gaussian Density. │ │ │ │ │ +Templated algorithms that are used in multiple places in linear. │ │ │ │ │ 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_._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/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/GaussianBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    KalmanFilter.cpp File Reference
    │ │ │ │ +
    GaussianBayesNet.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Simple linear Kalman filter. │ │ │ │ +

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Simple linear Kalman filter.

    │ │ │ │ -

    Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

    │ │ │ │ -
    Date
    Sep 3, 2011
    │ │ │ │ -
    Author
    Stephen Williams
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +

    Chordal Bayes Net, the result of eliminating a factor graph.

    │ │ │ │ +

    GaussianBayesNet

    Author
    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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple linear Kalman filter. │ │ │ │ │ -Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ - Date │ │ │ │ │ - Sep 3, 2011 │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ +GaussianBayesNet │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ 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_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/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/GaussianJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    ConjugateGradientSolver.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianJunctionTree.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::ConjugateGradientParameters
     parameters for the conjugate gradient method 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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Functions

    │ │ │ │ -template<class S , class 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
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Sungtae An
    │ │ │ │ -
    Date
    Nov 6, 2014
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _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 │ │ │ │ │ +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_:_:_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_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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ - Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Sungtae An │ │ │ │ │ Date │ │ │ │ │ - Nov 6, 2014 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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_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/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/GaussianJunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,189 +98,66 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ConjugateGradientSolver.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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    30
    │ │ │ │ -
    31public:
    │ │ │ │ - │ │ │ │ -
    33 typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
    │ │ │ │ -
    34
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    37 size_t reset_;
    │ │ │ │ -
    38 double epsilon_rel_;
    │ │ │ │ -
    39 double epsilon_abs_;
    │ │ │ │ -
    40
    │ │ │ │ -
    41 /* Matrix Operation Kernel */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    43 GTSAM = 0,
    │ │ │ │ -
    44 } blas_kernel_ ;
    │ │ │ │ -
    │ │ │ │ -
    45
    │ │ │ │ - │ │ │ │ -
    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 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
    │ │ │ │ -
    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}
    │ │ │ │ -
    Some support classes for iterative solvers.
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    27 // Forward declarations
    │ │ │ │ +
    28 class GaussianEliminationTree;
    │ │ │ │ +
    29
    │ │ │ │ +
    │ │ │ │ +
    38 class GTSAM_EXPORT GaussianJunctionTree :
    │ │ │ │ +
    39 public JunctionTree<GaussianBayesTree, GaussianFactorGraph> {
    │ │ │ │ +
    40 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    43 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    44
    │ │ │ │ +
    53 GaussianJunctionTree(const GaussianEliminationTree& eliminationTree);
    │ │ │ │ +
    54 };
    │ │ │ │ +
    │ │ │ │ +
    55
    │ │ │ │ +
    56}
    │ │ │ │ +
    The junction tree.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ -
    size_t minIterations_
    minimum number of cg iterations
    Definition ConjugateGradientSolver.h:35
    │ │ │ │ -
    size_t reset_
    number of iterations before reset
    Definition ConjugateGradientSolver.h:37
    │ │ │ │ -
    BLASKernel
    Definition ConjugateGradientSolver.h:42
    │ │ │ │ -
    double epsilon_rel_
    threshold for relative error decrease
    Definition ConjugateGradientSolver.h:38
    │ │ │ │ -
    size_t maxIterations_
    maximum number of cg iterations
    Definition ConjugateGradientSolver.h:36
    │ │ │ │ -
    double epsilon_abs_
    threshold for absolute error decrease
    Definition ConjugateGradientSolver.h:39
    │ │ │ │ -
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │ +
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ +
    Definition GaussianEliminationTree.h:29
    │ │ │ │ +
    A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
    Definition GaussianJunctionTree.h:39
    │ │ │ │ +
    GaussianJunctionTree This
    This class.
    Definition GaussianJunctionTree.h:42
    │ │ │ │ +
    JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base
    Base class.
    Definition GaussianJunctionTree.h:41
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition GaussianJunctionTree.h:43
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,216 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ConjugateGradientSolver.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 │ │ │ │ │ -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; } │ │ │ │ │ -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; │ │ │ │ │ -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. │ │ │ │ │ +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> │ │ │ │ │ +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_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_:_:_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_:_:_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 │ │ │ │ │ * _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_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/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/SubgraphSolver.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    linearExceptions.cpp File Reference
    │ │ │ │ +
    SubgraphSolver.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Exceptions that may be thrown by linear solver components. │ │ │ │ +

    Subgraph Solver from IROS 2010. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Exceptions that may be thrown by linear solver components.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Aug 17, 2012
    │ │ │ │ +

    Subgraph Solver from IROS 2010.

    │ │ │ │ +
    Date
    2010
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Yong Dian Jian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,31 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_:_:_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Exceptions that may be thrown by linear solver components. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2012 │ │ │ │ │ + 2010 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ * _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_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/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/VectorValues.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    linearAlgorithms-inst.h File Reference
    │ │ │ │ +
    VectorValues.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Templated algorithms that are used in multiple places in linear. │ │ │ │ +

    Implementations for VectorValues. │ │ │ │ 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)
     
    │ │ │ │ +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

    │ │ │ │ -

    Templated algorithms that are used in multiple places in linear.

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

    Implementations for VectorValues.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -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 | _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_._._. │ │ │ │ │ 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) │ │ │ │ │ +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 ********** │ │ │ │ │ -Templated algorithms that are used in multiple places in linear. │ │ │ │ │ +Implementations for _V_e_c_t_o_r_V_a_l_u_e_s. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/GaussianBayesTree-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianJunctionTree.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

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

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

    │ │ │ │ +

    GaussianBayesTree

    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ 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 │ │ │ │ │ -GaussianJunctionTree.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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _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_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/a00830.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    VectorValues.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    GaussianBayesTree.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor Graph Values. │ │ │ │ +

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ More...

    │ │ │ │ - │ │ │ │ -

    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 >
     traits 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

    │ │ │ │ -

    Factor Graph Values.

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

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

    │ │ │ │ +

    GaussianBayesTree

    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -VectorValues.h File Reference │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. _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_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -  _V_e_c_t_o_r_V_a_l_u_e_s represents a collection of vector-valued variables │ │ │ │ │ - associated each with a unique integer index. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_e_c_t_o_r_V_a_l_u_e_s_ _> │ │ │ │ │ -  traits _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 ********** │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ + * _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/a00833.html │ │ │ │ @@ -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/linear/JacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,46 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianConditional.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    JacobianFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Conditional Gaussian Base class. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::GaussianConditional
     A GaussianConditional functions as the node in a Bayes network. More...
    class  gtsam::JacobianFactor
     A Gaussian factor in the squared-error form. More...
     
    struct  gtsam::traits< GaussianConditional >
     traits 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_ptrgtsam::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

    │ │ │ │ -

    Conditional Gaussian Base class.

    │ │ │ │ -
    Author
    Christian Potthast
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    Dec 8, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -GaussianConditional.h 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 | _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_:_:_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_:_:_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_<_ _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_<_ _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 ********** │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00833.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00833 = [ │ │ │ │ │ - ["gtsam::traits< GaussianConditional >", "a03768.html", null] │ │ │ │ │ + ["gtsam::JacobianFactor", "a03844.html", "a03844"], │ │ │ │ │ + ["gtsam::traits< JacobianFactor >", "a03848.html", null], │ │ │ │ │ + ["EliminateQR", "a00833.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ ]; │ │ │ ├── ./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/JacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,224 +98,360 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianConditional.h
    │ │ │ │ +
    JacobianFactor.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/utility.hpp>
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    27#include <gtsam/inference/Conditional-inst.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:
    │ │ │ │ - │ │ │ │ -
    46 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    49
    │ │ │ │ -
    52
    │ │ │ │ - │ │ │ │ -
    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());
    │ │ │ │ -
    77
    │ │ │ │ -
    82 template<typename KEYS>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    98 static GaussianConditional FromMeanAndStddev(Key key, //
    │ │ │ │ -
    99 const Matrix& A1, Key parent1,
    │ │ │ │ -
    100 const Matrix& A2, Key parent2,
    │ │ │ │ -
    101 const Vector& b, double sigma);
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27#include <boost/make_shared.hpp>
    │ │ │ │ +
    28#include <boost/serialization/version.hpp>
    │ │ │ │ +
    29#include <boost/serialization/split_member.hpp>
    │ │ │ │ +
    30
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │ +
    32
    │ │ │ │ +
    33 // Forward declarations
    │ │ │ │ +
    34 class HessianFactor;
    │ │ │ │ +
    35 class VariableSlots;
    │ │ │ │ +
    36 class GaussianFactorGraph;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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);
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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()); }
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
    │ │ │ │ +
    224 using Base::hessianDiagonal;
    │ │ │ │
    225
    │ │ │ │ -
    227 const constBVector d() const { return BaseFactor::getb(); }
    │ │ │ │ +
    227 void hessianDiagonalAdd(VectorValues& d) const override;
    │ │ │ │
    228
    │ │ │ │ -
    240 inline double determinant() const { return exp(logDeterminant()); }
    │ │ │ │ -
    241
    │ │ │ │ -
    253 double logDeterminant() const;
    │ │ │ │ +
    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
    │ │ │ │ +
    253 Matrix augmentedJacobianUnweighted() 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
    │ │ │ │ -
    │ │ │ │ +
    256 const VerticalBlockMatrix& matrixObject() const { return Ab_; }
    │ │ │ │ +
    257
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    297template<>
    │ │ │ │ -
    298struct traits<GaussianConditional> : public Testable<GaussianConditional> {};
    │ │ │ │ -
    299
    │ │ │ │ -
    300} // \ namespace gtsam
    │ │ │ │ +
    297 const constBVector getb() const { return Ab_(size()).col(0); }
    │ │ │ │ +
    298
    │ │ │ │ +
    300 constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
    │ │ │ │
    301
    │ │ │ │ - │ │ │ │ -
    303
    │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ - │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A matrix with column blocks of pre-defined sizes.
    │ │ │ │ +
    VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
    │ │ │ │ + │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    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
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ +
    Block range(DenseIndex startBlock, DenseIndex endBlock)
    access ranges of blocks at a time
    Definition VerticalBlockMatrix.h:130
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ +
    DenseIndex cols() const
    Column size.
    Definition VerticalBlockMatrix.h:118
    │ │ │ │
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    GaussianConditional This
    Typedef to this class.
    Definition GaussianConditional.h:45
    │ │ │ │ -
    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
    │ │ │ │ -
    constABlock R() const
    Return a view of the upper-triangular R block of the conditional.
    Definition GaussianConditional.h:218
    │ │ │ │ -
    JacobianFactor BaseFactor
    Typedef to our factor base class.
    Definition GaussianConditional.h:47
    │ │ │ │ -
    GaussianConditional()
    default constructor needed for serialization
    Definition GaussianConditional.h:54
    │ │ │ │ -
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition GaussianConditional.h:48
    │ │ │ │ -
    static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional)
    Combine several GaussianConditional into a single dense GC.
    │ │ │ │ -
    double determinant() const
    Compute the determinant of the R matrix.
    Definition GaussianConditional.h:240
    │ │ │ │ -
    double operator()(const VectorValues &x) const
    Evaluate probability density, sugar.
    Definition GaussianConditional.h:158
    │ │ │ │ -
    static shared_ptr sharedMeanAndStddev(Args &&... args)
    Create shared pointer by forwarding arguments to fromMeanAndStddev.
    Definition GaussianConditional.h:105
    │ │ │ │ -
    constABlock S() const
    Get a view of the parent blocks.
    Definition GaussianConditional.h:221
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ -
    const constBVector d() const
    Get a view of the r.h.s.
    Definition GaussianConditional.h:227
    │ │ │ │ +
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    A combined factor is assembled as one block of rows for each component factor.
    Definition VariableSlots.h:52
    │ │ │ │
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ +
    BVector getb()
    Get a view of the r.h.s.
    Definition JacobianFactor.h:306
    │ │ │ │ +
    const SharedDiagonal & get_model() const
    get a copy of model
    Definition JacobianFactor.h:291
    │ │ │ │ +
    JacobianFactor(const JacobianFactor &jf)
    Copy constructor.
    Definition JacobianFactor.h:114
    │ │ │ │ +
    JacobianFactor This
    Typedef to this class.
    Definition JacobianFactor.h:94
    │ │ │ │ +
    bool isConstrained() const
    is noise model constrained ?
    Definition JacobianFactor.h:269
    │ │ │ │ +
    constABlock getA() const
    Get a view of the A matrix, not weighted by noise.
    Definition JacobianFactor.h:303
    │ │ │ │ +
    GaussianFactor::shared_ptr clone() const override
    Clone this JacobianFactor.
    Definition JacobianFactor.h:189
    │ │ │ │ +
    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
    │ │ │ │ +
    SharedDiagonal & get_model()
    get a copy of model (non-const version)
    Definition JacobianFactor.h:294
    │ │ │ │ +
    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
    │ │ │ │ +
    VerticalBlockMatrix & matrixObject()
    Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
    Definition JacobianFactor.h:259
    │ │ │ │ +
    ABlock getA()
    Get a view of the A matrix.
    Definition JacobianFactor.h:312
    │ │ │ │ +
    GaussianFactor Base
    Typedef to base class.
    Definition JacobianFactor.h:95
    │ │ │ │ +
    ~JacobianFactor() override
    Virtual destructor.
    Definition JacobianFactor.h:186
    │ │ │ │ +
    const VerticalBlockMatrix & matrixObject() const
    Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
    Definition JacobianFactor.h:256
    │ │ │ │ +
    size_t rows() const
    return the number of rows in the corresponding linear system
    Definition JacobianFactor.h:283
    │ │ │ │ +
    size_t cols() const
    return the number of columns in the corresponding linear system
    Definition JacobianFactor.h:288
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ +
    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
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    is the normalization constant.
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,298 +1,502 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianConditional.h │ │ │ │ │ +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/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 │ │ │ │ │ +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_/_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 │ │ │ │ │ -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()); │ │ │ │ │ -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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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()); } │ │ │ │ │ +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 │ │ │ │ │ -_2_2_4 constABlock _S(_c_o_n_s_t___i_t_e_r_a_t_o_r it) const { return BaseFactor::getA(it); } │ │ │ │ │ +224 using Base::hessianDiagonal; │ │ │ │ │ 225 │ │ │ │ │ -_2_2_7 const constBVector _d() const { return BaseFactor::getb(); } │ │ │ │ │ +227 void hessianDiagonalAdd(_V_e_c_t_o_r_V_a_l_u_e_s& d) const override; │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ +253 Matrix augmentedJacobianUnweighted() 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 │ │ │ │ │ +_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(); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +_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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _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/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/KalmanFilter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    GaussianFactorGraph.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    KalmanFilter.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Linear Factor Graph where all factors are Gaussians. │ │ │ │ +

    Simple linear Kalman filter. │ │ │ │ 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

    │ │ │ │ -

    Linear Factor Graph where all factors are Gaussians.

    │ │ │ │ -
    Author
    Kai Ni
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +

    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,30 +1,24 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Simple linear Kalman filter. │ │ │ │ │ +Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ + Date │ │ │ │ │ + Sep 3, 2011 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Stephen Williams │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/linearExceptions.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,34 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianFactor.cpp File Reference
    │ │ │ │ +
    linearExceptions.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A factor with a quadratic error function - a Gaussian. │ │ │ │ +

    Exceptions that may be thrown by linear solver components. │ │ │ │ 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
    │ │ │ │ +

    Exceptions that may be thrown by linear solver components.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Aug 17, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,22 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -GaussianFactor │ │ │ │ │ +Exceptions that may be thrown by linear solver components. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + 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_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _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/a00842_source.html │ │ │ │ @@ -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/JacobianFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,216 +98,144 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SubgraphBuilder.h
    │ │ │ │ +
    JacobianFactor-inl.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -Go to the documentation of this file.
    1/* ----------------------------------------------------------------------------
    │ │ │ │ +
    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/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
    │ │ │ │ -
    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;
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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/**************************************************************************/
    │ │ │ │ -
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    124
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ +
    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 // Assign RHS vector
    │ │ │ │ +
    101 getb() = b;
    │ │ │ │ +
    102
    │ │ │ │ +
    103 // Assign noise model
    │ │ │ │ +
    104 model_ = noiseModel;
    │ │ │ │ +
    105 }
    │ │ │ │ +
    │ │ │ │ +
    106
    │ │ │ │ +
    107} // gtsam
    │ │ │ │ +
    108
    │ │ │ │ +
    Exceptions that may be thrown by linear solver components.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Definition SubgraphBuilder.h:43
    │ │ │ │ -
    Definition SubgraphBuilder.h:45
    │ │ │ │ -
    Definition SubgraphBuilder.h:96
    │ │ │ │ -
    double augmentationFactor
    factor multiplied with n, yields number of extra edges.
    Definition SubgraphBuilder.h:123
    │ │ │ │ -
    Definition SubgraphBuilder.h:147
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ +
    DenseIndex cols() const
    Column size.
    Definition VerticalBlockMatrix.h:118
    │ │ │ │ +
    DenseIndex nBlocks() const
    Block count.
    Definition VerticalBlockMatrix.h:121
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ +
    JacobianFactor()
    default constructor for I/O
    Definition JacobianFactor.cpp:54
    │ │ │ │ +
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ +
    An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
    Definition linearExceptions.h:106
    │ │ │ │ +
    An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
    Definition linearExceptions.h:124
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,230 +1,183 @@ │ │ │ │ │ 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_. │ │ │ │ │ +JacobianFactor-inl.h │ │ │ │ │ 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_/_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 │ │ │ │ │ -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; │ │ │ │ │ +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 } │ │ │ │ │ +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/**************************************************************************/ │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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 // 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_:_:_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_:_:_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 │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_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/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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianBayesNet.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    HessianFactor.cpp 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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

    │ │ │ │ +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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Chordal Bayes Net, the result of eliminating a factor graph.

    │ │ │ │ -

    GaussianBayesNet

    Author
    Frank Dellaert
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Dec 8, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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< 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 ********** │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -GaussianBayesNet │ │ │ │ │ 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_._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.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00848 = [ │ │ │ │ │ - ["gtsam::traits< GaussianBayesNet >", "a03744.html", null] │ │ │ │ │ + ["EliminateCholesky", "a00848.html#ga37760d3ca31c6d8d60a3a2a17babc3a4", null], │ │ │ │ │ + ["EliminatePreferCholesky", "a00848.html#gadbb147d2a9039f67ad3b8b5515d2e5cc", null] │ │ │ │ │ ]; │ │ │ ├── ./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/GaussianJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    iterative.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussianJunctionTree.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Iterative methods, implementation. │ │ │ │ -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

    │ │ │ │ -

    Iterative methods, implementation.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Dec 28, 2009
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,50 +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 │ │ │ │ │ -iterative.cpp File Reference │ │ │ │ │ -Iterative methods, implementation. _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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -Iterative methods, implementation. │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._c_p_p │ │ │ │ │ + * _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/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, GaussianFactorGraphgtsam::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
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ +
    124
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Errors operator+(const Errors &a, const Errors &b)
    Addition.
    Definition Errors.cpp:60
    │ │ │ │ -
    void axpy(double alpha, const Errors &x, Errors &y)
    BLAS level 2 style AXPY, y := alpha*x + y
    Definition Errors.cpp:111
    │ │ │ │ -
    Errors createErrors(const VectorValues &V)
    Break V into pieces according to its start indices.
    Definition Errors.cpp:29
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    Errors operator-(const Errors &a, const Errors &b)
    Subtraction.
    Definition Errors.cpp:75
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ - │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Definition SubgraphBuilder.h:43
    │ │ │ │ +
    Definition SubgraphBuilder.h:45
    │ │ │ │ +
    Definition SubgraphBuilder.h:96
    │ │ │ │ +
    double augmentationFactor
    factor multiplied with n, yields number of extra edges.
    Definition SubgraphBuilder.h:123
    │ │ │ │ +
    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/SubgraphPreconditioner.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,44 +96,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Sampler.h File Reference
    │ │ │ │ +
    SubgraphPreconditioner.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    sampling from a NoiseModel │ │ │ │ -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::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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    sampling from a NoiseModel

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Alex Cunningham
    │ │ │ │ +
    Date
    Dec 31, 2009
    │ │ │ │ +
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -Sampler.h File Reference │ │ │ │ │ -sampling from a NoiseModel _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 │ │ │ │ │ -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_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_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -sampling from a NoiseModel │ │ │ │ │ + Date │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_a_m_p_l_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/a00860.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00860 = [ │ │ │ │ │ - ["gtsam::Sampler", "a04020.html", "a04020"] │ │ │ │ │ + ["gtsam::SubgraphPreconditionerParameters", "a04048.html", null], │ │ │ │ │ + ["gtsam::SubgraphPreconditioner", "a04052.html", "a04052"] │ │ │ │ │ ]; │ │ │ ├── ./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/SubgraphPreconditioner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,79 +98,158 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Sampler.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    60 explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
    │ │ │ │ -
    61
    │ │ │ │ -
    65
    │ │ │ │ -
    66 size_t dim() const { return model_->dim(); }
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    21#include <gtsam/linear/Errors.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/linear/Preconditioner.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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    41 typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    62 VectorValues xbar_;
    │ │ │ │ +
    63 Errors b2bar_;
    │ │ │ │ +
    64
    │ │ │ │ +
    65 KeyInfo keyInfo_;
    │ │ │ │ + │ │ │ │
    67
    │ │ │ │ -
    68 Vector sigmas() const { return model_->sigmas(); }
    │ │ │ │ +
    68 public:
    │ │ │ │
    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);
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    80
    │ │ │ │ +
    81 ~SubgraphPreconditioner() override {}
    │ │ │ │
    82
    │ │ │ │ -
    83 protected:
    │ │ │ │ -
    85 Vector sampleDiagonal(const Vector& sigmas) const;
    │ │ │ │ -
    86};
    │ │ │ │ -
    │ │ │ │ -
    87
    │ │ │ │ -
    88} // namespace gtsam
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Some support classes for iterative solvers.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    vector of errors
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Sampling structure that keeps internal random number generators for diagonal distributions specified ...
    Definition Sampler.h:31
    │ │ │ │ -
    noiseModel::Diagonal::shared_ptr model_
    noiseModel created at generation
    Definition Sampler.h:34
    │ │ │ │ -
    std::mt19937_64 generator_
    generator
    Definition Sampler.h:37
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ + │ │ │ │ +
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ +
    Definition Preconditioner.h:24
    │ │ │ │ +
    Definition Preconditioner.h:64
    │ │ │ │ +
    Definition SubgraphBuilder.h:96
    │ │ │ │ +
    Definition SubgraphPreconditioner.h:40
    │ │ │ │ +
    Subgraph conditioner class, as explained in the RSS 2010 submission.
    Definition SubgraphPreconditioner.h:54
    │ │ │ │ +
    const GaussianFactorGraph & Ab2() const
    Access Ab2.
    Definition SubgraphPreconditioner.h:87
    │ │ │ │ +
    const Errors b2bar() const
    Access b2bar.
    Definition SubgraphPreconditioner.h:93
    │ │ │ │ +
    const GaussianBayesNet & Rc1() const
    Access Rc1.
    Definition SubgraphPreconditioner.h:90
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,80 +1,189 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Sampler.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 │ │ │ │ │ -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 │ │ │ │ │ -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(); } │ │ │ │ │ +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; │ │ │ │ │ +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 Vector sigmas() const { return model_->sigmas(); } │ │ │ │ │ +68 public: │ │ │ │ │ 69 │ │ │ │ │ -70 const noiseModel::Diagonal::shared_ptr& model() const { return model_; } │ │ │ │ │ +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 │ │ │ │ │ -75 │ │ │ │ │ -77 Vector sample() const; │ │ │ │ │ -78 │ │ │ │ │ -80 static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ +_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_r_r_o_r_s_._h │ │ │ │ │ +vector of errors │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_a_m_p_l_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/a00863.html │ │ │ │ @@ -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/linear/SparseEigen.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros
    │ │ │ │ -
    KalmanFilter.h File Reference
    │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    SparseEigen.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Simple linear Kalman filter. │ │ │ │ +

    Utilities for creating Eigen sparse matrices (gtsam::SparseEigen) │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::KalmanFilter
     Kalman Filter class. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ -Macros

    │ │ │ │ -#define KALMANFILTER_DEFAULT_FACTORIZATION   QR
     

    │ │ │ │ +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

    │ │ │ │ -

    Simple linear Kalman filter.

    │ │ │ │ -

    Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

    Date
    Sep 3, 2011
    │ │ │ │ -
    Author
    Stephen Williams
    │ │ │ │ +

    Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)

    │ │ │ │ +
    Date
    Aug 2019
    │ │ │ │ +
    Author
    Mandy Xie
    │ │ │ │ +
    │ │ │ │ +Fan Jiang
    │ │ │ │ +
    │ │ │ │ +Gerry Chen
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -KalmanFilter.h 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 │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  KKAALLMMAANNFFIILLTTEERR__DDEEFFAAUULLTT__FFAACCTTOORRIIZZAATTIIOONN   QR │ │ │ │ │ +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 ********** │ │ │ │ │ -Simple linear Kalman filter. │ │ │ │ │ -Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ +Utilities for creating Eigen sparse matrices (_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n) │ │ │ │ │ Date │ │ │ │ │ - Sep 3, 2011 │ │ │ │ │ + Aug 2019 │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ + Mandy Xie │ │ │ │ │ + Fan Jiang │ │ │ │ │ + Gerry Chen │ │ │ │ │ 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 │ │ │ │ │ + * _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/a00863.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00863 = [ │ │ │ │ │ - ["gtsam::KalmanFilter", "a03852.html", "a03852"] │ │ │ │ │ + ["SparseEigen", "a00863.html#a52b9b28b076a8ae17d6d3d72ee1229c3", null], │ │ │ │ │ + ["sparseJacobianEigen", "a00863.html#aa9013a9053629f0c00b97726d9d8e19e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00863_source.html │ │ │ │ @@ -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/linear/SparseEigen.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,135 +98,79 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    KalmanFilter.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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    24#pragma once
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    50 QR, CHOLESKY
    │ │ │ │ -
    51 };
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │ +
    29#include <Eigen/Sparse>
    │ │ │ │ +
    30
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │ +
    32
    │ │ │ │ +
    35typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
    │ │ │ │ +
    36
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │
    │ │ │ │ -
    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
    │ │ │ │ -
    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) :
    │ │ │ │ - │ │ │ │ -
    75 }
    │ │ │ │ -
    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;
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    A Gaussian Density.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    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
    │ │ │ │ +
    58SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
    │ │ │ │ +
    59 gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
    │ │ │ │ +
    60 return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
    │ │ │ │ +
    61}
    │ │ │ │ +
    62
    │ │ │ │ +
    63} // namespace gtsam
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen
    Eigen-format sparse matrix.
    Definition SparseEigen.h:35
    │ │ │ │ +
    SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
    Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
    Definition SparseEigen.h:38
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Kalman Filter class.
    Definition KalmanFilter.h:41
    │ │ │ │ -
    Factorization
    This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
    Definition KalmanFilter.h:49
    │ │ │ │ -
    static Key step(const State &p)
    Return step index k, starts at 0, incremented at each predict.
    Definition KalmanFilter.h:92
    │ │ │ │ -
    GaussianDensity::shared_ptr State
    The Kalman filter state is simply a GaussianDensity.
    Definition KalmanFilter.h:56
    │ │ │ │ +
    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,167 +1,89 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KalmanFilter.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 │ │ │ │ │ -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> │ │ │ │ │ +24#pragma once │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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} // \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. │ │ │ │ │ +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. │ │ │ │ │ -_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _K_a_l_m_a_n_F_i_l_t_e_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/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/GaussianConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianISAM.h File Reference
    │ │ │ │ +
    GaussianConditional.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Conditional Gaussian Base class. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::GaussianISAM
    class  gtsam::GaussianConditional
     A GaussianConditional functions as the node in a Bayes network. More...
     
    struct  gtsam::traits< GaussianISAM >
     traits More...
    struct  gtsam::traits< GaussianConditional >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

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

    Conditional Gaussian Base class.

    │ │ │ │ +
    Author
    Christian Potthast
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -GaussianISAM.h File Reference │ │ │ │ │ +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_I_S_A_M │ │ │ │ │ + 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_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _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_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 │ │ │ │ │ +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_I_S_A_M_._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/a00866.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00866 = [ │ │ │ │ │ - ["gtsam::GaussianISAM", "a03800.html", "a03800"], │ │ │ │ │ - ["gtsam::traits< GaussianISAM >", "a03804.html", null] │ │ │ │ │ + ["gtsam::traits< GaussianConditional >", "a03768.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00866_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,70 +98,224 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianISAM.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <gtsam/base/Testable.h>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ -
    27 class GTSAM_EXPORT GaussianISAM : public ISAM<GaussianBayesTree>
    │ │ │ │ -
    28 {
    │ │ │ │ -
    29 public:
    │ │ │ │ - │ │ │ │ -
    31 typedef GaussianISAM This;
    │ │ │ │ -
    32 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    18// \callgraph
    │ │ │ │ +
    19
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <boost/utility.hpp>
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    27#include <gtsam/inference/Conditional-inst.h>
    │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30#include <random> // for std::mt19937_64
    │ │ │ │ +
    31
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │
    33
    │ │ │ │ -
    36
    │ │ │ │ - │ │ │ │ -
    39
    │ │ │ │ -
    41 GaussianISAM(const GaussianBayesTree& bayesTree);
    │ │ │ │ -
    42
    │ │ │ │ -
    44
    │ │ │ │ -
    45 };
    │ │ │ │ -
    │ │ │ │ -
    46
    │ │ │ │ -
    48 template <>
    │ │ │ │ -
    49 struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
    │ │ │ │ -
    50
    │ │ │ │ -
    51}
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    │ │ │ │ +
    40 class GTSAM_EXPORT GaussianConditional :
    │ │ │ │ +
    41 public JacobianFactor,
    │ │ │ │ +
    42 public Conditional<JacobianFactor, GaussianConditional>
    │ │ │ │ +
    43 {
    │ │ │ │ +
    44 public:
    │ │ │ │ + │ │ │ │ +
    46 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    49
    │ │ │ │ +
    52
    │ │ │ │ + │ │ │ │ +
    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());
    │ │ │ │ +
    77
    │ │ │ │ +
    82 template<typename KEYS>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    303
    │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ -
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ -
    Definition GaussianISAM.h:28
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ +
    GaussianConditional This
    Typedef to this class.
    Definition GaussianConditional.h:45
    │ │ │ │ +
    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
    │ │ │ │ +
    constABlock R() const
    Return a view of the upper-triangular R block of the conditional.
    Definition GaussianConditional.h:218
    │ │ │ │ +
    JacobianFactor BaseFactor
    Typedef to our factor base class.
    Definition GaussianConditional.h:47
    │ │ │ │ +
    GaussianConditional()
    default constructor needed for serialization
    Definition GaussianConditional.h:54
    │ │ │ │ +
    Conditional< BaseFactor, This > BaseConditional
    Typedef to our conditional base class.
    Definition GaussianConditional.h:48
    │ │ │ │ +
    static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional)
    Combine several GaussianConditional into a single dense GC.
    │ │ │ │ +
    double determinant() const
    Compute the determinant of the R matrix.
    Definition GaussianConditional.h:240
    │ │ │ │ +
    double operator()(const VectorValues &x) const
    Evaluate probability density, sugar.
    Definition GaussianConditional.h:158
    │ │ │ │ +
    static shared_ptr sharedMeanAndStddev(Args &&... args)
    Create shared pointer by forwarding arguments to fromMeanAndStddev.
    Definition GaussianConditional.h:105
    │ │ │ │ +
    constABlock S() const
    Get a view of the parent blocks.
    Definition GaussianConditional.h:221
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    const constBVector d() const
    Get a view of the r.h.s.
    Definition GaussianConditional.h:227
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,75 +1,298 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianISAM.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 │ │ │ │ │ -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_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_2_7 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_I_S_A_M : public _I_S_A_M │ │ │ │ │ -28 { │ │ │ │ │ -29 public: │ │ │ │ │ -30 typedef _I_S_A_M_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ -31 typedef _G_a_u_s_s_i_a_n_I_S_A_M _T_h_i_s; │ │ │ │ │ -32 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +18// \callgraph │ │ │ │ │ +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 │ │ │ │ │ -36 │ │ │ │ │ -38 _G_a_u_s_s_i_a_n_I_S_A_M(); │ │ │ │ │ -39 │ │ │ │ │ -41 _G_a_u_s_s_i_a_n_I_S_A_M(const _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ -42 │ │ │ │ │ -44 │ │ │ │ │ -45 }; │ │ │ │ │ -46 │ │ │ │ │ -48 template <> │ │ │ │ │ -_4_9 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_I_S_A_M> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -50 │ │ │ │ │ -51} │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_I_S_A_M_._h │ │ │ │ │ -Incremental update functionality (iSAM) for 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. │ │ │ │ │ +_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 │ │ │ │ │ +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()); │ │ │ │ │ +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 │ │ │ │ │ +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 } │ │ │ │ │ +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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_I_S_A_M │ │ │ │ │ -DDeeffiinniittiioonn GaussianISAM.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_:_:_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_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_I_S_A_M_._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/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/VectorValues.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SubgraphSolver.cpp File Reference
    │ │ │ │ +
    VectorValues.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Subgraph Solver from IROS 2010. │ │ │ │ +

    Factor Graph Values. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    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 >
     traits 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
    │ │ │ │ +

    Factor Graph Values.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,29 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +VectorValues.h File Reference │ │ │ │ │ +Factor Graph _V_a_l_u_e_s. _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_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +  _V_e_c_t_o_r_V_a_l_u_e_s represents a collection of vector-valued variables │ │ │ │ │ + associated each with a unique integer index. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_e_c_t_o_r_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 ********** │ │ │ │ │ -Subgraph Solver from IROS 2010. │ │ │ │ │ - Date │ │ │ │ │ - 2010 │ │ │ │ │ +Factor Graph _V_a_l_u_e_s. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _V_e_c_t_o_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/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/iterative-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    HessianFactor-inl.h File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    iterative-inl.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Contains the HessianFactor class, a general quadratic factor. │ │ │ │ +

    Iterative methods, template implementation. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    struct  gtsam::CGState< S, V, E >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

    template<class S , class V , class E >
    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

    │ │ │ │ -

    Contains the HessianFactor class, a general quadratic factor.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Dec 8, 2010
    │ │ │ │ +

    Iterative methods, template implementation.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Dec 28, 2009
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,34 @@ │ │ │ │ │ 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 | _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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +Iterative methods, template implementation. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ + Dec 28, 2009 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/iterative-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HessianFactor-inl.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,49 +114,163 @@ │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    27 /* ************************************************************************* */
    │ │ │ │ +
    28 // state for CG method
    │ │ │ │ +
    29 template<class S, class V, class E>
    │ │ │ │ +
    │ │ │ │ +
    30 struct CGState {
    │ │ │ │ +
    31
    │ │ │ │ + │ │ │ │ +
    33 const Parameters &parameters_;
    │ │ │ │ +
    34
    │ │ │ │ +
    35 int k;
    │ │ │ │ +
    36 bool steepest;
    │ │ │ │ +
    37 V g, d;
    │ │ │ │ +
    38 double gamma, threshold;
    │ │ │ │ +
    39 E Ad;
    │ │ │ │
    40
    │ │ │ │ -
    41}
    │ │ │ │ +
    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
    │ │ │ │ +
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ +
    Iterative methods, implementation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    Matrix augmentedInformation() const override
    Return the augmented information matrix represented by this GaussianFactor.
    Definition HessianFactor.cpp:282
    │ │ │ │ -
    HessianFactor()
    default constructor for I/O
    Definition HessianFactor.cpp:77
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ +
    Definition iterative-inl.h:30
    │ │ │ │ +
    bool steepest
    flag to indicate we are doing steepest descent
    Definition iterative-inl.h:36
    │ │ │ │ +
    double threshold
    gamma (squared L2 norm of g) and convergence threshold
    Definition iterative-inl.h:38
    │ │ │ │ +
    int k
    iteration
    Definition iterative-inl.h:35
    │ │ │ │ +
    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 │ │ │ │ │ -HessianFactor-inl.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,65 +16,191 @@ │ │ │ │ │ 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 } │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +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} │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._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/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/GaussianFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,51 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    SubgraphBuilder.cpp File Reference
    │ │ │ │ +
    GaussianFactorGraph.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    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

    │ │ │ │ -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, GaussianFactorGraphgtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
     Split the graph into a subgraph and the remaining edges.
     
    bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
     Evaluates whether linear factors have any constrained noise models.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Dec 31, 2009
    │ │ │ │ -
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │ +

    Linear Factor Graph where all factors are Gaussians.

    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +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 │ │ │ │ │ -SubgraphBuilder.cpp File Reference │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSppaarrsseeTTrriipplleettss = std::vector< std::tuple< int, int, double > > │ │ │ │ │   │ │ │ │ │ - _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 │ │ │ │ │ +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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Kai Ni │ │ │ │ │ + Christian Potthast │ │ │ │ │ + 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 │ │ │ │ │ + * _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/a00875.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00875 = [ │ │ │ │ │ - ["buildFactorSubgraph", "a00875.html#a0ec82adc6e13261cf4a012b65b301256", null], │ │ │ │ │ - ["splitFactorGraph", "a00875.html#a2c7eca9f27b43b52756c1afd85478dd8", null] │ │ │ │ │ + ["hasConstraints", "a00875.html#a35c269c3243cab16a7475239a9c91021", null] │ │ │ │ │ ]; │ │ │ ├── ./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/JacobianFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianJunctionTree.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    JacobianFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ 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_ptrgtsam::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

    │ │ │ │ -
    Date
    Mar 29, 2013
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    Dec 8, 2010
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ +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). │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Sampler.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,76 +94,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    iterative.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    Sampler.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Iterative methods, implementation. │ │ │ │ +

    sampling from a diagonal NoiseModel │ │ │ │ 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ Namespaces

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

    │ │ │ │ -Functions

    template<class S , class V , class E >
    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

    │ │ │ │ -

    Iterative methods, implementation.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Dec 28, 2009
    │ │ │ │ +

    sampling from a diagonal NoiseModel

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,68 +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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -Iterative methods, implementation. │ │ │ │ │ +sampling from a diagonal NoiseModel │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ + * _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/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/HessianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,58 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianEliminationTree.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    HessianFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Contains the HessianFactor class, a general quadratic factor. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::HessianFactor
     A Gaussian factor using the canonical parameters (information form) More...
     
    struct  gtsam::traits< HessianFactor >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +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.
     
    │ │ │ │

    Detailed Description

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

    Contains the HessianFactor class, a general quadratic factor.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Dec 8, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,44 @@ │ │ │ │ │ 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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HessianFactor.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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +  A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_e_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 │ │ │ │ │ + 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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +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_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _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/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/GaussianDensity.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    RegularHessianFactor.h File Reference
    │ │ │ │ +
    GaussianDensity.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    HessianFactor class with constant sized blocks. │ │ │ │ +

    A Gaussian Density. │ │ │ │ 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

    │ │ │ │ -

    HessianFactor class with constant sized blocks.

    │ │ │ │ -
    Author
    Sungtae An
    │ │ │ │ -
    Date
    March 2014
    │ │ │ │ +

    A Gaussian Density.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Jan 21, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -HessianFactor class with constant sized blocks. │ │ │ │ │ +A Gaussian Density. │ │ │ │ │ Author │ │ │ │ │ - Sungtae An │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - March 2014 │ │ │ │ │ + Jan 21, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/RegularJacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,41 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SubgraphPreconditioner.h File Reference
    │ │ │ │ +
    RegularJacobianFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    JacobianFactor class with fixed sized blcoks. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::SubgraphPreconditionerParameters
     
    class  gtsam::SubgraphPreconditioner
     Subgraph conditioner class, as explained in the RSS 2010 submission. More...
    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

    │ │ │ │ -
    Date
    Dec 31, 2009
    │ │ │ │ -
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │ +

    JacobianFactor class with fixed sized blcoks.

    │ │ │ │ +
    Author
    Sungtae An
    │ │ │ │ +
    Date
    Nov 11, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -SubgraphPreconditioner.h File Reference │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Sungtae An │ │ │ │ │ + Date │ │ │ │ │ + Nov 11, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00896.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00896 = [ │ │ │ │ │ - ["gtsam::SubgraphPreconditionerParameters", "a04048.html", null], │ │ │ │ │ - ["gtsam::SubgraphPreconditioner", "a04052.html", "a04052"] │ │ │ │ │ + ["gtsam::RegularJacobianFactor< D >", "a04016.html", "a04016"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00896_source.html │ │ │ │ @@ -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/RegularJacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,158 +98,215 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SubgraphPreconditioner.h
    │ │ │ │ +
    RegularJacobianFactor.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ -
    21#include <gtsam/linear/Errors.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/linear/Preconditioner.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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    41 typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    62 VectorValues xbar_;
    │ │ │ │ -
    63 Errors b2bar_;
    │ │ │ │ -
    64
    │ │ │ │ -
    65 KeyInfo keyInfo_;
    │ │ │ │ - │ │ │ │ -
    67
    │ │ │ │ -
    68 public:
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    31template<size_t D>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    65 const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
    │ │ │ │ +
    66 SharedDiagonal()) :
    │ │ │ │ +
    67 JacobianFactor(keys, augmentedMatrix, sigmas) {
    │ │ │ │ +
    68 }
    │ │ │ │ +
    │ │ │ │
    69
    │ │ │ │ - │ │ │ │ + │ │ │ │
    71
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    │ │ │ │ +
    73 void multiplyHessianAdd(double alpha, const VectorValues& x,
    │ │ │ │ +
    74 VectorValues& y) const override {
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Some support classes for iterative solvers.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ - │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    vector of errors
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ - │ │ │ │ -
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ -
    Definition Preconditioner.h:24
    │ │ │ │ -
    Definition Preconditioner.h:64
    │ │ │ │ -
    Definition SubgraphBuilder.h:96
    │ │ │ │ -
    Definition SubgraphPreconditioner.h:40
    │ │ │ │ -
    Subgraph conditioner class, as explained in the RSS 2010 submission.
    Definition SubgraphPreconditioner.h:54
    │ │ │ │ -
    const GaussianFactorGraph & Ab2() const
    Access Ab2.
    Definition SubgraphPreconditioner.h:87
    │ │ │ │ -
    const Errors b2bar() const
    Access b2bar.
    Definition SubgraphPreconditioner.h:93
    │ │ │ │ -
    const GaussianBayesNet & Rc1() const
    Access Rc1.
    Definition SubgraphPreconditioner.h:90
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ +
    VectorValues hessianDiagonal() const
    Return the diagonal of the Hessian for this factor.
    Definition GaussianFactor.cpp:35
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition JacobianFactor.cpp:649
    │ │ │ │ +
    VectorValues gradientAtZero() const override
    A'*b for Jacobian.
    Definition JacobianFactor.cpp:701
    │ │ │ │ +
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ +
    Vector operator*(const double *x) const
    double* Matrix-vector multiply, i.e.
    Definition RegularJacobianFactor.h:172
    │ │ │ │ +
    void hessianDiagonal(double *d) const override
    Raw memory access version of hessianDiagonal.
    Definition RegularJacobianFactor.h:109
    │ │ │ │ +
    void gradientAtZero(double *d) const override
    Raw memory access version of gradientAtZero.
    Definition RegularJacobianFactor.h:133
    │ │ │ │ +
    void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
    double* Transpose Matrix-vector multiply, i.e.
    Definition RegularJacobianFactor.h:161
    │ │ │ │ +
    VectorValues gradientAtZero() const override
    Expose base class gradientAtZero.
    Definition RegularJacobianFactor.h:128
    │ │ │ │ +
    RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Construct an n-ary factor.
    Definition RegularJacobianFactor.h:52
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition RegularJacobianFactor.h:73
    │ │ │ │ +
    RegularJacobianFactor()
    Default constructor.
    Definition RegularJacobianFactor.h:44
    │ │ │ │ +
    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
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const double *x, double *y) const
    double* Hessian-vector multiply, i.e.
    Definition RegularJacobianFactor.h:82
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,189 +1,269 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SubgraphPreconditioner.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-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; │ │ │ │ │ -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: │ │ │ │ │ +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 _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()); │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_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 │ │ │ │ │ +_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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ -Some support classes for iterative solvers. │ │ │ │ │ +_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. │ │ │ │ │ -_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. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -_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_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._h │ │ │ │ │ + * _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/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/HessianFactor-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,64 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    GaussianFactorGraph.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    HessianFactor-inl.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Linear Factor Graph where all factors are Gaussians. │ │ │ │ +

    Contains the HessianFactor class, a general quadratic factor. │ │ │ │ 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Linear Factor Graph where all factors are Gaussians.

    │ │ │ │ -
    Author
    Kai Ni
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ -
    │ │ │ │ -Alireza Fathi
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +

    Contains the HessianFactor class, a general quadratic factor.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Dec 8, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -GaussianFactorGraph.h File Reference │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. _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 │ │ │ │ │ -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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Alireza Fathi │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/HessianFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,338 +98,65 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianFactorGraph.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    31
    │ │ │ │ -
    32namespace gtsam {
    │ │ │ │ +
    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 // Forward declarations
    │ │ │ │ -
    35 class GaussianFactorGraph;
    │ │ │ │ -
    36 class GaussianFactor;
    │ │ │ │ - │ │ │ │ -
    38 class GaussianBayesNet;
    │ │ │ │ -
    39 class GaussianEliminationTree;
    │ │ │ │ -
    40 class GaussianBayesTree;
    │ │ │ │ -
    41 class GaussianJunctionTree;
    │ │ │ │ -
    42
    │ │ │ │ -
    43 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    45 {
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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); }
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    81 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    82
    │ │ │ │ -
    85
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    108
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
    │ │ │ │ -
    313
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Variable elimination algorithms for factor graphs.
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ - │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    vector of errors
    │ │ │ │ -
    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
    │ │ │ │ +
    40
    │ │ │ │ +
    41}
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    bool hasConstraints(const GaussianFactorGraph &factors)
    Evaluates whether linear factors have any constrained noise models.
    Definition GaussianFactorGraph.cpp:442
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    bool isEqual(const FactorGraph &other) const
    Check exact equality of the factor pointers. Useful for derived ==.
    Definition FactorGraph.h:134
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ -
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ -
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ -
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    Definition GaussianEliminationTree.h:29
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    virtual GaussianFactor::shared_ptr clone() const =0
    Clone a factor (make a deep copy)
    │ │ │ │ -
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition GaussianFactorGraph.h:58
    │ │ │ │ -
    GaussianBayesTree BayesTreeType
    Type of Bayes tree.
    Definition GaussianFactorGraph.h:51
    │ │ │ │ -
    GaussianConditional ConditionalType
    Type of conditionals from elimination.
    Definition GaussianFactorGraph.h:48
    │ │ │ │ -
    GaussianFactor FactorType
    Type of factors in factor graph.
    Definition GaussianFactorGraph.h:46
    │ │ │ │ -
    GaussianEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition GaussianFactorGraph.h:50
    │ │ │ │ -
    GaussianFactorGraph FactorGraphType
    Type of the factor graph (e.g. GaussianFactorGraph)
    Definition GaussianFactorGraph.h:47
    │ │ │ │ -
    GaussianBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition GaussianFactorGraph.h:49
    │ │ │ │ -
    GaussianJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition GaussianFactorGraph.h:52
    │ │ │ │ -
    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
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    EliminateableFactorGraph< This > BaseEliminateable
    Typedef to base elimination class.
    Definition GaussianFactorGraph.h:80
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Add an n-ary factor.
    Definition GaussianFactorGraph.h:156
    │ │ │ │ -
    GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
    Construct from an initializer lists of GaussianFactor shared pointers.
    Definition GaussianFactorGraph.h:94
    │ │ │ │ -
    GaussianFactorGraph()
    Default constructor.
    Definition GaussianFactorGraph.h:87
    │ │ │ │ -
    void add(const GaussianFactor &factor)
    Add a factor by value - makes a copy.
    Definition GaussianFactorGraph.h:127
    │ │ │ │ -
    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
    │ │ │ │ -
    void add(const sharedFactor &factor)
    Add a factor by pointer - stores pointer without copying the factor.
    Definition GaussianFactorGraph.h:130
    │ │ │ │ -
    friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
    Check exact equality.
    Definition GaussianFactorGraph.h:121
    │ │ │ │ -
    GaussianFactorGraph This
    Typedef to this class.
    Definition GaussianFactorGraph.h:78
    │ │ │ │ -
    KeySet Keys
    Return the set of variables involved in the factors (computes a set union).
    Definition GaussianFactorGraph.h:163
    │ │ │ │ -
    void add(const Vector &b)
    Add a null factor.
    Definition GaussianFactorGraph.h:133
    │ │ │ │ -
    void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
    Add a unary factor.
    Definition GaussianFactorGraph.h:137
    │ │ │ │ -
    virtual ~GaussianFactorGraph()
    Virtual destructor.
    Definition GaussianFactorGraph.h:110
    │ │ │ │ -
    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
    │ │ │ │ -
    GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition GaussianFactorGraph.h:99
    │ │ │ │ -
    FactorGraph< GaussianFactor > Base
    Typedef to base factor graph type.
    Definition GaussianFactorGraph.h:79
    │ │ │ │ -
    GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition GaussianFactorGraph.h:107
    │ │ │ │ -
    GaussianFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition GaussianFactorGraph.h:103
    │ │ │ │ -
    A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
    Definition GaussianJunctionTree.h:39
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    is the normalization constant.
    │ │ │ │ +
    Matrix augmentedInformation() const override
    Return the augmented information matrix represented by this GaussianFactor.
    Definition HessianFactor.cpp:282
    │ │ │ │ +
    HessianFactor()
    default constructor for I/O
    Definition HessianFactor.cpp:77
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,493 +1,80 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianFactorGraph.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 │ │ │ │ │ -22#pragma once │ │ │ │ │ -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 /* ************************************************************************* │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21namespace _g_t_s_a_m { │ │ │ │ │ +22 │ │ │ │ │ +23 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -_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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_F_a_c_t_o_r_G_r_a_p_h_._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/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/ManifoldPreintegration.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,24 +96,24 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PreintegrationParams.h File Reference
    │ │ │ │ +
    ManifoldPreintegration.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...
    class  gtsam::ManifoldPreintegration
     IMU pre-integration on NavSatet manifold. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -125,21 +125,19 @@ │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │ Vadim Indelman
    │ │ │ │
    │ │ │ │ David Jensen
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ 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 │ │ │ │ │ -PreintegrationParams.h File Reference │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** 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 │ │ │ │ │ + * _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/a00905.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00905 = [ │ │ │ │ │ - ["gtsam::PreintegrationParams", "a04220.html", "a04220"] │ │ │ │ │ + ["gtsam::ManifoldPreintegration", "a04192.html", "a04192"] │ │ │ │ │ ]; │ │ │ ├── ./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/ManifoldPreintegration.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,120 +98,124 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PreintegrationParams.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
    │ │ │ │ -
    17#pragma once
    │ │ │ │ -
    18
    │ │ │ │ - │ │ │ │ -
    20#include <boost/make_shared.hpp>
    │ │ │ │ -
    21
    │ │ │ │ -
    22namespace gtsam {
    │ │ │ │ +
    22#pragma once
    │ │ │ │
    23
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    32 Vector3 n_gravity;
    │ │ │ │ -
    33
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    37 accelerometerCovariance(I_3x3),
    │ │ │ │ -
    38 integrationCovariance(I_3x3),
    │ │ │ │ -
    39 use2ndOrderCoriolis(false),
    │ │ │ │ -
    40 n_gravity(0, 0, -1) {}
    │ │ │ │ -
    │ │ │ │ -
    41
    │ │ │ │ -
    │ │ │ │ -
    44 PreintegrationParams(const Vector3& n_gravity)
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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 }
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    │ │ │ │ +
    33class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
    │ │ │ │ +
    34 protected:
    │ │ │ │ +
    35
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    50 resetIntegration();
    │ │ │ │ +
    51 }
    │ │ │ │ +
    │ │ │ │ +
    52
    │ │ │ │ +
    53public:
    │ │ │ │ +
    56
    │ │ │ │ +
    62 ManifoldPreintegration(const boost::shared_ptr<Params>& p,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    87#ifdef GTSAM_USE_QUATERNIONS
    │ │ │ │ -
    88 // Align if we are using Quaternions
    │ │ │ │ -
    89public:
    │ │ │ │ - │ │ │ │ -
    91#endif
    │ │ │ │ -
    92};
    │ │ │ │ -
    │ │ │ │ -
    93
    │ │ │ │ -
    94} // namespace gtsam
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ - │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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}
    │ │ │ │ + │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegratedRotation.h:31
    │ │ │ │ -
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegrationParams.h:26
    │ │ │ │ -
    Matrix3 accelerometerCovariance
    Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/s²/√Hz.
    Definition PreintegrationParams.h:29
    │ │ │ │ -
    bool use2ndOrderCoriolis
    Whether to use second order Coriolis integration.
    Definition PreintegrationParams.h:31
    │ │ │ │ -
    PreintegrationParams()
    Default constructor for serialization only.
    Definition PreintegrationParams.h:35
    │ │ │ │ -
    Vector3 n_gravity
    Gravity vector in nav frame.
    Definition PreintegrationParams.h:32
    │ │ │ │ -
    PreintegrationParams(const Vector3 &n_gravity)
    The Params constructor insists on getting the navigation frame gravity vector For convenience,...
    Definition PreintegrationParams.h:44
    │ │ │ │ -
    Matrix3 integrationCovariance
    continuous-time "Covariance" describing integration uncertainty
    Definition PreintegrationParams.h:30
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │ +
    ManifoldPreintegration()
    Default constructor for serialization.
    Definition ManifoldPreintegration.h:49
    │ │ │ │ +
    Matrix3 delVdelBiasAcc_
    Jacobian of preintegrated velocity w.r.t. acceleration bias.
    Definition ManifoldPreintegration.h:45
    │ │ │ │ +
    Matrix3 delRdelBiasOmega_
    Jacobian of preintegrated rotation w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:42
    │ │ │ │ +
    Matrix3 delPdelBiasAcc_
    Jacobian of preintegrated position w.r.t. acceleration bias.
    Definition ManifoldPreintegration.h:43
    │ │ │ │ +
    NavState deltaXij_
    Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
    Definition ManifoldPreintegration.h:41
    │ │ │ │ +
    Matrix3 delPdelBiasOmega_
    Jacobian of preintegrated position w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:44
    │ │ │ │ +
    virtual boost::shared_ptr< ManifoldPreintegration > clone() const
    Dummy clone for MATLAB.
    Definition ManifoldPreintegration.h:109
    │ │ │ │ +
    Matrix3 delVdelBiasOmega_
    Jacobian of preintegrated velocity w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:46
    │ │ │ │ +
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ +
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,151 +1,148 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegrationParams.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 │ │ │ │ │ -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 } │ │ │ │ │ -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 │ │ │ │ │ -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 } │ │ │ │ │ +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 { │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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} │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._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/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/ScenarioRunner.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,53 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PreintegratedRotation.h File Reference
    │ │ │ │ +
    ScenarioRunner.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Simple class to test navigation scenarios. │ │ │ │ +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::ScenarioRunner
     
    struct  gtsam::traits< PreintegratedRotation >
    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,36 +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 │ │ │ │ │ -PreintegratedRotation.h File Reference │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │   │ │ │ │ │ - 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_ _> │ │ │ │ │ +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 │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ + * _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/a00908.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,4 @@ │ │ │ │ │ var a00908 = [ │ │ │ │ │ - ["gtsam::PreintegratedRotationParams", "a04204.html", "a04204"], │ │ │ │ │ - ["gtsam::PreintegratedRotation", "a04208.html", "a04208"], │ │ │ │ │ - ["gtsam::traits< PreintegratedRotation >", "a04212.html", null] │ │ │ │ │ + ["gtsam::ScenarioRunner", "a04236.html", "a04236"], │ │ │ │ │ + ["gtsam::CombinedScenarioRunner", "a04240.html", "a04240"] │ │ │ │ │ ]; │ │ │ ├── ./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/ScenarioRunner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,203 +98,177 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PreintegratedRotation.h
    │ │ │ │ +
    ScenarioRunner.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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │ - │ │ │ │ -
    25#include <gtsam/base/Matrix.h>
    │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ -
    31struct GTSAM_EXPORT PreintegratedRotationParams {
    │ │ │ │ - │ │ │ │ -
    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 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:
    │ │ │ │ - │ │ │ │ -
    81#endif
    │ │ │ │ -
    82};
    │ │ │ │ -
    │ │ │ │ -
    83
    │ │ │ │ -
    │ │ │ │ -
    89class GTSAM_EXPORT PreintegratedRotation {
    │ │ │ │ -
    90 public:
    │ │ │ │ - │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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); }
    │ │ │ │ +
    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
    │ │ │ │ -
    93 protected:
    │ │ │ │ -
    95 boost::shared_ptr<Params> p_;
    │ │ │ │ -
    96
    │ │ │ │ -
    97 double deltaTij_;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    100
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    194#endif
    │ │ │ │ -
    195};
    │ │ │ │ -
    │ │ │ │ -
    196
    │ │ │ │ -
    197template <>
    │ │ │ │ -
    198struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
    │ │ │ │ -
    199
    │ │ │ │ -
    200}
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    3D Pose
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    133 double T, const Bias& estimatedBias = Bias(),
    │ │ │ │ +
    134 bool corrupted = false) const;
    │ │ │ │ +
    135
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    sampling from a NoiseModel
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Simple class to test navigation scenarios.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegratedRotation.h:31
    │ │ │ │ -
    boost::optional< Pose3 > body_P_sensor
    The pose of the sensor in the body frame.
    Definition PreintegratedRotation.h:36
    │ │ │ │ -
    boost::optional< Vector3 > omegaCoriolis
    Coriolis constant.
    Definition PreintegratedRotation.h:35
    │ │ │ │ -
    Matrix3 gyroscopeCovariance
    Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
    Definition PreintegratedRotation.h:34
    │ │ │ │ -
    PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
    Definition PreintegratedRotation.h:89
    │ │ │ │ -
    Matrix3 delRdelBiasOmega_
    Jacobian of preintegrated rotation w.r.t. angular rate bias.
    Definition PreintegratedRotation.h:99
    │ │ │ │ -
    boost::shared_ptr< Params > p_
    Parameters.
    Definition PreintegratedRotation.h:95
    │ │ │ │ -
    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
    │ │ │ │ -
    PreintegratedRotation(const boost::shared_ptr< Params > &p)
    Default constructor, resets integration to zero.
    Definition PreintegratedRotation.h:109
    │ │ │ │ -
    double deltaTij_
    Time interval from i to j.
    Definition PreintegratedRotation.h:97
    │ │ │ │ -
    bool matchesParamsWith(const PreintegratedRotation &other) const
    check parameters equality: checks whether shared pointer points to same Params object.
    Definition PreintegratedRotation.h:128
    │ │ │ │ -
    PreintegratedRotation()
    Default constructor for serialization.
    Definition PreintegratedRotation.h:102
    │ │ │ │ -
    Rot3 deltaRij_
    Preintegrated relative orientation (in frame i)
    Definition PreintegratedRotation.h:98
    │ │ │ │ +
    Matrix3 transpose() const
    Return 3*3 transpose (inverse) rotation matrix.
    Definition Rot3M.cpp:144
    │ │ │ │ +
    static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
    A Gaussian noise model created by specifying a covariance matrix.
    Definition NoiseModel.cpp:117
    │ │ │ │ +
    Sampling structure that keeps internal random number generators for diagonal distributions specified ...
    Definition Sampler.h:31
    │ │ │ │ +
    Vector sample() const
    sample from distribution
    Definition Sampler.cpp:59
    │ │ │ │ +
    PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
    Definition CombinedImuFactor.h:129
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    const Vector3 & gyroscope() const
    get gyroscope bias
    Definition ImuBias.h:69
    │ │ │ │ +
    const Vector3 & accelerometer() const
    get accelerometer bias
    Definition ImuBias.h:64
    │ │ │ │ +
    PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
    Definition ImuFactor.h:72
    │ │ │ │ +
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ +
    Simple trajectory simulator.
    Definition Scenario.h:25
    │ │ │ │ +
    virtual Vector3 omega_b(double t) const =0
    angular velocity in body frame
    │ │ │ │ +
    Definition ScenarioRunner.h:40
    │ │ │ │ +
    Definition ScenarioRunner.h:114
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,253 +1,207 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegratedRotation.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 │ │ │ │ │ -22#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 // 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); │ │ │ │ │ } │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ -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} │ │ │ │ │ -_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_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ +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 │ │ │ │ │ +_I_m_u_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 │ │ │ │ │ +_S_c_e_n_a_r_i_o_._h │ │ │ │ │ +Simple class to test navigation scenarios. │ │ │ │ │ _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_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_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/AttitudeFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    ImuFactor.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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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 Attitude factor.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    January 28, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -ImuFactor.cpp File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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 Attitude factor. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + January 28, 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_._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/a00917.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    AHRSFactor.cpp File Reference
    │ │ │ │ +
    PreintegratedRotation.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +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...
     
    struct  gtsam::traits< PreintegratedRotation >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Krunal Chande
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │
    │ │ │ │ -Luca Carlone
    │ │ │ │ +David Jensen │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │ -
    Date
    July 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AHRSFactor.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +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_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 │ │ │ │ │ - Krunal Chande │ │ │ │ │ Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - July 2014 │ │ │ │ │ * _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_._c_p_p │ │ │ │ │ + * _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/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/PreintegrationParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,87 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
    │ │ │ │ -
    CombinedImuFactor.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    PreintegrationParams.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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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
    │ │ │ │ +Frank Dellaert │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Frank Dellaert │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,51 +1,30 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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 │ │ │ │ │ +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  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 │ │ │ │ │ Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ Richard Roberts │ │ │ │ │ Vadim Indelman │ │ │ │ │ David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/ImuFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,60 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ConstantVelocityFactor.h File Reference
    │ │ │ │ +
    ImuFactor.h 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...
    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 >
     
    struct  gtsam::traits< ImuFactor2 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Maintain a constant velocity motion model between two NavStates.

    │ │ │ │ -
    Author
    Asa Hammond
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │ +
    │ │ │ │ +David Jensen
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -ConstantVelocityFactor.h File Reference │ │ │ │ │ -Maintain a constant velocity motion model between two NavStates. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ + 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_:_:_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_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Maintain a constant velocity motion model between two NavStates. │ │ │ │ │ Author │ │ │ │ │ - Asa Hammond │ │ │ │ │ + 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 │ │ │ │ │ - * _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_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.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00926 = [ │ │ │ │ │ - ["gtsam::ConstantVelocityFactor", "a04128.html", "a04128"] │ │ │ │ │ + ["gtsam::traits< PreintegratedImuMeasurements >", "a04160.html", null], │ │ │ │ │ + ["gtsam::traits< ImuFactor >", "a04164.html", null], │ │ │ │ │ + ["gtsam::traits< ImuFactor2 >", "a04168.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00926_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ConstantVelocityFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,87 +98,298 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ConstantVelocityFactor.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    │ │ │ │ -
    29class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
    │ │ │ │ -
    30 double dt_;
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ +
    24/* GTSAM includes */
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28#include <gtsam/base/debug.h>
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │
    31
    │ │ │ │ -
    32 public:
    │ │ │ │ - │ │ │ │ -
    34
    │ │ │ │ -
    35 public:
    │ │ │ │ -
    36 ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
    │ │ │ │ -
    37 : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
    │ │ │ │ -
    38 ~ConstantVelocityFactor() override{};
    │ │ │ │ -
    39
    │ │ │ │ -
    │ │ │ │ -
    50 gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
    │ │ │ │ -
    51 boost::optional<gtsam::Matrix &> H1 = boost::none,
    │ │ │ │ -
    52 boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
    │ │ │ │ -
    53 // only used to use update() below
    │ │ │ │ -
    54 static const Vector3 b_accel{0.0, 0.0, 0.0};
    │ │ │ │ -
    55 static const Vector3 b_omega{0.0, 0.0, 0.0};
    │ │ │ │ -
    56
    │ │ │ │ -
    57 Matrix99 predicted_H_x1;
    │ │ │ │ -
    58 NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
    │ │ │ │ -
    59
    │ │ │ │ -
    60 Matrix99 error_H_predicted;
    │ │ │ │ -
    61 Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
    │ │ │ │ -
    62
    │ │ │ │ -
    63 if (H1) {
    │ │ │ │ -
    64 *H1 = error_H_predicted * predicted_H_x1;
    │ │ │ │ -
    65 }
    │ │ │ │ -
    66 return error;
    │ │ │ │ -
    67 }
    │ │ │ │ -
    │ │ │ │ -
    68};
    │ │ │ │ -
    │ │ │ │ -
    69
    │ │ │ │ -
    70} // namespace gtsam
    │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    73
    │ │ │ │ +
    74 friend class ImuFactor;
    │ │ │ │ +
    75 friend class ImuFactor2;
    │ │ │ │ +
    76
    │ │ │ │ +
    77protected:
    │ │ │ │ +
    78
    │ │ │ │ + │ │ │ │ +
    81
    │ │ │ │ +
    82public:
    │ │ │ │ +
    83
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    86 preintMeasCov_.setZero();
    │ │ │ │ +
    87 }
    │ │ │ │ +
    │ │ │ │ +
    88
    │ │ │ │ +
    │ │ │ │ +
    94 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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#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,
    │ │ │ │ + │ │ │ │ +
    178
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    268
    │ │ │ │ + │ │ │ │ +
    270
    │ │ │ │ +
    271public:
    │ │ │ │ +
    272
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Global debugging flags.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Binary factor for applying a constant velocity model to a moving body represented as a NavState.
    Definition ConstantVelocityFactor.h:29
    │ │ │ │ -
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
    Definition ImuFactor.h:72
    │ │ │ │ +
    ~PreintegratedImuMeasurements() override
    Virtual destructor.
    Definition ImuFactor.h:111
    │ │ │ │ +
    PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
    Constructor, initializes the class with no measurements.
    Definition ImuFactor.h:94
    │ │ │ │ +
    PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
    Construct preintegrated directly from members: base class and preintMeasCov.
    Definition ImuFactor.h:105
    │ │ │ │ +
    PreintegratedImuMeasurements()
    Default constructor for serialization and wrappers.
    Definition ImuFactor.h:85
    │ │ │ │ +
    Matrix preintMeasCov() const
    Return pre-integrated measurement covariance.
    Definition ImuFactor.h:141
    │ │ │ │ +
    Matrix9 preintMeasCov_
    COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
    Definition ImuFactor.h:79
    │ │ │ │ +
    ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
    Definition ImuFactor.h:172
    │ │ │ │ +
    const PreintegratedImuMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition ImuFactor.h:222
    │ │ │ │ +
    ImuFactor()
    Default constructor - only use for serialization.
    Definition ImuFactor.h:191
    │ │ │ │ +
    boost::shared_ptr< ImuFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition ImuFactor.h:187
    │ │ │ │ +
    ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
    Definition ImuFactor.h:263
    │ │ │ │ +
    ImuFactor2()
    Default constructor - only use for serialization.
    Definition ImuFactor.h:274
    │ │ │ │ +
    const PreintegratedImuMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition ImuFactor.h:301
    │ │ │ │ +
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
    Integrate forward in time given angular velocity and acceleration in body frame Uses second order int...
    Definition NavState.cpp:171
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,116 +1,361 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ConstantVelocityFactor.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -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#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 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -_2_9class _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -30 double dt_; │ │ │ │ │ +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_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -32 public: │ │ │ │ │ -33 using _B_a_s_e = _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_>; │ │ │ │ │ -34 │ │ │ │ │ -35 public: │ │ │ │ │ -36 _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r(_K_e_y i, _K_e_y j, double dt, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&model) │ │ │ │ │ -37 : _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_>(model, i, j), dt_(dt) {} │ │ │ │ │ -38 _~_C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r() override{}; │ │ │ │ │ -39 │ │ │ │ │ -_5_0 gtsam::Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _N_a_v_S_t_a_t_e &x1, const _N_a_v_S_t_a_t_e &x2, │ │ │ │ │ -51 boost::optional H1 = boost::none, │ │ │ │ │ -52 boost::optional H2 = boost::none) const override { │ │ │ │ │ -53 // only used to use update() below │ │ │ │ │ -54 static const Vector3 b_accel{0.0, 0.0, 0.0}; │ │ │ │ │ -55 static const Vector3 b_omega{0.0, 0.0, 0.0}; │ │ │ │ │ -56 │ │ │ │ │ -57 Matrix99 predicted_H_x1; │ │ │ │ │ -58 _N_a_v_S_t_a_t_e predicted = x1._u_p_d_a_t_e(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : │ │ │ │ │ -nullptr, {}, {}); │ │ │ │ │ -59 │ │ │ │ │ -60 Matrix99 error_H_predicted; │ │ │ │ │ -61 Vector9 _e_r_r_o_r = predicted._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(x2, H1 ? &error_H_predicted : │ │ │ │ │ -nullptr, H2); │ │ │ │ │ -62 │ │ │ │ │ -63 if (H1) { │ │ │ │ │ -64 *H1 = error_H_predicted * predicted_H_x1; │ │ │ │ │ -65 } │ │ │ │ │ -66 return _e_r_r_o_r; │ │ │ │ │ -67 } │ │ │ │ │ -68}; │ │ │ │ │ -69 │ │ │ │ │ -70} // namespace gtsam │ │ │ │ │ -_N_a_v_S_t_a_t_e_._h │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +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; │ │ │ │ │ +76 │ │ │ │ │ +77protected: │ │ │ │ │ +78 │ │ │ │ │ +_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; │ │ │ │ │ +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#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 │ │ │ │ │ +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; │ │ │ │ │ +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. │ │ │ │ │ +_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ +_T_a_n_g_e_n_t_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_:_:_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_:_:_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 NavState. │ │ │ │ │ -DDeeffiinniittiioonn ConstantVelocityFactor.h:29 │ │ │ │ │ -_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_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -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... │ │ │ │ │ -DDeeffiinniittiioonn ConstantVelocityFactor.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_:_:_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_:_:_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_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1=boost:: │ │ │ │ │ -none, OptionalJacobian< 9, 9 > H2=boost::none) const │ │ │ │ │ -localCoordinates with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn NavState.cpp:135 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_u_p_d_a_t_e │ │ │ │ │ -NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const │ │ │ │ │ -double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, │ │ │ │ │ -OptionalJacobian< 9, 3 > G2) const │ │ │ │ │ -Integrate forward in time given angular velocity and acceleration in body frame │ │ │ │ │ -Uses second order int... │ │ │ │ │ -DDeeffiinniittiioonn NavState.cpp:171 │ │ │ │ │ -_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_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_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/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/AHRSFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Scenario.h File Reference
    │ │ │ │ +
    AHRSFactor.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::PreintegratedAhrsMeasurements
     PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) and the corresponding covariance matrix. More...
     
    class  gtsam::AcceleratingScenario
     Accelerating from an arbitrary initial state, with optional rotation. More...
    class  gtsam::AHRSFactor
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Simple class to test navigation scenarios.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Author
    Krunal Chande
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    July 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +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 │ │ │ │ │ -Scenario.h File Reference │ │ │ │ │ -Simple class to test navigation scenarios. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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. │ │ │ │ │ +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 ********** │ │ │ │ │ -Simple class to test navigation scenarios. │ │ │ │ │ Author │ │ │ │ │ + Krunal Chande │ │ │ │ │ + Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + July 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ + * _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/a00932.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,4 @@ │ │ │ │ │ var a00932 = [ │ │ │ │ │ - ["gtsam::Scenario", "a04224.html", "a04224"], │ │ │ │ │ - ["gtsam::ConstantTwistScenario", "a04228.html", "a04228"], │ │ │ │ │ - ["gtsam::AcceleratingScenario", "a04232.html", "a04232"] │ │ │ │ │ + ["gtsam::PreintegratedAhrsMeasurements", "a04072.html", "a04072"], │ │ │ │ │ + ["gtsam::AHRSFactor", "a04076.html", "a04076"] │ │ │ │ │ ]; │ │ │ ├── ./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/AHRSFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,152 +98,217 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Scenario.h
    │ │ │ │ +
    AHRSFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    20#pragma once
    │ │ │ │
    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
    │ │ │ │ -
    47 Vector3 acceleration_b(double t) const {
    │ │ │ │ -
    48 const Rot3 nRb = rotation(t);
    │ │ │ │ -
    49 return nRb.transpose() * acceleration_n(t);
    │ │ │ │ -
    50 }
    │ │ │ │ -
    51};
    │ │ │ │ -
    │ │ │ │ -
    52
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ +
    22/* GTSAM includes */
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ +
    36 protected:
    │ │ │ │ +
    37
    │ │ │ │ +
    38 Vector3 biasHat_;
    │ │ │ │ + │ │ │ │ +
    40
    │ │ │ │ +
    41 friend class AHRSFactor;
    │ │ │ │ +
    42
    │ │ │ │ +
    43 public:
    │ │ │ │ +
    44
    │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    │ │ │ │ +
    52 PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
    │ │ │ │ +
    53 const Vector3& biasHat) :
    │ │ │ │ +
    54 PreintegratedRotation(p), biasHat_(biasHat) {
    │ │ │ │ +
    55 resetIntegration();
    │ │ │ │ +
    56 }
    │ │ │ │
    │ │ │ │ -
    66
    │ │ │ │ +
    57
    │ │ │ │
    │ │ │ │ -
    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};
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    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
    │ │ │ │ +
    133 typedef AHRSFactor This;
    │ │ │ │ + │ │ │ │ +
    135
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    190
    │ │ │ │ +
    192 AHRSFactor(Key rot_i, Key rot_j, Key bias,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    3D Pose
    │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
    cross product
    Definition Point3.cpp:64
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    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
    │ │ │ │ -
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Matrix3 matrix() const
    return 3*3 rotation matrix
    Definition Rot3M.cpp:219
    │ │ │ │ -
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    Simple trajectory simulator.
    Definition Scenario.h:25
    │ │ │ │ -
    virtual Pose3 pose(double t) const =0
    pose at time t
    │ │ │ │ -
    virtual Vector3 acceleration_n(double t) const =0
    acceleration in nav frame
    │ │ │ │ -
    virtual Vector3 velocity_n(double t) const =0
    velocity at time t, in nav frame
    │ │ │ │ -
    virtual ~Scenario()
    virtual destructor
    Definition Scenario.h:28
    │ │ │ │ -
    virtual Vector3 omega_b(double t) const =0
    angular velocity in body frame
    │ │ │ │ -
    Scenario with constant twist 3D trajectory.
    Definition Scenario.h:60
    │ │ │ │ -
    ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
    Construct scenario with constant twist [w,v].
    Definition Scenario.h:63
    │ │ │ │ -
    Vector3 acceleration_n(double t) const override
    acceleration in nav frame
    Definition Scenario.h:74
    │ │ │ │ -
    Vector3 omega_b(double t) const override
    angular velocity in body frame
    Definition Scenario.h:70
    │ │ │ │ -
    Pose3 pose(double t) const override
    pose at time t
    Definition Scenario.h:67
    │ │ │ │ -
    Vector3 velocity_n(double t) const override
    velocity at time t, in nav frame
    Definition Scenario.h:71
    │ │ │ │ -
    Accelerating from an arbitrary initial state, with optional rotation.
    Definition Scenario.h:83
    │ │ │ │ -
    Pose3 pose(double t) const override
    pose at time t
    Definition Scenario.h:92
    │ │ │ │ -
    Vector3 omega_b(double t) const override
    angular velocity in body frame
    Definition Scenario.h:95
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector3 acceleration_n(double t) const override
    acceleration in nav frame
    Definition Scenario.h:97
    │ │ │ │ -
    Vector3 velocity_n(double t) const override
    velocity at time t, in nav frame
    Definition Scenario.h:96
    │ │ │ │ +
    PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
    Definition AHRSFactor.h:34
    │ │ │ │ +
    Vector3 biasHat_
    Angular rate bias values used during preintegration.
    Definition AHRSFactor.h:38
    │ │ │ │ +
    PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
    Definition AHRSFactor.h:112
    │ │ │ │ +
    PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
    Default constructor, initialize with no measurements.
    Definition AHRSFactor.h:52
    │ │ │ │ +
    Matrix3 preintMeasCov_
    Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
    Definition AHRSFactor.h:39
    │ │ │ │ +
    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
    │ │ │ │ +
    PreintegratedAhrsMeasurements()
    Default constructor, only for serialization and wrappers.
    Definition AHRSFactor.h:46
    │ │ │ │ +
    Definition AHRSFactor.h:131
    │ │ │ │ +
    const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition AHRSFactor.h:174
    │ │ │ │ +
    boost::shared_ptr< AHRSFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition AHRSFactor.h:147
    │ │ │ │ +
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegratedRotation.h:31
    │ │ │ │ +
    PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
    Definition PreintegratedRotation.h:89
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,213 +1,267 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Scenario.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#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> │ │ │ │ │ +20#pragma once │ │ │ │ │ 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 │ │ │ │ │ -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}; │ │ │ │ │ +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_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. │ │ │ │ │ +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 │ │ │ │ │ +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#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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ + * _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/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/AHRSFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,40 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    ManifoldPreintegration.cpp File Reference
    │ │ │ │ +
    AHRSFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ +
    Author
    Krunal Chande
    │ │ │ │
    │ │ │ │ -Stephen Williams
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Vadim Indelman
    │ │ │ │ -
    │ │ │ │ -David Jensen
    │ │ │ │ +Luca Carlone │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │ +
    Date
    July 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ManifoldPreintegration.cpp File Reference │ │ │ │ │ +AHRSFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ + Krunal Chande │ │ │ │ │ Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + July 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _A_H_R_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/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/PreintegrationBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,70 +95,63 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    CombinedImuFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    PreintegrationBase.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::PreintegratedCombinedMeasurements
     PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. 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...
     
    struct  gtsam::traits< PreintegrationCombinedParams >
     
    struct  gtsam::traits< PreintegratedCombinedMeasurements >
     
    struct  gtsam::traits< CombinedImuFactor >
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Typedefs

    │ │ │ │ -typedef ManifoldPreintegration gtsam::PreintegrationType
     
    │ │ │ │

    Detailed Description

    │ │ │ │
    Author
    Luca Carlone
    │ │ │ │
    │ │ │ │ Stephen Williams
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │ Vadim Indelman
    │ │ │ │
    │ │ │ │ David Jensen
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Varun Agrawal │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │ +
    │ │ │ │ +David Jensen
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,50 +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 │ │ │ │ │ -CombinedImuFactor.h File Reference │ │ │ │ │ +_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 │ │ │ │ │ -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_ _> │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ + 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 │ │ │ │ │ - * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._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/a00938.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ 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::PreintegrationBase", "a04216.html", "a04216"] │ │ │ │ │ ]; │ │ │ ├── ./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/PreintegrationBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,319 +98,175 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    CombinedImuFactor.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
    │ │ │ │
    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 */
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    29#include <gtsam/base/Matrix.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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    66
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    70 : biasAccCovariance(I_3x3),
    │ │ │ │ -
    71 biasOmegaCovariance(I_3x3),
    │ │ │ │ -
    72 biasAccOmegaInt(I_6x6) {}
    │ │ │ │ -
    │ │ │ │ -
    73
    │ │ │ │ -
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    117};
    │ │ │ │ -
    │ │ │ │ -
    118
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    130
    │ │ │ │ -
    131public:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    151 preintMeasCov_.setZero();
    │ │ │ │ -
    152 }
    │ │ │ │ -
    │ │ │ │ -
    153
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    160 const boost::shared_ptr<Params>& p,
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    267
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    281
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    345};
    │ │ │ │ -
    │ │ │ │ -
    346// class CombinedImuFactor
    │ │ │ │ -
    347
    │ │ │ │ -
    348template <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    350 : public Testable<PreintegrationCombinedParams> {};
    │ │ │ │ -
    │ │ │ │ -
    351
    │ │ │ │ -
    352template <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    354 : public Testable<PreintegratedCombinedMeasurements> {};
    │ │ │ │ -
    │ │ │ │ -
    355
    │ │ │ │ -
    356template <>
    │ │ │ │ -
    357struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
    │ │ │ │ -
    358
    │ │ │ │ -
    359} // namespace gtsam
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │ +
    29#include <iosfwd>
    │ │ │ │ +
    30#include <string>
    │ │ │ │ +
    31#include <utility>
    │ │ │ │ +
    32
    │ │ │ │ +
    33namespace gtsam {
    │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ +
    41class GTSAM_EXPORT PreintegrationBase {
    │ │ │ │ +
    42 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    45
    │ │ │ │ +
    46 protected:
    │ │ │ │ +
    47 boost::shared_ptr<Params> p_;
    │ │ │ │ +
    48
    │ │ │ │ + │ │ │ │ +
    51
    │ │ │ │ +
    53 double deltaTij_;
    │ │ │ │ +
    54
    │ │ │ │ + │ │ │ │ +
    57
    │ │ │ │ + │ │ │ │ +
    60
    │ │ │ │ +
    61 public:
    │ │ │ │ +
    64
    │ │ │ │ +
    70 PreintegrationBase(const boost::shared_ptr<Params>& p,
    │ │ │ │ + │ │ │ │ +
    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_;
    │ │ │ │ +
    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;
    │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    171 boost::none, OptionalJacobian<9, 3> H2 = boost::none,
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    187};
    │ │ │ │ +
    │ │ │ │ +
    188
    │ │ │ │ +
    189}
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
    Definition CombinedImuFactor.h:62
    │ │ │ │ -
    Matrix6 biasAccOmegaInt
    covariance of bias used as initial estimate.
    Definition CombinedImuFactor.h:65
    │ │ │ │ -
    PreintegrationCombinedParams(const Vector3 &n_gravity)
    See two named constructors below for good values of n_gravity in body frame.
    Definition CombinedImuFactor.h:75
    │ │ │ │ -
    PreintegrationCombinedParams()
    Default constructor makes uninitialized params struct.
    Definition CombinedImuFactor.h:69
    │ │ │ │ -
    Matrix3 biasOmegaCovariance
    continuous-time "Covariance" describing gyroscope bias random walk
    Definition CombinedImuFactor.h:64
    │ │ │ │ -
    Matrix3 biasAccCovariance
    continuous-time "Covariance" describing accelerometer bias random walk
    Definition CombinedImuFactor.h:63
    │ │ │ │ -
    PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
    Definition CombinedImuFactor.h:129
    │ │ │ │ -
    PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
    Construct preintegrated directly from members: base class and preintMeasCov.
    Definition CombinedImuFactor.h:171
    │ │ │ │ -
    ~PreintegratedCombinedMeasurements() override
    Virtual destructor.
    Definition CombinedImuFactor.h:177
    │ │ │ │ -
    PreintegratedCombinedMeasurements()
    Default constructor only for serialization and wrappers.
    Definition CombinedImuFactor.h:150
    │ │ │ │ -
    Params & p() const
    const reference to params, shadows definition in base class
    Definition CombinedImuFactor.h:188
    │ │ │ │ -
    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
    │ │ │ │ -
    CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
    Definition CombinedImuFactor.h:259
    │ │ │ │ -
    boost::shared_ptr< CombinedImuFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition CombinedImuFactor.h:276
    │ │ │ │ -
    CombinedImuFactor()
    Default constructor - only use for serialization.
    Definition CombinedImuFactor.h:280
    │ │ │ │ -
    const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition CombinedImuFactor.h:317
    │ │ │ │
    Definition ImuBias.h:30
    │ │ │ │ -
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │ +
    Vector6 vector() const
    return the accelerometer and gyro biases in a single vector
    Definition ImuBias.h:57
    │ │ │ │ +
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ +
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │ +
    double deltaTij_
    Time interval from i to j.
    Definition PreintegrationBase.h:53
    │ │ │ │ +
    virtual ~PreintegrationBase()
    Virtual destructor for serialization.
    Definition PreintegrationBase.h:59
    │ │ │ │ +
    bool matchesParamsWith(const PreintegrationBase &other) const
    check parameters equality: checks whether shared pointer points to same Params object.
    Definition PreintegrationBase.h:86
    │ │ │ │ +
    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...
    │ │ │ │ +
    Params & p() const
    const reference to params
    Definition PreintegrationBase.h:96
    │ │ │ │ +
    Bias biasHat_
    Acceleration and gyro bias used for preintegration.
    Definition PreintegrationBase.h:50
    │ │ │ │ +
    PreintegrationBase()
    Default constructor for serialization.
    Definition PreintegrationBase.h:56
    │ │ │ │ +
    const boost::shared_ptr< Params > & params() const
    shared pointer to params
    Definition PreintegrationBase.h:91
    │ │ │ │ +
    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...
    │ │ │ │
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegrationParams.h:26
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,389 +1,219 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CombinedImuFactor.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 │ │ │ │ │ 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 <_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_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 │ │ │ │ │ -98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } │ │ │ │ │ -99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; │ │ │ │ │ -} │ │ │ │ │ -100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } │ │ │ │ │ +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_; │ │ │ │ │ +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 │ │ │ │ │ +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_; │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ 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 _~_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 │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -_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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ + * _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/a00944_source.html │ │ │ │ @@ -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/navigation/MagPoseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,220 +98,167 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ImuBias.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
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ +
    12#pragma once
    │ │ │ │ +
    13
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    16
    │ │ │ │ +
    17namespace gtsam {
    │ │ │ │ +
    18
    │ │ │ │ +
    27template <class POSE>
    │ │ │ │ +
    │ │ │ │ +
    28class MagPoseFactor: public NoiseModelFactorN<POSE> {
    │ │ │ │ +
    29 private:
    │ │ │ │ +
    30 using This = MagPoseFactor<POSE>;
    │ │ │ │ + │ │ │ │ +
    32 using Point = typename POSE::Translation;
    │ │ │ │ +
    33 using Rot = typename POSE::Rotation;
    │ │ │ │
    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 explicit ConstantBias(const Vector6& v) :
    │ │ │ │ -
    51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
    │ │ │ │ -
    52 }
    │ │ │ │ +
    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>>;
    │ │ │ │ +
    46
    │ │ │ │ +
    48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
    │ │ │ │ +
    49 GTSAM_CONCEPT_POSE_TYPE(POSE)
    │ │ │ │ +
    50
    │ │ │ │ +
    51 public:
    │ │ │ │ +
    52 ~MagPoseFactor() override {}
    │ │ │ │
    53
    │ │ │ │ -
    55
    │ │ │ │ -
    │ │ │ │ -
    57 Vector6 vector() const {
    │ │ │ │ -
    58 Vector6 v;
    │ │ │ │ -
    59 v << biasAcc_, biasGyro_;
    │ │ │ │ -
    60 return v;
    │ │ │ │ -
    61 }
    │ │ │ │ -
    │ │ │ │ -
    62
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ + │ │ │ │ +
    56
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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: ");
    │ │ │ │ +
    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
    │ │ │ │ -
    110
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
    │ │ │ │ -
    129 }
    │ │ │ │ -
    │ │ │ │ -
    130
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Special class for optional Jacobian arguments.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    VectorSpace provides both Testable and VectorSpaceTraits.
    Definition VectorSpace.h:207
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    const Vector3 & gyroscope() const
    get gyroscope bias
    Definition ImuBias.h:69
    │ │ │ │ -
    static ConstantBias Identity()
    identity for group operation
    Definition ImuBias.h:112
    │ │ │ │ -
    bool equals(const ConstantBias &expected, double tol=1e-5) const
    equality up to tolerance
    Definition ImuBias.h:102
    │ │ │ │ -
    ConstantBias operator-(const ConstantBias &b) const
    subtraction
    Definition ImuBias.h:132
    │ │ │ │ -
    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
    │ │ │ │ -
    ConstantBias operator-() const
    inverse
    Definition ImuBias.h:117
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector6 vector() const
    return the accelerometer and gyro biases in a single vector
    Definition ImuBias.h:57
    │ │ │ │ -
    ConstantBias operator+(const Vector6 &v) const
    addition of vector on right
    Definition ImuBias.h:122
    │ │ │ │ -
    ConstantBias operator+(const ConstantBias &b) const
    addition
    Definition ImuBias.h:127
    │ │ │ │ -
    const Vector3 & accelerometer() const
    get accelerometer bias
    Definition ImuBias.h:64
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
    Definition MagPoseFactor.h:28
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Implement functions needed for Testable.
    Definition MagPoseFactor.h:89
    │ │ │ │ +
    ~MagPoseFactor() override
    Concept check by type.
    Definition MagPoseFactor.h:52
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    Equals function.
    Definition MagPoseFactor.h:97
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition MagPoseFactor.h:81
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition MagPoseFactor.h:129
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost::none) const override
    Implement functions needed to derive from Factor.
    Definition MagPoseFactor.h:111
    │ │ │ │ +
    MagPoseFactor()
    Default constructor - only use for serialization.
    Definition MagPoseFactor.h:55
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,245 +1,216 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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_/_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_; │ │ │ │ │ +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 │ │ │ │ │ -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 explicit ConstantBias(const Vector6& v) : │ │ │ │ │ -51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { │ │ │ │ │ -52 } │ │ │ │ │ +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>; │ │ │ │ │ +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 │ │ │ │ │ -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 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 } │ │ │ │ │ +_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: "); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _I_m_u_B_i_a_s_._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/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/CombinedImuFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,167 +98,319 @@ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    MagPoseFactor.h
    │ │ │ │ +
    CombinedImuFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    16
    │ │ │ │ -
    17namespace gtsam {
    │ │ │ │ -
    18
    │ │ │ │ -
    27template <class POSE>
    │ │ │ │ -
    │ │ │ │ -
    28class MagPoseFactor: public NoiseModelFactorN<POSE> {
    │ │ │ │ -
    29 private:
    │ │ │ │ -
    30 using This = MagPoseFactor<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_;
    │ │ │ │ +
    23#pragma once
    │ │ │ │ +
    24
    │ │ │ │ +
    25/* GTSAM includes */
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29#include <gtsam/base/Matrix.h>
    │ │ │ │ + │ │ │ │ +
    31
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │ +
    33
    │ │ │ │ +
    34#ifdef GTSAM_TANGENT_PREINTEGRATION
    │ │ │ │ +
    35typedef TangentPreintegration PreintegrationType;
    │ │ │ │ +
    36#else
    │ │ │ │ +
    37typedef ManifoldPreintegration PreintegrationType;
    │ │ │ │ +
    38#endif
    │ │ │ │
    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>>;
    │ │ │ │ -
    46
    │ │ │ │ -
    48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
    │ │ │ │ -
    49 GTSAM_CONCEPT_POSE_TYPE(POSE)
    │ │ │ │ -
    50
    │ │ │ │ -
    51 public:
    │ │ │ │ -
    52 ~MagPoseFactor() override {}
    │ │ │ │ -
    53
    │ │ │ │ - │ │ │ │ -
    56
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)));
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    66
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    70 : biasAccCovariance(I_3x3),
    │ │ │ │ +
    71 biasOmegaCovariance(I_3x3),
    │ │ │ │ +
    72 biasAccOmegaInt(I_6x6) {}
    │ │ │ │ +
    │ │ │ │ +
    73
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    117};
    │ │ │ │ +
    │ │ │ │ +
    118
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    130
    │ │ │ │ +
    131public:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    151 preintMeasCov_.setZero();
    │ │ │ │ +
    152 }
    │ │ │ │ +
    │ │ │ │ +
    153
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    160 const boost::shared_ptr<Params>& p,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    267
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    281
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    345};
    │ │ │ │ +
    │ │ │ │ +
    346// class CombinedImuFactor
    │ │ │ │ +
    347
    │ │ │ │ +
    348template <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    350 : public Testable<PreintegrationCombinedParams> {};
    │ │ │ │ +
    │ │ │ │ +
    351
    │ │ │ │ +
    352template <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    354 : public Testable<PreintegratedCombinedMeasurements> {};
    │ │ │ │ +
    │ │ │ │ +
    355
    │ │ │ │ +
    356template <>
    │ │ │ │ +
    357struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
    │ │ │ │ +
    358
    │ │ │ │ +
    359} // namespace gtsam
    │ │ │ │ +
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
    Definition MagPoseFactor.h:28
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Implement functions needed for Testable.
    Definition MagPoseFactor.h:89
    │ │ │ │ -
    ~MagPoseFactor() override
    Concept check by type.
    Definition MagPoseFactor.h:52
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    Equals function.
    Definition MagPoseFactor.h:97
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition MagPoseFactor.h:81
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition MagPoseFactor.h:129
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost::none) const override
    Implement functions needed to derive from Factor.
    Definition MagPoseFactor.h:111
    │ │ │ │ -
    MagPoseFactor()
    Default constructor - only use for serialization.
    Definition MagPoseFactor.h:55
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
    Definition CombinedImuFactor.h:62
    │ │ │ │ +
    Matrix6 biasAccOmegaInt
    covariance of bias used as initial estimate.
    Definition CombinedImuFactor.h:65
    │ │ │ │ +
    PreintegrationCombinedParams(const Vector3 &n_gravity)
    See two named constructors below for good values of n_gravity in body frame.
    Definition CombinedImuFactor.h:75
    │ │ │ │ +
    PreintegrationCombinedParams()
    Default constructor makes uninitialized params struct.
    Definition CombinedImuFactor.h:69
    │ │ │ │ +
    Matrix3 biasOmegaCovariance
    continuous-time "Covariance" describing gyroscope bias random walk
    Definition CombinedImuFactor.h:64
    │ │ │ │ +
    Matrix3 biasAccCovariance
    continuous-time "Covariance" describing accelerometer bias random walk
    Definition CombinedImuFactor.h:63
    │ │ │ │ +
    PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
    Definition CombinedImuFactor.h:129
    │ │ │ │ +
    PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
    Construct preintegrated directly from members: base class and preintMeasCov.
    Definition CombinedImuFactor.h:171
    │ │ │ │ +
    ~PreintegratedCombinedMeasurements() override
    Virtual destructor.
    Definition CombinedImuFactor.h:177
    │ │ │ │ +
    PreintegratedCombinedMeasurements()
    Default constructor only for serialization and wrappers.
    Definition CombinedImuFactor.h:150
    │ │ │ │ +
    Params & p() const
    const reference to params, shadows definition in base class
    Definition CombinedImuFactor.h:188
    │ │ │ │ +
    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
    │ │ │ │ +
    CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
    Definition CombinedImuFactor.h:259
    │ │ │ │ +
    boost::shared_ptr< CombinedImuFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition CombinedImuFactor.h:276
    │ │ │ │ +
    CombinedImuFactor()
    Default constructor - only use for serialization.
    Definition CombinedImuFactor.h:280
    │ │ │ │ +
    const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition CombinedImuFactor.h:317
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │ +
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegrationParams.h:26
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,216 +1,389 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MagPoseFactor.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 │ │ │ │ │ -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_; │ │ │ │ │ +23#pragma once │ │ │ │ │ +24 │ │ │ │ │ +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 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>; │ │ │ │ │ -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))); │ │ │ │ │ +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_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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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 _~_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 │ │ │ │ │ { │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +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. │ │ │ │ │ +_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 │ │ │ │ │ +_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ +_T_a_n_g_e_n_t_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_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() │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ - * MMaaggPPoosseeFFaaccttoorr..hh │ │ │ │ │ + * _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/a00956.html │ │ │ │ @@ -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/navigation/TangentPreintegration.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    ImuBias.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    TangentPreintegration.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::TangentPreintegration
     Integrate on the 9D tangent space of the NavState manifold. 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
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Adam Bry
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -ImuBias.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ Author │ │ │ │ │ - Vadim Indelman, Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Adam Bry │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00956.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00956 = [ │ │ │ │ │ - ["operator<<", "a00956.html#ae7a6582902c6daf9f20f2f7e466c60d8", null] │ │ │ │ │ + ["gtsam::TangentPreintegration", "a04244.html", "a04244"] │ │ │ │ │ ]; │ │ │ ├── ./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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    NavState.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    ManifoldPreintegration.cpp 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 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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,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 │ │ │ │ │ -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 │ │ │ │ │ - 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_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ManifoldPreintegration.cpp File Reference │ │ │ │ │ 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_._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/a00965.html │ │ │ │ @@ -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/navigation/AttitudeFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,64 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Macros
    │ │ │ │ -
    TangentPreintegration.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    AttitudeFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Implementation file for Attitude factor. │ │ │ │ +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)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Adam Bry
    │ │ │ │ +

    Implementation file for Attitude factor.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    January 28, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,22 @@ │ │ │ │ │ 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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Implementation file for Attitude factor. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Adam Bry │ │ │ │ │ + Date │ │ │ │ │ + January 28, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Scenario.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PreintegratedRotation.cpp File Reference
    │ │ │ │ +
    Scenario.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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PreintegratedRotation.cpp File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ +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_._._. │ │ │ │ │ +  │ │ │ │ │ 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 │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._c_p_p │ │ │ │ │ + * _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/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/BarometricFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GPSFactor.cpp File Reference
    │ │ │ │ +
    BarometricFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation file for GPS factor. │ │ │ │ +

    Implementation file for Barometric factor. │ │ │ │ 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
    │ │ │ │ +

    Implementation file for Barometric factor.

    │ │ │ │ +
    Author
    Peter Milani
    │ │ │ │ +
    Date
    December 16, 2021
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,22 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Implementation file for GPS 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 │ │ │ │ │ - * _G_P_S_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_._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.html │ │ │ │ @@ -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/navigation/GPSFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    TangentPreintegration.h File Reference
    │ │ │ │ +
    GPSFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Implementation file for GPS factor. │ │ │ │ +More...

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

    │ │ │ │ -Classes

    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

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Adam Bry
    │ │ │ │ +

    Implementation file for GPS factor.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    January 28, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ +Implementation file for GPS factor. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Adam Bry │ │ │ │ │ + Date │ │ │ │ │ + January 28, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/TangentPreintegration.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    MagFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Macros
    │ │ │ │ +
    TangentPreintegration.cpp 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...
     
    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +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

    │ │ │ │ -

    Factors involving magnetometers.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    January 29, 2014
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Adam Bry
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ +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) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factors involving magnetometers. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 29, 2014 │ │ │ │ │ + Adam Bry │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00983.html │ │ │ │ @@ -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/navigation/BarometricFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,62 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    PreintegrationBase.h File Reference
    │ │ │ │ +
    BarometricFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Header file for Barometric factor. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    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...
    class  gtsam::BarometricFactor
     Prior on height in a cartesian frame. 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
    │ │ │ │ -
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    │ │ │ │ -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,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 │ │ │ │ │ -PreintegrationBase.h File Reference │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Header file for Barometric factor. │ │ │ │ │ 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 │ │ │ │ │ + Peter Milani │ │ │ │ │ + Date │ │ │ │ │ + December 16, 2021 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00983_source.html │ │ │ │ @@ -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/navigation/BarometricFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,175 +98,126 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PreintegrationBase.h
    │ │ │ │ +
    BarometricFactor.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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28
    │ │ │ │ -
    29#include <iosfwd>
    │ │ │ │ -
    30#include <string>
    │ │ │ │ -
    31#include <utility>
    │ │ │ │ -
    32
    │ │ │ │ -
    33namespace gtsam {
    │ │ │ │ -
    34
    │ │ │ │ -
    │ │ │ │ -
    41class GTSAM_EXPORT PreintegrationBase {
    │ │ │ │ -
    42 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    45
    │ │ │ │ -
    46 protected:
    │ │ │ │ -
    47 boost::shared_ptr<Params> p_;
    │ │ │ │ -
    48
    │ │ │ │ - │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    │ │ │ │ +
    34class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
    │ │ │ │ +
    35 private:
    │ │ │ │ + │ │ │ │ +
    37
    │ │ │ │ +
    38 double nT_;
    │ │ │ │ +
    39
    │ │ │ │ +
    40 public:
    │ │ │ │ +
    42 typedef boost::shared_ptr<BarometricFactor> shared_ptr;
    │ │ │ │ +
    43
    │ │ │ │ + │ │ │ │ +
    46
    │ │ │ │ +
    48 BarometricFactor() : nT_(0) {}
    │ │ │ │ +
    49
    │ │ │ │ +
    50 ~BarometricFactor() override {}
    │ │ │ │
    51
    │ │ │ │ -
    53 double deltaTij_;
    │ │ │ │ -
    54
    │ │ │ │ - │ │ │ │ -
    57
    │ │ │ │ - │ │ │ │ -
    60
    │ │ │ │ -
    61 public:
    │ │ │ │ -
    64
    │ │ │ │ -
    70 PreintegrationBase(const boost::shared_ptr<Params>& p,
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ -
    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;
    │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    171 boost::none, OptionalJacobian<9, 3> H2 = boost::none,
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    187};
    │ │ │ │ -
    │ │ │ │ -
    188
    │ │ │ │ -
    189}
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    Vector6 vector() const
    return the accelerometer and gyro biases in a single vector
    Definition ImuBias.h:57
    │ │ │ │ -
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │ -
    double deltaTij_
    Time interval from i to j.
    Definition PreintegrationBase.h:53
    │ │ │ │ -
    virtual ~PreintegrationBase()
    Virtual destructor for serialization.
    Definition PreintegrationBase.h:59
    │ │ │ │ -
    bool matchesParamsWith(const PreintegrationBase &other) const
    check parameters equality: checks whether shared pointer points to same Params object.
    Definition PreintegrationBase.h:86
    │ │ │ │ -
    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...
    │ │ │ │ -
    Params & p() const
    const reference to params
    Definition PreintegrationBase.h:96
    │ │ │ │ -
    Bias biasHat_
    Acceleration and gyro bias used for preintegration.
    Definition PreintegrationBase.h:50
    │ │ │ │ -
    PreintegrationBase()
    Default constructor for serialization.
    Definition PreintegrationBase.h:56
    │ │ │ │ -
    const boost::shared_ptr< Params > & params() const
    shared pointer to params
    Definition PreintegrationBase.h:91
    │ │ │ │ -
    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...
    │ │ │ │ -
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegrationParams.h:26
    │ │ │ │ +
    Prior on height in a cartesian frame.
    Definition BarometricFactor.h:34
    │ │ │ │ +
    BarometricFactor This
    Typedef to this class.
    Definition BarometricFactor.h:45
    │ │ │ │ +
    BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
    Constructor from a measurement of pressure in KPa.
    Definition BarometricFactor.h:59
    │ │ │ │ +
    boost::shared_ptr< BarometricFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition BarometricFactor.h:42
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition BarometricFactor.h:64
    │ │ │ │ +
    BarometricFactor()
    default constructor - only use for serialization
    Definition BarometricFactor.h:48
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,219 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegrationBase.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 │ │ │ │ │ -22#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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_; │ │ │ │ │ -48 │ │ │ │ │ -_5_0 _B_i_a_s _b_i_a_s_H_a_t__; │ │ │ │ │ +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) {} │ │ │ │ │ +49 │ │ │ │ │ +50 _~_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r() override {} │ │ │ │ │ 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 │ │ │ │ │ -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_; │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ + * _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/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/NavState.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,61 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    ImuFactor.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.

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

    │ │ │ │ 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...
    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< PreintegratedImuMeasurements >
     
    struct  gtsam::traits< ImuFactor >
     
    struct  gtsam::traits< ImuFactor2 >
    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,45 +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 │ │ │ │ │ -ImuFactor.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 │ │ │ │ │ - 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_:_:_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_:_:_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_ _> │ │ │ │ │ +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 │ │ │ │ │ - * _I_m_u_F_a_c_t_o_r_._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/a00986.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,5 @@ │ │ │ │ │ var a00986 = [ │ │ │ │ │ - ["gtsam::traits< PreintegratedImuMeasurements >", "a04160.html", null], │ │ │ │ │ - ["gtsam::traits< ImuFactor >", "a04164.html", null], │ │ │ │ │ - ["gtsam::traits< ImuFactor2 >", "a04168.html", null] │ │ │ │ │ + ["gtsam::NavState", "a04196.html", "a04196"], │ │ │ │ │ + ["gtsam::traits< NavState >", "a04200.html", null], │ │ │ │ │ + ["Velocity3", "a00986.html#ad6ba0a40d5dd8936f4ca53f7bb3ef53b", null] │ │ │ │ │ ]; │ │ │ ├── ./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/NavState.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,298 +98,217 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ImuFactor.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/* GTSAM includes */
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28#include <gtsam/base/debug.h>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/base/Vector.h>
    │ │ │ │ +
    23#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    28typedef Vector3 Velocity3;
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    73
    │ │ │ │ -
    74 friend class ImuFactor;
    │ │ │ │ -
    75 friend class ImuFactor2;
    │ │ │ │ -
    76
    │ │ │ │ -
    77protected:
    │ │ │ │ -
    78
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    75 static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
    │ │ │ │ + │ │ │ │ +
    77
    │ │ │ │
    81
    │ │ │ │ -
    82public:
    │ │ │ │ -
    83
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    86 preintMeasCov_.setZero();
    │ │ │ │ -
    87 }
    │ │ │ │ -
    │ │ │ │ -
    88
    │ │ │ │ -
    │ │ │ │ -
    94 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
    │ │ │ │ -
    138 const Matrix& dts);
    │ │ │ │ -
    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:
    │ │ │ │ -
    174
    │ │ │ │ -
    175 typedef ImuFactor This;
    │ │ │ │ -
    176 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
    │ │ │ │ - │ │ │ │ -
    178
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    268
    │ │ │ │ - │ │ │ │ -
    270
    │ │ │ │ -
    271public:
    │ │ │ │ -
    272
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Global debugging flags.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector3 Velocity3
    Velocity is currently typedef'd to Vector3.
    Definition NavState.h:28
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
    Definition ImuFactor.h:72
    │ │ │ │ -
    ~PreintegratedImuMeasurements() override
    Virtual destructor.
    Definition ImuFactor.h:111
    │ │ │ │ -
    PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
    Constructor, initializes the class with no measurements.
    Definition ImuFactor.h:94
    │ │ │ │ -
    PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
    Construct preintegrated directly from members: base class and preintMeasCov.
    Definition ImuFactor.h:105
    │ │ │ │ -
    PreintegratedImuMeasurements()
    Default constructor for serialization and wrappers.
    Definition ImuFactor.h:85
    │ │ │ │ -
    Matrix preintMeasCov() const
    Return pre-integrated measurement covariance.
    Definition ImuFactor.h:141
    │ │ │ │ -
    Matrix9 preintMeasCov_
    COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
    Definition ImuFactor.h:79
    │ │ │ │ -
    ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
    Definition ImuFactor.h:172
    │ │ │ │ -
    const PreintegratedImuMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition ImuFactor.h:222
    │ │ │ │ -
    ImuFactor()
    Default constructor - only use for serialization.
    Definition ImuFactor.h:191
    │ │ │ │ -
    boost::shared_ptr< ImuFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition ImuFactor.h:187
    │ │ │ │ -
    ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
    Definition ImuFactor.h:263
    │ │ │ │ -
    ImuFactor2()
    Default constructor - only use for serialization.
    Definition ImuFactor.h:274
    │ │ │ │ -
    const PreintegratedImuMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition ImuFactor.h:301
    │ │ │ │ -
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    gtsam::Quaternion toQuaternion() const
    Compute the quaternion representation of this rotation.
    Definition Rot3M.cpp:233
    │ │ │ │ +
    Matrix3 matrix() const
    return 3*3 rotation matrix
    Definition Rot3M.cpp:219
    │ │ │ │
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    NavState()
    Default constructor.
    Definition NavState.h:55
    │ │ │ │ +
    NavState(const Matrix3 &R, const Vector6 &tv)
    Construct from SO(3) and R^6.
    Definition NavState.h:67
    │ │ │ │ +
    NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
    Construct from attitude, position, velocity.
    Definition NavState.h:59
    │ │ │ │ +
    Matrix3 R() const
    Return rotation matrix. Induces computation in quaternion mode.
    Definition NavState.h:95
    │ │ │ │ +
    NavState(const Pose3 &pose, const Velocity3 &v)
    Construct from pose and velocity.
    Definition NavState.h:63
    │ │ │ │ +
    const Vector3 & v() const
    Return velocity as Vector3. Computation-free.
    Definition NavState.h:107
    │ │ │ │ +
    Vector3 t() const
    Return position as Vector3.
    Definition NavState.h:103
    │ │ │ │ +
    Quaternion quaternion() const
    Return quaternion. Induces computation in matrix mode.
    Definition NavState.h:99
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,361 +1,253 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ImuFactor.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/* 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> │ │ │ │ │ +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 │ │ │ │ │ +_2_8typedef Vector3 _V_e_l_o_c_i_t_y_3; │ │ │ │ │ 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; │ │ │ │ │ -76 │ │ │ │ │ -77protected: │ │ │ │ │ -78 │ │ │ │ │ -_7_9 Matrix9 _p_r_e_i_n_t_M_e_a_s_C_o_v__; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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#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_; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_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_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ - * _I_m_u_F_a_c_t_o_r_._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/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/PreintegratedRotation.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    BarometricFactor.cpp File Reference
    │ │ │ │ +
    PreintegratedRotation.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_._._. │ │ │ │ │ +PreintegratedRotation.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 │ │ │ │ │ + * _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/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/MagFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ManifoldPreintegration.h File Reference
    │ │ │ │ +
    MagFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Factors involving magnetometers. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::ManifoldPreintegration
     IMU pre-integration on NavSatet manifold. 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::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

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Stephen Williams
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Vadim Indelman
    │ │ │ │ -
    │ │ │ │ -David Jensen
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +

    Factors involving magnetometers.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    January 29, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -ManifoldPreintegration.h File Reference │ │ │ │ │ +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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Factors involving magnetometers. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + January 29, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00992.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00992 = [ │ │ │ │ │ - ["gtsam::ManifoldPreintegration", "a04192.html", "a04192"] │ │ │ │ │ + ["gtsam::MagFactor", "a04172.html", "a04172"], │ │ │ │ │ + ["gtsam::MagFactor1", "a04176.html", "a04176"], │ │ │ │ │ + ["gtsam::MagFactor2", "a04180.html", "a04180"], │ │ │ │ │ + ["gtsam::MagFactor3", "a04184.html", "a04184"] │ │ │ │ │ ]; │ │ │ ├── ./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/MagFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,124 +98,241 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ManifoldPreintegration.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    23#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │
    │ │ │ │ -
    33class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
    │ │ │ │ -
    34 protected:
    │ │ │ │ -
    35
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    50 resetIntegration();
    │ │ │ │ -
    51 }
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    52
    │ │ │ │ -
    53public:
    │ │ │ │
    56
    │ │ │ │ -
    62 ManifoldPreintegration(const boost::shared_ptr<Params>& p,
    │ │ │ │ - │ │ │ │ -
    64
    │ │ │ │ -
    66
    │ │ │ │ -
    70 void resetIntegration() override;
    │ │ │ │ -
    71
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    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}
    │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ - │ │ │ │ +
    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:
    │ │ │ │ +
    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
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    IMU pre-integration on NavSatet manifold.
    Definition ManifoldPreintegration.h:33
    │ │ │ │ -
    ManifoldPreintegration()
    Default constructor for serialization.
    Definition ManifoldPreintegration.h:49
    │ │ │ │ -
    Matrix3 delVdelBiasAcc_
    Jacobian of preintegrated velocity w.r.t. acceleration bias.
    Definition ManifoldPreintegration.h:45
    │ │ │ │ -
    Matrix3 delRdelBiasOmega_
    Jacobian of preintegrated rotation w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:42
    │ │ │ │ -
    Matrix3 delPdelBiasAcc_
    Jacobian of preintegrated position w.r.t. acceleration bias.
    Definition ManifoldPreintegration.h:43
    │ │ │ │ -
    NavState deltaXij_
    Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
    Definition ManifoldPreintegration.h:41
    │ │ │ │ -
    Matrix3 delPdelBiasOmega_
    Jacobian of preintegrated position w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:44
    │ │ │ │ -
    virtual boost::shared_ptr< ManifoldPreintegration > clone() const
    Dummy clone for MATLAB.
    Definition ManifoldPreintegration.h:109
    │ │ │ │ -
    Matrix3 delVdelBiasOmega_
    Jacobian of preintegrated velocity w.r.t. angular rate bias.
    Definition ManifoldPreintegration.h:46
    │ │ │ │ -
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    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
    │ │ │ │ +
    static Rot3 Yaw(double t)
    Positive yaw is to right (as in aircraft heading). See ypr.
    Definition Rot3.h:174
    │ │ │ │ +
    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
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Point3.
    Definition Unit3.cpp:144
    │ │ │ │ +
    Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
    Definition MagFactor.h:33
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:58
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition MagFactor.h:77
    │ │ │ │ +
    Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
    Definition MagFactor.h:90
    │ │ │ │ +
    MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:99
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:107
    │ │ │ │ +
    Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition MagFactor.h:115
    │ │ │ │ +
    Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
    Definition MagFactor.h:128
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:143
    │ │ │ │ +
    MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:136
    │ │ │ │ +
    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
    │ │ │ │ +
    Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
    Definition MagFactor.h:169
    │ │ │ │ +
    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
    │ │ │ │ +
    MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:177
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:184
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,148 +1,297 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ManifoldPreintegration.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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +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: │ │ │ │ │ +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: │ │ │ │ │ +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_:_:_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_:_:_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_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_:_:_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 │ │ │ │ │ - * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._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 │ │ │ │ @@ -114,17 +114,17 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    18#pragma once
    │ │ │ │
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │
    │ │ │ │
    35class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
    │ │ │ │
    36
    │ │ │ │
    37private:
    │ │ │ │ @@ -240,17 +240,17 @@ │ │ │ │
    170 boost::serialization::base_object<Base>(*this));
    │ │ │ │
    171 ar & BOOST_SERIALIZATION_NVP(nT_);
    │ │ │ │
    172 }
    │ │ │ │
    173};
    │ │ │ │
    │ │ │ │
    174
    │ │ │ │
    175}
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ ├── ./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/ConstantVelocityFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    BarometricFactor.h File Reference
    │ │ │ │ +
    ConstantVelocityFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Header file for Barometric factor. │ │ │ │ +

    Maintain a constant velocity motion model between two NavStates. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::BarometricFactor
     Prior on height in a cartesian frame. More...
    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

    │ │ │ │ -

    Header file for Barometric factor.

    │ │ │ │ -
    Author
    Peter Milani
    │ │ │ │ -
    Date
    December 16, 2021
    │ │ │ │ +

    Maintain a constant velocity motion model between two NavStates.

    │ │ │ │ +
    Author
    Asa Hammond
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -BarometricFactor.h File Reference │ │ │ │ │ -Header file for Barometric factor. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_:_:_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 ********** │ │ │ │ │ -Header file for Barometric factor. │ │ │ │ │ +Maintain a constant velocity motion model between two NavStates. │ │ │ │ │ Author │ │ │ │ │ - Peter Milani │ │ │ │ │ - Date │ │ │ │ │ - December 16, 2021 │ │ │ │ │ + Asa Hammond │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/ConstantVelocityFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BarometricFactor.h
    │ │ │ │ +
    ConstantVelocityFactor.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,71 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    18#pragma once
    │ │ │ │
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    │ │ │ │ -
    34class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
    │ │ │ │ -
    35 private:
    │ │ │ │ - │ │ │ │ -
    37
    │ │ │ │ -
    38 double nT_;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    │ │ │ │ +
    29class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
    │ │ │ │ +
    30 double dt_;
    │ │ │ │ +
    31
    │ │ │ │ +
    32 public:
    │ │ │ │ + │ │ │ │ +
    34
    │ │ │ │ +
    35 public:
    │ │ │ │ +
    36 ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model)
    │ │ │ │ +
    37 : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {}
    │ │ │ │ +
    38 ~ConstantVelocityFactor() override{};
    │ │ │ │
    39
    │ │ │ │ -
    40 public:
    │ │ │ │ -
    42 typedef boost::shared_ptr<BarometricFactor> shared_ptr;
    │ │ │ │ -
    43
    │ │ │ │ - │ │ │ │ -
    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)) {}
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    50 gtsam::Vector evaluateError(const NavState &x1, const NavState &x2,
    │ │ │ │ +
    51 boost::optional<gtsam::Matrix &> H1 = boost::none,
    │ │ │ │ +
    52 boost::optional<gtsam::Matrix &> H2 = boost::none) const override {
    │ │ │ │ +
    53 // only used to use update() below
    │ │ │ │ +
    54 static const Vector3 b_accel{0.0, 0.0, 0.0};
    │ │ │ │ +
    55 static const Vector3 b_omega{0.0, 0.0, 0.0};
    │ │ │ │ +
    56
    │ │ │ │ +
    57 Matrix99 predicted_H_x1;
    │ │ │ │ +
    58 NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
    │ │ │ │ +
    59
    │ │ │ │ +
    60 Matrix99 error_H_predicted;
    │ │ │ │ +
    61 Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
    │ │ │ │
    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)));
    │ │ │ │ +
    63 if (H1) {
    │ │ │ │ +
    64 *H1 = error_H_predicted * predicted_H_x1;
    │ │ │ │ +
    65 }
    │ │ │ │ +
    66 return error;
    │ │ │ │
    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
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    68};
    │ │ │ │ +
    │ │ │ │ +
    69
    │ │ │ │ +
    70} // namespace gtsam
    │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Prior on height in a cartesian frame.
    Definition BarometricFactor.h:34
    │ │ │ │ -
    BarometricFactor This
    Typedef to this class.
    Definition BarometricFactor.h:45
    │ │ │ │ -
    BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
    Constructor from a measurement of pressure in KPa.
    Definition BarometricFactor.h:59
    │ │ │ │ -
    boost::shared_ptr< BarometricFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition BarometricFactor.h:42
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition BarometricFactor.h:64
    │ │ │ │ -
    BarometricFactor()
    default constructor - only use for serialization
    Definition BarometricFactor.h:48
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    Binary factor for applying a constant velocity model to a moving body represented as a NavState.
    Definition ConstantVelocityFactor.h:29
    │ │ │ │ +
    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
    │ │ │ │ +
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
    Integrate forward in time given angular velocity and acceleration in body frame Uses second order int...
    Definition NavState.cpp:171
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BarometricFactor.h │ │ │ │ │ +ConstantVelocityFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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,101 @@ │ │ │ │ │ 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_; │ │ │ │ │ +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#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 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +_2_9class _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +30 double dt_; │ │ │ │ │ +31 │ │ │ │ │ +32 public: │ │ │ │ │ +33 using _B_a_s_e = _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_>; │ │ │ │ │ +34 │ │ │ │ │ +35 public: │ │ │ │ │ +36 _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r(_K_e_y i, _K_e_y j, double dt, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&model) │ │ │ │ │ +37 : _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_>(model, i, j), dt_(dt) {} │ │ │ │ │ +38 _~_C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r() override{}; │ │ │ │ │ 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) {} │ │ │ │ │ -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)) {} │ │ │ │ │ +_5_0 gtsam::Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _N_a_v_S_t_a_t_e &x1, const _N_a_v_S_t_a_t_e &x2, │ │ │ │ │ +51 boost::optional H1 = boost::none, │ │ │ │ │ +52 boost::optional H2 = boost::none) const override { │ │ │ │ │ +53 // only used to use update() below │ │ │ │ │ +54 static const Vector3 b_accel{0.0, 0.0, 0.0}; │ │ │ │ │ +55 static const Vector3 b_omega{0.0, 0.0, 0.0}; │ │ │ │ │ +56 │ │ │ │ │ +57 Matrix99 predicted_H_x1; │ │ │ │ │ +58 _N_a_v_S_t_a_t_e predicted = x1._u_p_d_a_t_e(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : │ │ │ │ │ +nullptr, {}, {}); │ │ │ │ │ +59 │ │ │ │ │ +60 Matrix99 error_H_predicted; │ │ │ │ │ +61 Vector9 _e_r_r_o_r = predicted._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(x2, H1 ? &error_H_predicted : │ │ │ │ │ +nullptr, H2); │ │ │ │ │ 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))); │ │ │ │ │ +63 if (H1) { │ │ │ │ │ +64 *H1 = error_H_predicted * predicted_H_x1; │ │ │ │ │ +65 } │ │ │ │ │ +66 return _e_r_r_o_r; │ │ │ │ │ 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 │ │ │ │ │ +68}; │ │ │ │ │ +69 │ │ │ │ │ +70} // namespace gtsam │ │ │ │ │ _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_:_:_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_:_:_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 NavState. │ │ │ │ │ +DDeeffiinniittiioonn ConstantVelocityFactor.h:29 │ │ │ │ │ +_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_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +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... │ │ │ │ │ +DDeeffiinniittiioonn ConstantVelocityFactor.h:50 │ │ │ │ │ +_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_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1=boost:: │ │ │ │ │ +none, OptionalJacobian< 9, 9 > H2=boost::none) const │ │ │ │ │ +localCoordinates with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn NavState.cpp:135 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_u_p_d_a_t_e │ │ │ │ │ +NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const │ │ │ │ │ +double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, │ │ │ │ │ +OptionalJacobian< 9, 3 > G2) const │ │ │ │ │ +Integrate forward in time given angular velocity and acceleration in body frame │ │ │ │ │ +Uses second order int... │ │ │ │ │ +DDeeffiinniittiioonn NavState.cpp:171 │ │ │ │ │ +_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_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 │ │ │ │ │ + * _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/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/ExpressionFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    NonlinearOptimizerParams.cpp File Reference
    │ │ │ │ +
    ExpressionFactorGraph.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Parameters for nonlinear optimization. │ │ │ │ +

    Factor graph that supports adding ExpressionFactors directly. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    class  gtsam::ExpressionFactorGraph
     Factor graph that supports adding ExpressionFactors directly. 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
    │ │ │ │ +

    Factor graph that supports adding ExpressionFactors directly.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    December 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,27 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ - Date │ │ │ │ │ - Jul 24, 2012 │ │ │ │ │ +Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Andrew Melim │ │ │ │ │ + Date │ │ │ │ │ + December 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/FunctorizedFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,67 +95,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Values.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    FunctorizedFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    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::FunctorizedFactor< R, T >
     Factor which evaluates provided unary functor and uses the result to compute error with respect to the provided measurement. More...
     
    class  gtsam::ValuesKeyAlreadyExists
    struct  gtsam::traits< FunctorizedFactor< R, T > >
     traits More...
     
    class  gtsam::ValuesKeyDoesNotExist
    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...
     
    class  gtsam::ValuesIncorrectType
     
    class  gtsam::DynamicValuesMismatched
     
    class  gtsam::NoMatchFoundForFixed
     
    struct  gtsam::traits< Values >
     traits 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

    │ │ │ │ -

    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.

    │ │ │ │ +
    Date
    May 31, 2020
    │ │ │ │ +
    Author
    Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,55 +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 │ │ │ │ │ -Values.h File Reference │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. _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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ - 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 │ │ │ │ │ +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_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ + 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_<_ _V_a_l_u_e_s_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ + Date │ │ │ │ │ + May 31, 2020 │ │ │ │ │ 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. │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _V_a_l_u_e_s_._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/a01004.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,13 +1,8 @@ │ │ │ │ │ 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::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", "a01004.html#a9d2a0b16c9f78f20fc78fa39c7426242", null], │ │ │ │ │ + ["MakeFunctorizedFactor2", "a01004.html#a313f0e290f370cff0b7e25024d1b8c6d", 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/FunctorizedFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,513 +98,250 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Values.h
    │ │ │ │ +
    FunctorizedFactor.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 <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23#include <cmath>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ - │ │ │ │ -
    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_(); }
    │ │ │ │ - │ │ │ │ -
    54 };
    │ │ │ │ -
    │ │ │ │ -
    55
    │ │ │ │ -
    │ │ │ │ -
    65 class GTSAM_EXPORT Values {
    │ │ │ │ +
    58template <typename R, typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    60 private:
    │ │ │ │ + │ │ │ │ +
    62
    │ │ │ │ +
    63 R measured_;
    │ │ │ │ +
    64 SharedNoiseModel noiseModel_;
    │ │ │ │ +
    65 std::function<R(T, boost::optional<Matrix &>)> func_;
    │ │ │ │
    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>,
    │ │ │ │ - │ │ │ │ -
    79 KeyValuePtrPairAllocator > KeyValueMap;
    │ │ │ │ -
    80
    │ │ │ │ -
    81 // The member to store the values, see just above
    │ │ │ │ -
    82 KeyValueMap values_;
    │ │ │ │ +
    67 public:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    134 : public Testable<FunctorizedFactor<R, T>> {};
    │ │ │ │ +
    │ │ │ │ +
    135
    │ │ │ │ +
    142template <typename T, typename R, typename FUNC>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    169 public:
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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);
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │ - │ │ │ │ +
    257} // namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    bool operator!=(const Matrix &A, const Matrix &B)
    inequality
    Definition Matrix.h:107
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Default allocator for list, map, and set types.
    Definition FastDefaultAllocator.h:50
    │ │ │ │ - │ │ │ │ -
    Wraps any type T so it can play as a Value.
    Definition GenericValue.h:47
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ -
    virtual void deallocate_() const =0
    Deallocate a raw pointer of this value.
    │ │ │ │ -
    virtual Value * clone_() const =0
    Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Definition Values.h:49
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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...
    │ │ │ │ -
    bool empty() const
    whether the config is empty
    Definition Values.h:184
    │ │ │ │ -
    deref_iterator upper_bound(Key j) const
    Find the lowest-ordered element greater than the specified key.
    Definition Values.h:219
    │ │ │ │ -
    boost::shared_ptr< Values > shared_ptr
    A shared_ptr to this class.
    Definition Values.h:87
    │ │ │ │ -
    void clear()
    Remove all variables from the config.
    Definition Values.h:301
    │ │ │ │ -
    void swap(Values &other)
    Swap the contents of two Values without copying data.
    Definition Values.h:298
    │ │ │ │ -
    void insertDouble(Key j, double c)
    version for double
    Definition Values.h:252
    │ │ │ │ -
    size_t size() const
    The number of variables in this config.
    Definition Values.h:181
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< const Values > const_shared_ptr
    A const shared_ptr to this class.
    Definition Values.h:90
    │ │ │ │ -
    Values()=default
    Default constructor creates an empty Values class.
    │ │ │ │ -
    deref_iterator lower_bound(Key j) const
    Find the element greater than or equal to the specified key.
    Definition Values.h:216
    │ │ │ │ -
    double atDouble(size_t key) const
    version for double
    Definition Values.h:159
    │ │ │ │ -
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ -
    Value & value
    The value.
    Definition Values.h:95
    │ │ │ │ -
    const Key key
    The key.
    Definition Values.h:94
    │ │ │ │ -
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:101
    │ │ │ │ -
    const Key key
    The key.
    Definition Values.h:102
    │ │ │ │ -
    const Value & value
    The value.
    Definition Values.h:103
    │ │ │ │ -
    Definition Values.h:190
    │ │ │ │ -
    Definition Values.h:453
    │ │ │ │ -
    const Key key_
    The key that already existed.
    Definition Values.h:455
    │ │ │ │ -
    GTSAM_EXPORT const char * what() const noexcept override
    The message to be displayed to the user.
    Definition Values.cpp:280
    │ │ │ │ -
    ValuesKeyAlreadyExists(Key key) noexcept
    Construct with the key-value pair attempted to be added.
    Definition Values.h:462
    │ │ │ │ -
    Key key() const noexcept
    The duplicate key that was attempted to be added.
    Definition Values.h:468
    │ │ │ │ -
    Definition Values.h:475
    │ │ │ │ -
    ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
    Construct with the key that does not exist in the values.
    Definition Values.h:485
    │ │ │ │ -
    const Key key_
    The key that does not exist.
    Definition Values.h:478
    │ │ │ │ -
    Key key() const noexcept
    The key that was attempted to be accessed that does not exist.
    Definition Values.h:491
    │ │ │ │ -
    const char * operation_
    The operation that attempted to access the key.
    Definition Values.h:477
    │ │ │ │ -
    Definition Values.h:498
    │ │ │ │ -
    const std::type_info & storedTypeId() const
    The typeid of the value stores in the Values.
    Definition Values.h:519
    │ │ │ │ -
    Key key() const noexcept
    The key that was attempted to be accessed that does not exist.
    Definition Values.h:516
    │ │ │ │ -
    const Key key_
    The key requested.
    Definition Values.h:500
    │ │ │ │ -
    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
    │ │ │ │ -
    const std::type_info & requestedTypeId() const
    The requested typeid.
    Definition Values.h:522
    │ │ │ │ -
    Definition Values.h:529
    │ │ │ │ -
    Definition Values.h:542
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
    Definition FunctorizedFactor.h:59
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equals(const NonlinearFactor &other, double tol=1e-9) const override
    Check if two factors are equal.
    Definition FunctorizedFactor.h:110
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition FunctorizedFactor.h:99
    │ │ │ │ +
    FunctorizedFactor()
    default constructor - only use for serialization
    Definition FunctorizedFactor.h:69
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition FunctorizedFactor.h:120
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition FunctorizedFactor.h:85
    │ │ │ │ +
    Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
    Definition FunctorizedFactor.h:159
    │ │ │ │ +
    FunctorizedFactor2()
    default constructor - only use for serialization
    Definition FunctorizedFactor.h:171
    │ │ │ │ +
    NonlinearFactor::shared_ptr clone() const override
    Definition FunctorizedFactor.h:190
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition FunctorizedFactor.h:206
    │ │ │ │ +
    bool equals(const NonlinearFactor &other, double tol=1e-9) const override
    Check if two factors are equal.
    Definition FunctorizedFactor.h:218
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition FunctorizedFactor.h:228
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,668 +1,306 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Values.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 │ │ │ │ │ -25#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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 { │ │ │ │ │ +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 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_; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ +169 public: │ │ │ │ │ +_1_7_1 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2() {} │ │ │ │ │ 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)); } │ │ │ │ │ +_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_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 │ │ │ │ │ +_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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ 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_:_:_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 │ │ │ │ │ - * _V_a_l_u_e_s_._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/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/NonlinearISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,109 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
    │ │ │ │ -
    factorTesting.h File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    NonlinearISAM.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

    │ │ │ │ -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)
     
    │ │ │ │

    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.
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +
    Date
    Jan 19, 2010
    │ │ │ │ +
    Author
    Viorela Ila and Richard Roberts
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,60 +1,25 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_._._. │ │ │ │ │ +  │ │ │ │ │ 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. │ │ │ │ │ -  │ │ │ │ │ - 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) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ │ Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ + Jan 19, 2010 │ │ │ │ │ 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. │ │ │ │ │ + Viorela Ila and Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01010.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01010 = [ │ │ │ │ │ - ["EXPECT_CORRECT_FACTOR_JACOBIANS", "a01010.html#a8ec37fe83eda47404b8588e1f012df21", null], │ │ │ │ │ - ["linearizeNumerically", "a01010.html#a9753b4e62378151d30bf8af5e2fc1602", null] │ │ │ │ │ + ["gtsam::NonlinearISAM", "a04548.html", "a04548"] │ │ │ │ │ ]; │ │ │ ├── ./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/NonlinearISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,136 +98,118 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    factorTesting.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
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <string>
    │ │ │ │ -
    25#include <vector>
    │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 // 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)); }
    │ │ │ │ -
    │ │ │ │ -
    116
    │ │ │ │ -
    117} // namespace gtsam
    │ │ │ │ -
    Some functions to compute numerical derivatives.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    │ │ │ │ +
    27class GTSAM_EXPORT NonlinearISAM {
    │ │ │ │ +
    28protected:
    │ │ │ │ +
    29
    │ │ │ │ + │ │ │ │ +
    32
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ + │ │ │ │ +
    38
    │ │ │ │ + │ │ │ │ +
    41 int reorderCounter_;
    │ │ │ │ +
    42
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
    equals with an tolerance, prints out message if unequal
    Definition Matrix.cpp:43
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equal(const T &obj1, const T &obj2, double tol)
    Call equal on the object.
    Definition Testable.h:84
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    size_t dim(Key j) const
    Return the dimension of variable j.
    Definition VectorValues.h:130
    │ │ │ │ -
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ -
    Vector whitenedError(const Values &c) const
    Vector of errors, whitened This is the raw error, i.e., i.e.
    Definition NonlinearFactor.cpp:109
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition GaussianISAM.h:28
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    Wrapper class to manage ISAM in a nonlinear context.
    Definition NonlinearISAM.h:27
    │ │ │ │ +
    GaussianFactorGraph::Eliminate eliminationFunction_
    The elimination function.
    Definition NonlinearISAM.h:44
    │ │ │ │ +
    NonlinearFactorGraph factors_
    The original factors, used when relinearizing.
    Definition NonlinearISAM.h:37
    │ │ │ │ +
    const GaussianISAM & bayesTree() const
    access the underlying bayes tree
    Definition NonlinearISAM.h:75
    │ │ │ │ +
    int reorderInterval() const
    get counters
    Definition NonlinearISAM.h:84
    │ │ │ │ +
    NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate &eliminationFunction=GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
    Periodically reorder and relinearize.
    Definition NonlinearISAM.h:58
    │ │ │ │ +
    int reorderCounter() const
    TODO: comment.
    Definition NonlinearISAM.h:85
    │ │ │ │ +
    const NonlinearFactorGraph & getFactorsUnsafe() const
    get underlying nonlinear graph
    Definition NonlinearISAM.h:81
    │ │ │ │ +
    int reorderInterval_
    The reordering interval and counter.
    Definition NonlinearISAM.h:40
    │ │ │ │ +
    Values linPoint_
    The current linearization point.
    Definition NonlinearISAM.h:34
    │ │ │ │ +
    gtsam::GaussianISAM isam_
    The internal iSAM object.
    Definition NonlinearISAM.h:31
    │ │ │ │ +
    const Values & getLinearizationPoint() const
    Return the current linearization point.
    Definition NonlinearISAM.h:78
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    Values retract(const VectorValues &delta) const
    Add a delta config to current config and returns a new config.
    Definition Values.cpp:99
    │ │ │ │ -
    VectorValues zeroVectors() const
    Return a VectorValues of zero vectors for each variable in this Values.
    Definition Values.cpp:272
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ -
    noise model to the factor, and calculates the error by asking the user to implement the method
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,170 +1,166 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -factorTesting.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 │ │ │ │ │ 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_/_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(); │ │ │ │ │ -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); │ │ │ │ │ -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. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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; │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/NonlinearFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,45 +95,78 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    ExtendedKalmanFilter.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Macros
    │ │ │ │ +
    NonlinearFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ +

    Non-linear factor base classes. │ │ │ │ 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...
    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

    │ │ │ │ -

    Class to perform generic Kalman Filtering using nonlinear factor graphs.

    │ │ │ │ -
    Author
    Stephen Williams
    │ │ │ │ +

    Non-linear factor base classes.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │ -Chris Beall
    │ │ │ │ +Gerry Chen
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,53 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_:_:_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_._._. │ │ │ │ │ + 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 ********** │ │ │ │ │ -Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Chris Beall │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Gerry Chen │ │ │ │ │ * _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_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.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a01013 = [ │ │ │ │ │ - ["gtsam::ExtendedKalmanFilter< VALUE >", "a04332.html", "a04332"] │ │ │ │ │ + ["gtsam::NonlinearFactor", "a04496.html", "a04496"], │ │ │ │ │ + ["gtsam::traits< NonlinearFactor >", "a04500.html", null], │ │ │ │ │ + ["gtsam::NoiseModelFactor", "a04504.html", "a04504"], │ │ │ │ │ + ["gtsam::NoiseModelFactorN< ValueTypes >", "a04536.html", "a04536"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01013_source.html │ │ │ │ @@ -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/nonlinear/NonlinearFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,110 +98,469 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ExtendedKalmanFilter.h
    │ │ │ │ +
    NonlinearFactor.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
    │ │ │ │ +
    20// \callgraph
    │ │ │ │
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    44template <class VALUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46 // Check that VALUE type is a testable Manifold
    │ │ │ │ -
    47 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
    │ │ │ │ -
    48 BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    50 public:
    │ │ │ │ -
    51 typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
    │ │ │ │ -
    52 typedef VALUE T;
    │ │ │ │ +
    50public:
    │ │ │ │ +
    51
    │ │ │ │ +
    52 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │
    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
    │ │ │ │ +
    56
    │ │ │ │ + │ │ │ │
    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);
    │ │ │ │ +
    63 template<typename CONTAINER>
    │ │ │ │ +
    │ │ │ │ +
    64 NonlinearFactor(const CONTAINER& keys) :
    │ │ │ │ +
    65 Base(keys) {}
    │ │ │ │ +
    │ │ │ │
    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
    │ │ │ │ -
    87
    │ │ │ │ -
    93 T predict(const NoiseModelFactor& motionFactor);
    │ │ │ │ -
    94
    │ │ │ │ -
    99 T update(const NoiseModelFactor& measurementFactor);
    │ │ │ │ -
    100
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    103 return priorFactor_;
    │ │ │ │ -
    104 }
    │ │ │ │ -
    │ │ │ │ -
    105
    │ │ │ │ -
    107};
    │ │ │ │ -
    │ │ │ │ -
    108
    │ │ │ │ -
    109} // namespace
    │ │ │ │ -
    110
    │ │ │ │ - │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Class to perform generic Kalman Filtering using nonlinear factor graphs.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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;
    │ │ │ │ +
    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; }
    │ │ │ │ +
    119
    │ │ │ │ +
    121 virtual boost::shared_ptr<GaussianFactor>
    │ │ │ │ +
    122 linearize(const Values& c) const = 0;
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    470
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ -
    This is a generic Extended Kalman Filter class implemented using nonlinear factors.
    Definition ExtendedKalmanFilter.h:45
    │ │ │ │ -
    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
    │ │ │ │ -
    const JacobianFactor::shared_ptr Density() const
    Return current predictive (if called after predict)/posterior (if called after update)
    Definition ExtendedKalmanFilter.h:102
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="") const
    print
    Definition ExtendedKalmanFilter.h:78
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
    linearize to a GaussianFactor
    │ │ │ │ +
    virtual size_t dim() const =0
    get the dimension of the factor (number of rows on linearization)
    │ │ │ │ +
    NonlinearFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:58
    │ │ │ │ +
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    NonlinearFactor(const CONTAINER &keys)
    Constructor from a collection of the keys involved in this factor.
    Definition NonlinearFactor.h:64
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual ~NonlinearFactor()
    Destructor.
    Definition NonlinearFactor.h:83
    │ │ │ │
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
    Constructor.
    Definition NonlinearFactor.h:198
    │ │ │ │ +
    ~NoiseModelFactor() override
    Destructor.
    Definition NonlinearFactor.h:192
    │ │ │ │ +
    NoiseModelFactor(const SharedNoiseModel &noiseModel)
    Constructor - only for subclasses, as this does not set keys.
    Definition NonlinearFactor.h:206
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Noise model.
    Definition NonlinearFactor.h:186
    │ │ │ │ +
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition NonlinearFactor.h:218
    │ │ │ │ +
    NoiseModelFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:189
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
    Error function without the NoiseModel, .
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Vector evaluateError(const ValueTypes &... x) const
    No-Jacobians requested function overload.
    Definition NonlinearFactor.h:589
    │ │ │ │ +
    NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys)
    Constructor.
    Definition NonlinearFactor.h:491
    │ │ │ │ +
    NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys)
    Constructor.
    Definition NonlinearFactor.h:478
    │ │ │ │ +
    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
    │ │ │ │ +
    NoiseModelFactorN()
    Default Constructor for I/O.
    Definition NonlinearFactor.h:469
    │ │ │ │ +
    virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< ValueTypes >... H) const =0
    Override evaluateError to finish implementing an n-way factor.
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition NonlinearFactor.h:632
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const
    Some (but not all) optional Jacobians are omitted (function overload)
    Definition NonlinearFactor.h:599
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,574 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExtendedKalmanFilter.h │ │ │ │ │ +NonlinearFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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 │ │ │ │ │ +20// \callgraph │ │ │ │ │ 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)); │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +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 │ │ │ │ │ -50 public: │ │ │ │ │ -51 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -52 typedef VALUE T; │ │ │ │ │ +50public: │ │ │ │ │ +51 │ │ │ │ │ +52 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 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 │ │ │ │ │ +56 │ │ │ │ │ +_5_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r() {} │ │ │ │ │ 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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +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; } │ │ │ │ │ +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; │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _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_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/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/ISAM2Params.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,54 +96,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    NonlinearOptimizer.h File Reference
    │ │ │ │ +Typedefs
    │ │ │ │ +
    ISAM2Params.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Base class and parameters for nonlinear optimization algorithms. │ │ │ │ +

    Parameters for iSAM 2. │ │ │ │ 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...
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ -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)
     

    │ │ │ │ +Typedefs

    typedef FastMap< char, Vector > gtsam::ISAM2ThresholdMap
     Parameters for the ISAM2 algorithm.
     
    │ │ │ │ +typedef ISAM2ThresholdMap::value_type gtsam::ISAM2ThresholdMapValue
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Base class and parameters for nonlinear optimization algorithms.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Sep 7, 2009
    │ │ │ │ +

    Parameters for iSAM 2.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +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 │ │ │ │ │ -NonlinearOptimizer.h File Reference │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. _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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ +typedef ISAM2ThresholdMap::value_type  ggttssaamm::::IISSAAMM22TThhrreesshhoollddMMaappVVaalluuee │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Sep 7, 2009 │ │ │ │ │ + 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_._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/a01019.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01019 = [ │ │ │ │ │ - ["gtsam::NonlinearOptimizer", "a04552.html", "a04552"], │ │ │ │ │ - ["checkConvergence", "a01019.html#a3382482454c164c6f5817dcbe6f932b3", null] │ │ │ │ │ + ["gtsam::ISAM2Params", "a04416.html", "a04416"], │ │ │ │ │ + ["ISAM2ThresholdMap", "a01019.html#ga238e84a1d4a91f43465b12592a8bf8e4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01019_source.html │ │ │ │ @@ -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/ISAM2Params.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,307 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearOptimizer.h
    │ │ │ │ +
    ISAM2Params.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace internal { struct NonlinearOptimizerState; }
    │ │ │ │ -
    27
    │ │ │ │ -
    │ │ │ │ -
    75class GTSAM_EXPORT NonlinearOptimizer {
    │ │ │ │ -
    76
    │ │ │ │ -
    77protected:
    │ │ │ │ - │ │ │ │ -
    79
    │ │ │ │ -
    80 std::unique_ptr<internal::NonlinearOptimizerState> state_;
    │ │ │ │ -
    81
    │ │ │ │ -
    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();
    │ │ │ │ +
    18// \callgraph
    │ │ │ │ +
    19
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <boost/variant.hpp>
    │ │ │ │ +
    25#include <string>
    │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    │ │ │ │ +
    35struct GTSAM_EXPORT ISAM2GaussNewtonParams {
    │ │ │ │ +
    36 double
    │ │ │ │ + │ │ │ │ +
    39
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    75 bool
    │ │ │ │ + │ │ │ │ +
    77
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    80 double _initialDelta = 1.0,
    │ │ │ │ +
    81 double _wildfireThreshold =
    │ │ │ │ +
    82 1e-5,
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    129 virtual VectorValues solve(const GaussianFactorGraph &gfg,
    │ │ │ │ -
    130 const NonlinearOptimizerParams& params) const;
    │ │ │ │ -
    131
    │ │ │ │ - │ │ │ │ -
    137
    │ │ │ │ -
    139
    │ │ │ │ -
    140protected:
    │ │ │ │ -
    144 void defaultOptimize();
    │ │ │ │ -
    145
    │ │ │ │ -
    146 virtual const NonlinearOptimizerParams& _params() const = 0;
    │ │ │ │ -
    147
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Parameters for nonlinear optimization.
    │ │ │ │ + │ │ │ │ +
    134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
    │ │ │ │ +
    │ │ │ │ +
    135struct GTSAM_EXPORT ISAM2Params {
    │ │ │ │ +
    136 typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
    │ │ │ │ + │ │ │ │ +
    139 typedef boost::variant<double, FastMap<char, Vector> >
    │ │ │ │ + │ │ │ │ +
    143
    │ │ │ │ + │ │ │ │ +
    151
    │ │ │ │ + │ │ │ │ +
    169
    │ │ │ │ + │ │ │ │ +
    173
    │ │ │ │ + │ │ │ │ +
    176
    │ │ │ │ + │ │ │ │ +
    180
    │ │ │ │ +
    181 enum Factorization { CHOLESKY, QR };
    │ │ │ │ +
    193 Factorization factorization;
    │ │ │ │ +
    194
    │ │ │ │ + │ │ │ │ +
    201
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    205
    │ │ │ │ + │ │ │ │ +
    209
    │ │ │ │ + │ │ │ │ +
    219
    │ │ │ │ + │ │ │ │ +
    225
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
    │ │ │ │ +
    FastMap< char, Vector > ISAM2ThresholdMap
    Parameters for the ISAM2 algorithm.
    Definition ISAM2Params.h:133
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ -
    const NonlinearFactorGraph & graph() const
    return the graph with nonlinear factors
    Definition NonlinearOptimizer.h:118
    │ │ │ │ -
    NonlinearFactorGraph graph_
    The graph with nonlinear factors.
    Definition NonlinearOptimizer.h:78
    │ │ │ │ -
    virtual const Values & optimize()
    Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
    Definition NonlinearOptimizer.h:98
    │ │ │ │ -
    std::unique_ptr< internal::NonlinearOptimizerState > state_
    PIMPL'd state.
    Definition NonlinearOptimizer.h:80
    │ │ │ │ -
    boost::shared_ptr< const NonlinearOptimizer > shared_ptr
    A shared pointer to this class.
    Definition NonlinearOptimizer.h:84
    │ │ │ │ -
    virtual GaussianFactorGraph::shared_ptr iterate()=0
    Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor grap...
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ -
    Verbosity
    See NonlinearOptimizerParams::verbosity.
    Definition NonlinearOptimizerParams.h:37
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    This class contains the implementation of the Dogleg algorithm.
    Definition DoglegOptimizerImpl.h:32
    │ │ │ │ +
    TrustRegionAdaptationMode
    Specifies how the trust region is adapted at each Dogleg iteration.
    Definition DoglegOptimizerImpl.h:53
    │ │ │ │ +
    Parameters for ISAM2 using Gauss-Newton optimization.
    Definition ISAM2Params.h:35
    │ │ │ │ +
    double wildfireThreshold
    Continue updating the linear delta only when changes are above this threshold (default: 0....
    Definition ISAM2Params.h:37
    │ │ │ │ +
    ISAM2GaussNewtonParams(double _wildfireThreshold=0.001)
    Specify parameters as constructor arguments.
    Definition ISAM2Params.h:41
    │ │ │ │ +
    Parameters for ISAM2 using Dogleg optimization.
    Definition ISAM2Params.h:67
    │ │ │ │ +
    DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode
    See description in DoglegOptimizerImpl::TrustRegionAdaptationMode.
    Definition ISAM2Params.h:73
    │ │ │ │ +
    bool verbose
    Whether Dogleg prints iteration and convergence information.
    Definition ISAM2Params.h:76
    │ │ │ │ +
    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
    │ │ │ │ +
    double initialDelta
    The initial trust region radius for Dogleg.
    Definition ISAM2Params.h:68
    │ │ │ │ +
    double wildfireThreshold
    Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
    Definition ISAM2Params.h:70
    │ │ │ │ +
    Definition ISAM2Params.h:135
    │ │ │ │ +
    bool enablePartialRelinearizationCheck
    Check variables for relinearization in tree-order, stopping the check once a variable does not need t...
    Definition ISAM2Params.h:218
    │ │ │ │ +
    OptimizationParams optimizationParams
    Optimization parameters, this both selects the nonlinear optimization method and specifies its parame...
    Definition ISAM2Params.h:150
    │ │ │ │ +
    void print(const std::string &str="") const
    print iSAM2 parameters
    Definition ISAM2Params.h:252
    │ │ │ │ +
    bool enableRelinearization
    Controls whether ISAM2 will ever relinearize any variables (default: true)
    Definition ISAM2Params.h:174
    │ │ │ │ +
    bool cacheLinearizedFactors
    Whether to cache linear factors (default: true).
    Definition ISAM2Params.h:200
    │ │ │ │ +
    bool evaluateNonlinearError
    Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from up...
    Definition ISAM2Params.h:177
    │ │ │ │ +
    bool enableDetailedResults
    Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: ...
    Definition ISAM2Params.h:206
    │ │ │ │ +
    boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
    Either ISAM2GaussNewtonParams or ISAM2DoglegParams.
    Definition ISAM2Params.h:137
    │ │ │ │ +
    bool findUnusedFactorSlots
    When you will be removing many factors, e.g.
    Definition ISAM2Params.h:224
    │ │ │ │ +
    boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
    Either a constant relinearization threshold or a per-variable-type set of thresholds.
    Definition ISAM2Params.h:140
    │ │ │ │ +
    int relinearizeSkip
    Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
    Definition ISAM2Params.h:170
    │ │ │ │ +
    Factorization factorization
    Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
    Definition ISAM2Params.h:193
    │ │ │ │ +
    KeyFormatter keyFormatter
    A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
    Definition ISAM2Params.h:203
    │ │ │ │ +
    RelinearizationThreshold relinearizeThreshold
    Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0....
    Definition ISAM2Params.h:168
    │ │ │ │ +
    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,151 +1,405 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearOptimizer.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 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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; } │ │ │ │ │ -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 │ │ │ │ │ -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(); │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._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/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/DoglegOptimizerImpl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    NonlinearFactorGraph.cpp File Reference
    │ │ │ │ +
    DoglegOptimizerImpl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor Graph Consisting of non-linear factors. │ │ │ │ +

    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

    │ │ │ │ -

    Factor Graph Consisting of non-linear factors.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Carlos Nieto
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ +

    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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Factor Graph Consisting of non-linear factors. │ │ │ │ │ +Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ +implementation) │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ + 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_F_a_c_t_o_r_G_r_a_p_h_._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/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/GaussNewtonOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,58 +95,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    ISAM2Params.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GaussNewtonOptimizer.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::GaussNewtonParams
     Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
     
    struct  gtsam::ISAM2Params
    class  gtsam::GaussNewtonOptimizer
     This class performs Gauss-Newton 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,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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ -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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ + * _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/a01025.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01025 = [ │ │ │ │ │ - ["gtsam::ISAM2Params", "a04416.html", "a04416"], │ │ │ │ │ - ["ISAM2ThresholdMap", "a01025.html#ga238e84a1d4a91f43465b12592a8bf8e4", null] │ │ │ │ │ + ["gtsam::GaussNewtonParams", "a04352.html", null], │ │ │ │ │ + ["gtsam::GaussNewtonOptimizer", "a04356.html", "a04356"] │ │ │ │ │ ]; │ │ │ ├── ./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/GaussNewtonOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,307 +98,91 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2Params.h
    │ │ │ │ +
    GaussNewtonOptimizer.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <boost/variant.hpp>
    │ │ │ │ -
    25#include <string>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    25class GaussNewtonOptimizer;
    │ │ │ │
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ -
    35struct GTSAM_EXPORT ISAM2GaussNewtonParams {
    │ │ │ │ -
    36 double
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    30class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
    │ │ │ │ +
    31public:
    │ │ │ │ + │ │ │ │ +
    33};
    │ │ │ │ +
    │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ +
    38class GTSAM_EXPORT GaussNewtonOptimizer : public NonlinearOptimizer {
    │ │ │ │
    39
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    75 bool
    │ │ │ │ - │ │ │ │ -
    77
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    80 double _initialDelta = 1.0,
    │ │ │ │ -
    81 double _wildfireThreshold =
    │ │ │ │ -
    82 1e-5,
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    85 SEARCH_EACH_ITERATION,
    │ │ │ │ -
    86 bool _verbose = false
    │ │ │ │ -
    87 )
    │ │ │ │ -
    88 : initialDelta(_initialDelta),
    │ │ │ │ -
    89 wildfireThreshold(_wildfireThreshold),
    │ │ │ │ -
    90 adaptationMode(_adaptationMode),
    │ │ │ │ -
    91 verbose(_verbose) {}
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ - │ │ │ │ -
    134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
    │ │ │ │ -
    │ │ │ │ -
    135struct GTSAM_EXPORT ISAM2Params {
    │ │ │ │ -
    136 typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
    │ │ │ │ - │ │ │ │ -
    139 typedef boost::variant<double, FastMap<char, Vector> >
    │ │ │ │ - │ │ │ │ -
    143
    │ │ │ │ - │ │ │ │ -
    151
    │ │ │ │ - │ │ │ │ -
    169
    │ │ │ │ - │ │ │ │ -
    173
    │ │ │ │ - │ │ │ │ -
    176
    │ │ │ │ - │ │ │ │ -
    180
    │ │ │ │ -
    181 enum Factorization { CHOLESKY, QR };
    │ │ │ │ -
    193 Factorization factorization;
    │ │ │ │ -
    194
    │ │ │ │ - │ │ │ │ -
    201
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    205
    │ │ │ │ - │ │ │ │ -
    209
    │ │ │ │ - │ │ │ │ -
    219
    │ │ │ │ - │ │ │ │ -
    225
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ +
    93};
    │ │ │ │
    │ │ │ │ -
    337
    │ │ │ │ -
    338} // namespace gtsam
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
    │ │ │ │ -
    FastMap< char, Vector > ISAM2ThresholdMap
    Parameters for the ISAM2 algorithm.
    Definition ISAM2Params.h:133
    │ │ │ │ +
    94
    │ │ │ │ +
    95}
    │ │ │ │ +
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ -
    This class contains the implementation of the Dogleg algorithm.
    Definition DoglegOptimizerImpl.h:32
    │ │ │ │ -
    TrustRegionAdaptationMode
    Specifies how the trust region is adapted at each Dogleg iteration.
    Definition DoglegOptimizerImpl.h:53
    │ │ │ │ -
    Parameters for ISAM2 using Gauss-Newton optimization.
    Definition ISAM2Params.h:35
    │ │ │ │ -
    double wildfireThreshold
    Continue updating the linear delta only when changes are above this threshold (default: 0....
    Definition ISAM2Params.h:37
    │ │ │ │ -
    ISAM2GaussNewtonParams(double _wildfireThreshold=0.001)
    Specify parameters as constructor arguments.
    Definition ISAM2Params.h:41
    │ │ │ │ -
    Parameters for ISAM2 using Dogleg optimization.
    Definition ISAM2Params.h:67
    │ │ │ │ -
    DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode
    See description in DoglegOptimizerImpl::TrustRegionAdaptationMode.
    Definition ISAM2Params.h:73
    │ │ │ │ -
    bool verbose
    Whether Dogleg prints iteration and convergence information.
    Definition ISAM2Params.h:76
    │ │ │ │ -
    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
    │ │ │ │ -
    double initialDelta
    The initial trust region radius for Dogleg.
    Definition ISAM2Params.h:68
    │ │ │ │ -
    double wildfireThreshold
    Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
    Definition ISAM2Params.h:70
    │ │ │ │ -
    Definition ISAM2Params.h:135
    │ │ │ │ -
    bool enablePartialRelinearizationCheck
    Check variables for relinearization in tree-order, stopping the check once a variable does not need t...
    Definition ISAM2Params.h:218
    │ │ │ │ -
    OptimizationParams optimizationParams
    Optimization parameters, this both selects the nonlinear optimization method and specifies its parame...
    Definition ISAM2Params.h:150
    │ │ │ │ -
    void print(const std::string &str="") const
    print iSAM2 parameters
    Definition ISAM2Params.h:252
    │ │ │ │ -
    bool enableRelinearization
    Controls whether ISAM2 will ever relinearize any variables (default: true)
    Definition ISAM2Params.h:174
    │ │ │ │ -
    bool cacheLinearizedFactors
    Whether to cache linear factors (default: true).
    Definition ISAM2Params.h:200
    │ │ │ │ -
    bool evaluateNonlinearError
    Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from up...
    Definition ISAM2Params.h:177
    │ │ │ │ -
    bool enableDetailedResults
    Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: ...
    Definition ISAM2Params.h:206
    │ │ │ │ -
    boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
    Either ISAM2GaussNewtonParams or ISAM2DoglegParams.
    Definition ISAM2Params.h:137
    │ │ │ │ -
    bool findUnusedFactorSlots
    When you will be removing many factors, e.g.
    Definition ISAM2Params.h:224
    │ │ │ │ -
    boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
    Either a constant relinearization threshold or a per-variable-type set of thresholds.
    Definition ISAM2Params.h:140
    │ │ │ │ -
    int relinearizeSkip
    Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
    Definition ISAM2Params.h:170
    │ │ │ │ -
    Factorization factorization
    Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
    Definition ISAM2Params.h:193
    │ │ │ │ -
    KeyFormatter keyFormatter
    A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
    Definition ISAM2Params.h:203
    │ │ │ │ -
    RelinearizationThreshold relinearizeThreshold
    Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0....
    Definition ISAM2Params.h:168
    │ │ │ │ -
    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
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams.
    Definition GaussNewtonOptimizer.h:30
    │ │ │ │ +
    This class performs Gauss-Newton nonlinear optimization.
    Definition GaussNewtonOptimizer.h:38
    │ │ │ │ +
    const GaussNewtonParams & params() const
    Read-only access the parameters.
    Definition GaussNewtonOptimizer.h:82
    │ │ │ │ +
    ~GaussNewtonOptimizer() override
    Virtual destructor.
    Definition GaussNewtonOptimizer.h:73
    │ │ │ │ +
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition GaussNewtonOptimizer.h:88
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,405 +1,111 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Params.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 GaussNewtonOptimizer; │ │ │ │ │ 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_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 │ │ │ │ │ -_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) {} │ │ │ │ │ +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 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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_P_a_r_a_m_s_._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/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/NonlinearOptimizerParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,50 +97,55 @@ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs
    │ │ │ │ -
    CustomFactor.h File Reference
    │ │ │ │ +
    NonlinearOptimizerParams.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ +

    Parameters for nonlinear optimization. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::CustomFactor
    class  gtsam::NonlinearOptimizerParams
     The common parameters for Nonlinear optimizers. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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 *)>
     
    │ │ │ │ +typedef NonlinearOptimizerParams gtsam::SuccessiveLinearizationParams
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Class to enable arbitrary factors with runtime swappable error function.

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

    Parameters for nonlinear optimization.

    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Andrew Melim
    │ │ │ │ +
    Date
    Apr 1, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -CustomFactor.h File Reference │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +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_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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 *)> │ │ │ │ │ +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 ********** │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Andrew Melim │ │ │ │ │ + Date │ │ │ │ │ + Apr 1, 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_._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/a01031.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01031 = [ │ │ │ │ │ - ["gtsam::CustomFactor", "a04248.html", "a04248"] │ │ │ │ │ + ["gtsam::NonlinearOptimizerParams", "a04556.html", "a04556"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01031_source.html │ │ │ │ @@ -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/nonlinear/NonlinearOptimizerParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,116 +98,196 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    CustomFactor.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ -
    21
    │ │ │ │ -
    22namespace gtsam {
    │ │ │ │ +
    22#pragma once
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46protected:
    │ │ │ │ -
    47 CustomErrorFunction error_function_;
    │ │ │ │ -
    48
    │ │ │ │ -
    49protected:
    │ │ │ │ -
    50
    │ │ │ │ -
    51 using Base = NoiseModelFactor;
    │ │ │ │ -
    52 using This = CustomFactor;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    54public:
    │ │ │ │ -
    55
    │ │ │ │ -
    59 CustomFactor() = default;
    │ │ │ │ -
    60
    │ │ │ │ -
    │ │ │ │ -
    67 CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    95
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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 */
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Subgraph Solver from IROS 2010.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    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
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    Definition CustomFactor.h:45
    │ │ │ │ -
    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
    │ │ │ │ -
    CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction)
    Constructor.
    Definition CustomFactor.h:67
    │ │ │ │ -
    CustomFactor()=default
    Default Constructor for I/O.
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition CustomFactor.h:94
    │ │ │ │ -
    bool sendable() const override
    Mark not sendable.
    Definition CustomFactor.h:87
    │ │ │ │ -
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition CustomFactor.cpp:59
    │ │ │ │ -
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ -
    NoiseModelFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:189
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    OrderingType
    Type of ordering to use.
    Definition Ordering.h:41
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    IterationHook iterationHook
    Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
    Definition NonlinearOptimizerParams.h:94
    │ │ │ │ +
    IterativeOptimizationParameters::shared_ptr iterativeParams
    The container for iterativeOptimization parameters. used in CG Solvers.
    Definition NonlinearOptimizerParams.h:108
    │ │ │ │ +
    Verbosity
    See NonlinearOptimizerParams::verbosity.
    Definition NonlinearOptimizerParams.h:37
    │ │ │ │ +
    LinearSolverType
    See NonlinearOptimizerParams::linearSolverType.
    Definition NonlinearOptimizerParams.h:97
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::optional< Ordering > ordering
    The optional variable elimination ordering, or empty to use COLAMD (default: empty)
    Definition NonlinearOptimizerParams.h:107
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,233 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CustomFactor.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 │ │ │ │ │ -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 { │ │ │ │ │ +22#pragma once │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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; │ │ │ │ │ +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 */ │ │ │ │ │ +_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. │ │ │ │ │ +_S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ +_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_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_:_:_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... │ │ │ │ │ +_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 │ │ │ │ │ - * _C_u_s_t_o_m_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_._h │ │ │ │ │ * Generated on Tue Jan 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/LinearContainerFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    DoglegOptimizer.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    LinearContainerFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Typedefs

    │ │ │ │ -typedef internal::DoglegState gtsam::State
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +

    Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.

    │ │ │ │ +
    Date
    Jul 6, 2012
    │ │ │ │ +
    Author
    Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +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 │ │ │ │ │ -DoglegOptimizer.cpp File Reference │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef internal::DoglegState  ggttssaamm::::SSttaattee │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ +nonlinear graph. │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Jul 6, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/DoglegOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,53 +95,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    ISAM2Clique.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    DoglegOptimizer.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Specialized iSAM2 Clique. │ │ │ │ -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...
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -ISAM2Clique.h 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 │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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_._._. │ │ │ │ │   │ │ │ │ │ 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. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts │ │ │ │ │ + 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_C_l_i_q_u_e_._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/a01040.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01040 = [ │ │ │ │ │ - ["gtsam::ISAM2Clique", "a04404.html", "a04404"], │ │ │ │ │ - ["optimizeWildfire", "a01040.html#a2b0857edd76f8d63eeee0ce9944e28d4", null] │ │ │ │ │ + ["gtsam::DoglegParams", "a04256.html", "a04256"], │ │ │ │ │ + ["gtsam::DoglegOptimizer", "a04260.html", "a04260"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01040_source.html │ │ │ │ @@ -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/nonlinear/DoglegOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,166 +98,127 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2Clique.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
    │ │ │ │ - │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    27#include <string>
    │ │ │ │ -
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    │ │ │ │ -
    36class GTSAM_EXPORT ISAM2Clique
    │ │ │ │ -
    37 : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
    │ │ │ │ -
    38 public:
    │ │ │ │ -
    39 typedef ISAM2Clique This;
    │ │ │ │ - │ │ │ │ -
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    42 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ - │ │ │ │ -
    44 typedef ConditionalType::shared_ptr sharedConditional;
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    25class DoglegOptimizer;
    │ │ │ │ +
    26
    │ │ │ │ +
    │ │ │ │ +
    32class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams {
    │ │ │ │ +
    33public:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    36 SILENT,
    │ │ │ │ +
    37 VERBOSE
    │ │ │ │ +
    38 };
    │ │ │ │ +
    │ │ │ │ +
    39
    │ │ │ │ +
    40 double deltaInitial;
    │ │ │ │ + │ │ │ │ +
    42
    │ │ │ │ +
    43 DoglegParams() :
    │ │ │ │ +
    44 deltaInitial(1.0), verbosityDL(SILENT) {}
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    59 : Base(other),
    │ │ │ │ -
    60 cachedFactor_(other.cachedFactor_),
    │ │ │ │ -
    61 gradientContribution_(other.gradientContribution_) {}
    │ │ │ │ -
    │ │ │ │ -
    62
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    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 {
    │ │ │ │ +
    69
    │ │ │ │ +
    70protected:
    │ │ │ │ +
    71 DoglegParams params_;
    │ │ │ │ +
    72
    │ │ │ │ +
    73public:
    │ │ │ │ +
    74 typedef boost::shared_ptr<DoglegOptimizer> shared_ptr;
    │ │ │ │
    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
    │ │ │ │ -
    121 void findAll(const KeySet& markedMask, KeySet* keys) const;
    │ │ │ │ -
    122
    │ │ │ │ -
    123 private:
    │ │ │ │ -
    128 bool isDirty(const KeySet& replaced, const KeySet& changed) const;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    Base class for cliques of a BayesTree.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    130}
    │ │ │ │ +
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
    Definition ISAM2Clique.h:37
    │ │ │ │ -
    const Vector & gradientContribution() const
    Access the gradient contribution.
    Definition ISAM2Clique.h:80
    │ │ │ │ -
    ISAM2Clique(const ISAM2Clique &other)
    Copy constructor, does not copy solution pointers as these are invalid in different trees.
    Definition ISAM2Clique.h:58
    │ │ │ │ -
    ISAM2Clique & operator=(const ISAM2Clique &other)
    Assignment operator, does not copy solution pointers as these are invalid in different trees.
    Definition ISAM2Clique.h:65
    │ │ │ │ -
    Base::FactorType::shared_ptr & cachedFactor()
    Access the cached factor.
    Definition ISAM2Clique.h:77
    │ │ │ │ -
    ISAM2Clique()
    Default constructor.
    Definition ISAM2Clique.h:53
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    Parameters for Levenberg-Marquardt optimization.
    Definition DoglegOptimizer.h:32
    │ │ │ │ +
    VerbosityDL
    See DoglegParams::dlVerbosity.
    Definition DoglegOptimizer.h:35
    │ │ │ │ +
    VerbosityDL verbosityDL
    The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity.
    Definition DoglegOptimizer.h:41
    │ │ │ │ +
    double deltaInitial
    The initial trust region radius (default: 10.0)
    Definition DoglegOptimizer.h:40
    │ │ │ │ +
    This class performs Dogleg nonlinear optimization.
    Definition DoglegOptimizer.h:68
    │ │ │ │ +
    ~DoglegOptimizer() override
    Virtual destructor.
    Definition DoglegOptimizer.h:106
    │ │ │ │ +
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition DoglegOptimizer.h:124
    │ │ │ │ +
    const DoglegParams & params() const
    Read-only access the parameters.
    Definition DoglegOptimizer.h:115
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,205 +1,158 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Clique.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_/_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; │ │ │ │ │ +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 │ │ │ │ │ +_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 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); │ │ │ │ │ +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 { │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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 │ │ │ │ │ +_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 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. │ │ │ │ │ +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_C_l_i_q_u_e_._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/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/ISAM2UpdateParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    NonlinearConjugateGradientOptimizer.cpp File Reference
    │ │ │ │ +
    ISAM2UpdateParams.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ +

    Class that stores extra params for ISAM2::update() │ │ │ │ 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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
    │ │ │ │ +

    Class that stores extra params for ISAM2::update()

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,27 @@ │ │ │ │ │ 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 │ │ │ │ │ +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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. │ │ │ │ │ +Class that stores extra params for ISAM2::update() │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Date │ │ │ │ │ - Jun 11, 2012 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/GncOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    NonlinearOptimizer.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    GncOptimizer.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Convergence functions not dependent on graph types. │ │ │ │ +

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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
    │ │ │ │ +

    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,35 +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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +The GncOptimizer class. │ │ │ │ │ Author │ │ │ │ │ + Jingnan Shi │ │ │ │ │ + Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Jul 17, 2010 │ │ │ │ │ +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 │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._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/a01049.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01049 = [ │ │ │ │ │ - ["checkConvergence", "a01049.html#a3382482454c164c6f5817dcbe6f932b3", null] │ │ │ │ │ + ["gtsam::GncOptimizer< GncParameters >", "a04360.html", "a04360"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ │ @@ -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/nonlinear/GraphvizFormatting.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM2Params.cpp File Reference
    │ │ │ │ +
    GraphvizFormatting.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Parameters for iSAM 2. │ │ │ │ +

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Parameters for iSAM 2.

    │ │ │ │ -
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │ +

    Graphviz formatter for NonlinearFactorGraph.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    December, 2021
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,28 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for iSAM 2. │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/NonlinearConjugateGradientOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,90 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Expression.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    NonlinearConjugateGradientOptimizer.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Expressions for Block Automatic Differentiation. │ │ │ │ +

    Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ 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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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

    │ │ │ │ -

    Expressions for Block Automatic Differentiation.

    │ │ │ │ -
    Date
    September 18, 2014
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Paul Furgale
    │ │ │ │ +

    Simple non-linear optimizer that solves using non-preconditioned CG.

    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │ +
    Date
    Jun 11, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,89 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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_._._. │ │ │ │ │ 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. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Expressions for Block Automatic Differentiation. │ │ │ │ │ - Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ +Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Date │ │ │ │ │ + Jun 11, 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_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/a01058.html │ │ │ │ @@ -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/nonlinear/ExtendedKalmanFilter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    LinearContainerFactor.cpp 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

    │ │ │ │ -
    Date
    Jul 6, 2012
    │ │ │ │ -
    Author
    Alex Cunningham
    │ │ │ │ +

    Class to perform generic Kalman Filtering using nonlinear factor graphs.

    │ │ │ │ +
    Author
    Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LinearContainerFactor.cpp 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 ********** │ │ │ │ │ - Date │ │ │ │ │ - Jul 6, 2012 │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Chris Beall │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Marginals.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM2UpdateParams.h File Reference
    │ │ │ │ +
    Marginals.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class that stores extra params for ISAM2::update() │ │ │ │ +

    A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ 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...
    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

    │ │ │ │ -

    Class that stores extra params for ISAM2::update()

    │ │ │ │ -
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
    │ │ │ │ +

    A class for computing marginals in a NonlinearFactorGraph.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    May 14, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -ISAM2UpdateParams.h File Reference │ │ │ │ │ -Class that stores extra params for ISAM2::update() _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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Class that stores extra params for ISAM2::update() │ │ │ │ │ +A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + May 14, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/Marginals.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,75 +98,188 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2UpdateParams.h
    │ │ │ │ +
    Marginals.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>
    │ │ │ │ +
    2
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * All Rights Reserved
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ +
    27class JointMarginal;
    │ │ │ │ +
    28
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    33 ISAM2UpdateParams() = default;
    │ │ │ │ -
    34
    │ │ │ │ - │ │ │ │ -
    37
    │ │ │ │ -
    40 boost::optional<FastMap<Key, int>> constrainedKeys{boost::none};
    │ │ │ │ +
    32class GTSAM_EXPORT Marginals {
    │ │ │ │ +
    33
    │ │ │ │ +
    34public:
    │ │ │ │ +
    35
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    38 CHOLESKY,
    │ │ │ │ +
    39 QR
    │ │ │ │ +
    40 };
    │ │ │ │ +
    │ │ │ │
    41
    │ │ │ │ -
    44 boost::optional<FastList<Key>> noRelinKeys{boost::none};
    │ │ │ │ -
    45
    │ │ │ │ -
    49 boost::optional<FastList<Key>> extraReelimKeys{boost::none};
    │ │ │ │ +
    42protected:
    │ │ │ │ +
    43
    │ │ │ │ + │ │ │ │ +
    45 Values values_;
    │ │ │ │ +
    46 Factorization factorization_;
    │ │ │ │ +
    47 GaussianBayesTree bayesTree_;
    │ │ │ │ +
    48
    │ │ │ │ +
    49public:
    │ │ │ │
    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
    │ │ │ │ -
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ - │ │ │ │ -
    Class that stores detailed iSAM2 result.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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);
    │ │ │ │ +
    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;
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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 */
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
    Definition ISAM2UpdateParams.h:32
    │ │ │ │ -
    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
    │ │ │ │ -
    bool force_relinearize
    Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
    Definition ISAM2UpdateParams.h:54
    │ │ │ │ -
    FactorIndices removeFactorIndices
    Indices of factors to remove from system (default: empty)
    Definition ISAM2UpdateParams.h:36
    │ │ │ │ -
    bool forceFullSolve
    By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
    Definition ISAM2UpdateParams.h:71
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< FastList< Key > > noRelinKeys
    An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
    Definition ISAM2UpdateParams.h:44
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    Matrix block(DenseIndex I, DenseIndex J) const
    Get a copy of a block (anywhere in the matrix).
    Definition SymmetricBlockMatrix.cpp:60
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
    Definition Marginals.h:32
    │ │ │ │ +
    Factorization
    The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
    Definition Marginals.h:37
    │ │ │ │ +
    Marginals()
    Default constructor only for wrappers.
    Definition Marginals.h:52
    │ │ │ │ +
    A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
    Definition Marginals.h:153
    │ │ │ │ +
    Matrix at(Key iVariable, Key jVariable) const
    Synonym for operator()
    Definition Marginals.h:184
    │ │ │ │ +
    Matrix fullMatrix() const
    The full, dense covariance/information matrix of the joint marginal.
    Definition Marginals.h:189
    │ │ │ │ +
    Matrix operator()(Key iVariable, Key jVariable) const
    Access a block, corresponding to a pair of variables, of the joint marginal.
    Definition Marginals.h:177
    │ │ │ │ +
    JointMarginal()
    Default constructor only for wrappers.
    Definition Marginals.h:162
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,102 +1,260 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2UpdateParams.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 * 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 │ │ │ │ │ +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 │ │ │ │ │ -_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}; │ │ │ │ │ +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 │ │ │ │ │ -_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}; │ │ │ │ │ +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 │ │ │ │ │ +49public: │ │ │ │ │ 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. │ │ │ │ │ +_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 │ │ │ │ │ +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); │ │ │ │ │ +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; │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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. │ │ │ │ │ +_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 │ │ │ │ │ - * _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._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/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/Expression.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,89 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GraphvizFormatting.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Expression.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Graphviz formatter for NonlinearFactorGraph. │ │ │ │ +

    Expressions for Block Automatic Differentiation. │ │ │ │ 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::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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +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

    │ │ │ │ -

    Graphviz formatter for NonlinearFactorGraph.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    December, 2021
    │ │ │ │ +

    Expressions for Block Automatic Differentiation.

    │ │ │ │ +
    Date
    September 18, 2014
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Paul Furgale
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,89 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 │ │ │ │ │ -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_:_:_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. │ │ │ │ │   │ │ │ │ │ +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 ********** │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ +Expressions for Block Automatic Differentiation. │ │ │ │ │ + Date │ │ │ │ │ + September 18, 2014 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Paul Furgale │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01064.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,14 @@ │ │ │ │ │ var a01064 = [ │ │ │ │ │ - ["gtsam::GraphvizFormatting", "a04368.html", "a04368"] │ │ │ │ │ + ["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", "a01064.html#a51881c20dd5ecdd129cb993ea8374846", null], │ │ │ │ │ + ["linearExpression", "a01064.html#a794029fe8dfc03f67a8cb64ad05d47c9", null], │ │ │ │ │ + ["operator*", "a01064.html#a3db519caa1b7e43412c9a7e13acf8329", null], │ │ │ │ │ + ["operator*", "a01064.html#a405f3793afc2cc3173c3fbdecadb162a", null], │ │ │ │ │ + ["operator+", "a01064.html#a589e3ff8cd71e5a17ee1dcfae8b2df26", null], │ │ │ │ │ + ["operator-", "a01064.html#a9512eb02324210895442f2bf8268902d", null] │ │ │ │ │ ]; │ │ │ ├── ./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/Expression.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,86 +98,310 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GraphvizFormatting.h
    │ │ │ │ +
    Expression.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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    40 double scale;
    │ │ │ │ - │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/nonlinear/internal/JacobianMap.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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    47 : paperHorizontalAxis(Y),
    │ │ │ │ -
    48 paperVerticalAxis(X),
    │ │ │ │ -
    49 scale(1),
    │ │ │ │ -
    50 mergeSimilarFactors(false) {}
    │ │ │ │ -
    │ │ │ │ +
    47template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    49
    │ │ │ │ +
    50public:
    │ │ │ │
    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
    │ │ │ │ -
    Graphviz formatter.
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    71 typedef std::function<
    │ │ │ │ +
    72 T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
    │ │ │ │ +
    73 };
    │ │ │ │ +
    │ │ │ │ +
    74
    │ │ │ │ +
    75 template<class A1, class A2>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ +
    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;
    │ │ │ │ +
    175
    │ │ │ │ + │ │ │ │ +
    178
    │ │ │ │ +
    179protected:
    │ │ │ │ +
    180
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    262 return ScalarMultiplyExpression<T>(s, e);
    │ │ │ │ +
    263}
    │ │ │ │ +
    │ │ │ │ +
    264
    │ │ │ │ +
    271template <typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    273 return BinarySumExpression<T>(e1, e2);
    │ │ │ │ +
    274}
    │ │ │ │ +
    │ │ │ │ +
    275
    │ │ │ │ +
    277template <typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    302
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │ +
    Internals for Expression.h, not for general consumption.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    Key symbol(unsigned char c, std::uint64_t j)
    Create a symbol key from a character and index, i.e.
    Definition Symbol.h:135
    │ │ │ │ +
    std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
    Construct an array of leaves.
    Definition Expression-inl.h:284
    │ │ │ │ +
    Errors operator+(const Errors &a, const Errors &b)
    Addition.
    Definition Errors.cpp:60
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ - │ │ │ │ -
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ -
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ -
    Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
    Definition GraphvizFormatting.h:32
    │ │ │ │ -
    Axis
    World axes to be assigned to paper axes.
    Definition GraphvizFormatting.h:34
    │ │ │ │ -
    Axis paperVerticalAxis
    The world axis assigned to the vertical paper axis.
    Definition GraphvizFormatting.h:38
    │ │ │ │ -
    GraphvizFormatting()
    Default constructor sets up robot coordinates.
    Definition GraphvizFormatting.h:46
    │ │ │ │ -
    bool mergeSimilarFactors
    Merge multiple factors that have the same connectivity.
    Definition GraphvizFormatting.h:41
    │ │ │ │ -
    double scale
    Scale all positions to reduce / increase density.
    Definition GraphvizFormatting.h:40
    │ │ │ │ -
    Axis paperHorizontalAxis
    The world axis assigned to the horizontal paper axis.
    Definition GraphvizFormatting.h:36
    │ │ │ │ +
    Errors operator-(const Errors &a, const Errors &b)
    Subtraction.
    Definition Errors.cpp:75
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    : meta-function to generate JacobianTA optional reference Used mainly by Expressions
    Definition OptionalJacobian.h:261
    │ │ │ │ +
    Vector Space concept.
    Definition VectorSpace.h:470
    │ │ │ │ +
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ +
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │ +
    Definition Expression.h:40
    │ │ │ │ +
    Definition Expression.h:41
    │ │ │ │ +
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ +
    const boost::shared_ptr< internal::ExpressionNode< T > > & root() const
    Return root.
    Definition Expression-inl.h:162
    │ │ │ │ +
    Expression< T > type
    Define type so we can apply it as a meta-function.
    Definition Expression.h:53
    │ │ │ │ +
    Expression()
    Default constructor, for serialization.
    Definition Expression.h:182
    │ │ │ │ +
    virtual ~Expression()
    Destructor.
    Definition Expression.h:141
    │ │ │ │ +
    virtual boost::shared_ptr< Expression > clone() const
    Definition Expression.h:166
    │ │ │ │ +
    Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root)
    Construct with a custom root.
    Definition Expression.h:61
    │ │ │ │ +
    std::set< Key > keys() const
    Return keys that play in this expression.
    Definition Expression-inl.h:132
    │ │ │ │ +
    std::pair< KeyVector, FastVector< int > > KeysAndDims
    Keys and dimensions in same order.
    Definition Expression.h:185
    │ │ │ │ +
    void dims(std::map< Key, int > &map) const
    Return dimensions for each argument, as a map.
    Definition Expression-inl.h:137
    │ │ │ │ +
    void print(const std::string &s) const
    Print.
    Definition Expression-inl.h:142
    │ │ │ │ +
    T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
    brief Return value and derivatives, reverse AD version
    Definition Expression-inl.h:205
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Expression< T > & operator+=(const Expression< T > &e)
    Add another expression to this expression.
    Definition Expression-inl.h:302
    │ │ │ │ +
    size_t traceSize() const
    Return size needed for memory buffer in traceExecution.
    Definition Expression-inl.h:167
    │ │ │ │ +
    T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
    trace execution, very unsafe
    Definition Expression-inl.h:198
    │ │ │ │ +
    Definition Expression.h:70
    │ │ │ │ +
    Definition Expression.h:76
    │ │ │ │ +
    Definition Expression.h:83
    │ │ │ │ +
    A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimiz...
    Definition Expression.h:213
    │ │ │ │ +
    A BinarySumExpression is a specialization of Expression that adds two expressions together It optimiz...
    Definition Expression.h:226
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ + │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,110 +1,404 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GraphvizFormatting.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-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> │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_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) {} │ │ │ │ │ +47template │ │ │ │ │ +_4_8class _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ +49 │ │ │ │ │ +50public: │ │ │ │ │ 51 │ │ │ │ │ -52 // Find bounds │ │ │ │ │ -53 Vector2 findBounds(const _V_a_l_u_e_s& values, const _K_e_y_S_e_t& keys) const; │ │ │ │ │ +_5_3 typedef _E_x_p_r_e_s_s_i_o_n_<_T_> _t_y_p_e; │ │ │ │ │ 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. │ │ │ │ │ +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 │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/LinearContainerFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -
    GaussNewtonOptimizer.h File Reference
    │ │ │ │ +
    LinearContainerFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +
    Date
    Jul 6, 2012
    │ │ │ │ +
    Author
    Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Jul 6, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/factorTesting.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,109 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    Marginals.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions
    │ │ │ │ +
    factorTesting.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ +

    Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ 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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

    │ │ │ │ +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)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A class for computing marginals in a NonlinearFactorGraph.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    May 14, 2012
    │ │ │ │ -
    │ │ │ │ +

    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.
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +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 │ │ │ │ │ -Marginals.h File Reference │ │ │ │ │ -A class for computing marginals in a NonlinearFactorGraph. _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 │ │ │ │ │ +factorTesting.h File Reference │ │ │ │ │ +Evaluate derivatives of a nonlinear factor numerically. _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_._._. │ │ │ │ │ -  │ │ │ │ │ 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. │ │ │ │ │ +  │ │ │ │ │ + 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) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ │ Date │ │ │ │ │ - May 14, 2012 │ │ │ │ │ + September 18, 2014 │ │ │ │ │ + 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. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ + * _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/a01070.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01070 = [ │ │ │ │ │ - ["gtsam::Marginals", "a04452.html", "a04452"], │ │ │ │ │ - ["gtsam::JointMarginal", "a04456.html", "a04456"] │ │ │ │ │ + ["EXPECT_CORRECT_FACTOR_JACOBIANS", "a01070.html#a8ec37fe83eda47404b8588e1f012df21", null], │ │ │ │ │ + ["linearizeNumerically", "a01070.html#a9753b4e62378151d30bf8af5e2fc1602", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01070_source.html │ │ │ │ @@ -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/factorTesting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,188 +98,136 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Marginals.h
    │ │ │ │ +
    factorTesting.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <string>
    │ │ │ │ +
    25#include <vector>
    │ │ │ │
    26
    │ │ │ │ -
    27class JointMarginal;
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │ -
    │ │ │ │ -
    32class GTSAM_EXPORT Marginals {
    │ │ │ │ -
    33
    │ │ │ │ -
    34public:
    │ │ │ │ -
    35
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    38 CHOLESKY,
    │ │ │ │ -
    39 QR
    │ │ │ │ -
    40 };
    │ │ │ │ -
    │ │ │ │ -
    41
    │ │ │ │ -
    42protected:
    │ │ │ │ -
    43
    │ │ │ │ - │ │ │ │ -
    45 Values values_;
    │ │ │ │ -
    46 Factorization factorization_;
    │ │ │ │ -
    47 GaussianBayesTree bayesTree_;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    49public:
    │ │ │ │ -
    50
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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);
    │ │ │ │ -
    102
    │ │ │ │ -
    104 void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │ -
    105
    │ │ │ │ -
    107 GaussianFactor::shared_ptr marginalFactor(Key variable) const;
    │ │ │ │ +
    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
    │ │ │ │ -
    111 Matrix marginalInformation(Key variable) 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:
    │ │ │ │ - │ │ │ │ -
    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 */
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    Some functions to compute numerical derivatives.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
    equals with an tolerance, prints out message if unequal
    Definition Matrix.cpp:43
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equal(const T &obj1, const T &obj2, double tol)
    Call equal on the object.
    Definition Testable.h:84
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    Matrix block(DenseIndex I, DenseIndex J) const
    Get a copy of a block (anywhere in the matrix).
    Definition SymmetricBlockMatrix.cpp:60
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
    Definition Marginals.h:32
    │ │ │ │ -
    Factorization
    The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
    Definition Marginals.h:37
    │ │ │ │ -
    Marginals()
    Default constructor only for wrappers.
    Definition Marginals.h:52
    │ │ │ │ -
    A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
    Definition Marginals.h:153
    │ │ │ │ -
    Matrix at(Key iVariable, Key jVariable) const
    Synonym for operator()
    Definition Marginals.h:184
    │ │ │ │ -
    Matrix fullMatrix() const
    The full, dense covariance/information matrix of the joint marginal.
    Definition Marginals.h:189
    │ │ │ │ -
    Matrix operator()(Key iVariable, Key jVariable) const
    Access a block, corresponding to a pair of variables, of the joint marginal.
    Definition Marginals.h:177
    │ │ │ │ -
    JointMarginal()
    Default constructor only for wrappers.
    Definition Marginals.h:162
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    size_t dim(Key j) const
    Return the dimension of variable j.
    Definition VectorValues.h:130
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    Vector whitenedError(const Values &c) const
    Vector of errors, whitened This is the raw error, i.e., i.e.
    Definition NonlinearFactor.cpp:109
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    Values retract(const VectorValues &delta) const
    Add a delta config to current config and returns a new config.
    Definition Values.cpp:99
    │ │ │ │ +
    VectorValues zeroVectors() const
    Return a VectorValues of zero vectors for each variable in this Values.
    Definition Values.cpp:272
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    noise model to the factor, and calculates the error by asking the user to implement the method
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,260 +1,170 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Marginals.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) │ │ │ │ │ 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_/_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 { │ │ │ │ │ +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_/_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 │ │ │ │ │ -27class JointMarginal; │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ 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_; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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); │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -111 Matrix marginalInformation(_K_e_y variable) 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. │ │ │ │ │ -_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. │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_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_:_:_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 │ │ │ │ │ - * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ + * _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/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/nonlinearExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GraphvizFormatting.cpp File Reference
    │ │ │ │ +
    nonlinearExceptions.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Graphviz formatter for NonlinearFactorGraph. │ │ │ │ +

    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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Graphviz formatter for NonlinearFactorGraph.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    December, 2021
    │ │ │ │ +

    Exceptions that may be thrown by nonlinear optimization components.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Aug 17, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,28 @@ │ │ │ │ │ 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 │ │ │ │ │ +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_:_:_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 ********** │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ +Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Aug 17, 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_._c_p_p │ │ │ │ │ + * _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/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/NonlinearOptimizerParams.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DoglegOptimizer.h File Reference
    │ │ │ │ +
    NonlinearOptimizerParams.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Parameters for nonlinear optimization. │ │ │ │ +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
    │ │ │ │ +

    Parameters for nonlinear optimization.

    │ │ │ │ +
    Date
    Jul 24, 2012
    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Andrew Melim
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ + Date │ │ │ │ │ + Jul 24, 2012 │ │ │ │ │ Author │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Andrew Melim │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/NonlinearOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    ISAM2.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    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

    │ │ │ │ +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, Frank Dellaert
    │ │ │ │ +

    Base class and parameters for nonlinear optimization algorithms.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Sep 7, 2009
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,41 @@ │ │ │ │ │ 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 │ │ │ │ │ +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 │ │ │ │ │ + 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, Frank Dellaert │ │ │ │ │ + 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_._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/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/ExpressionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,131 +98,371 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    LevenbergMarquardtOptimizer.h
    │ │ │ │ +
    ExpressionFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <boost/date_time/posix_time/posix_time.hpp>
    │ │ │ │ -
    27
    │ │ │ │ -
    28class NonlinearOptimizerMoreOptimizationTest;
    │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    31
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    36
    │ │ │ │ -
    37protected:
    │ │ │ │ - │ │ │ │ -
    39 boost::posix_time::ptime startTime_;
    │ │ │ │ -
    40
    │ │ │ │ -
    41 void initTime();
    │ │ │ │ -
    42
    │ │ │ │ -
    43public:
    │ │ │ │ -
    44 typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
    │ │ │ │ -
    45
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <array>
    │ │ │ │ +
    23#include <gtsam/config.h>
    │ │ │ │ +
    24#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    27#include <numeric>
    │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    43template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    45 BOOST_CONCEPT_ASSERT((IsTestable<T>));
    │ │ │ │ +
    46
    │ │ │ │ +
    47protected:
    │ │ │ │
    48
    │ │ │ │ -
    57 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
    │ │ │ │ - │ │ │ │ + │ │ │ │ +
    50 static const int Dim = traits<T>::dimension;
    │ │ │ │ +
    51
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    55
    │ │ │ │ +
    56
    │ │ │ │ +
    57 public:
    │ │ │ │ +
    58 typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
    │ │ │ │
    59
    │ │ │ │ -
    67 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
    │ │ │ │ -
    68 const Ordering& ordering,
    │ │ │ │ - │ │ │ │ -
    70
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    73 }
    │ │ │ │ -
    │ │ │ │ -
    74
    │ │ │ │ -
    76
    │ │ │ │ -
    79
    │ │ │ │ -
    81 double lambda() const;
    │ │ │ │ -
    82
    │ │ │ │ -
    84 int getInnerIterations() const;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    68 const T& measurement, const Expression<T>& expression)
    │ │ │ │ +
    69 : NoiseModelFactor(noiseModel), measured_(measurement) {
    │ │ │ │ + │ │ │ │ +
    71 }
    │ │ │ │ +
    │ │ │ │ +
    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 void print(const std::string& str = "") const {
    │ │ │ │ -
    88 std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;
    │ │ │ │ -
    89 this->params_.print(" parameters:\n");
    │ │ │ │ -
    90 }
    │ │ │ │ +
    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) &&
    │ │ │ │ + │ │ │ │ +
    91 dims_ == p->dims_;
    │ │ │ │ +
    92 }
    │ │ │ │
    │ │ │ │ -
    91
    │ │ │ │
    93
    │ │ │ │ -
    96
    │ │ │ │ -
    101 GaussianFactorGraph::shared_ptr iterate() override;
    │ │ │ │ -
    102
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    105 return params_;
    │ │ │ │ -
    106 }
    │ │ │ │ -
    │ │ │ │ -
    107
    │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    118 bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);
    │ │ │ │ -
    119
    │ │ │ │ -
    121
    │ │ │ │ -
    122protected:
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    125 const NonlinearOptimizerParams& _params() const override {
    │ │ │ │ -
    126 return params_;
    │ │ │ │ -
    127 }
    │ │ │ │ -
    │ │ │ │ -
    128};
    │ │ │ │ -
    │ │ │ │ -
    129
    │ │ │ │ -
    130}
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │ -
    Parameters for Levenberg-Marquardt trust-region scheme.
    │ │ │ │ +
    124 // Create a writeable JacobianFactor in advance
    │ │ │ │ +
    125 boost::shared_ptr<JacobianFactor> factor(
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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.");
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    223};
    │ │ │ │ +
    │ │ │ │ +
    224// ExpressionFactor
    │ │ │ │ +
    225
    │ │ │ │ +
    227template <typename T>
    │ │ │ │ +
    228struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
    │ │ │ │ +
    229
    │ │ │ │ +
    241template <typename T, typename... Args>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    Expressions for Block Automatic Differentiation.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    This class performs Levenberg-Marquardt nonlinear optimization.
    Definition LevenbergMarquardtOptimizer.h:35
    │ │ │ │ -
    ~LevenbergMarquardtOptimizer() override
    Virtual destructor.
    Definition LevenbergMarquardtOptimizer.h:72
    │ │ │ │ -
    const LevenbergMarquardtParams & params() const
    Read-only access the parameters.
    Definition LevenbergMarquardtOptimizer.h:104
    │ │ │ │ -
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition LevenbergMarquardtOptimizer.h:125
    │ │ │ │ -
    const LevenbergMarquardtParams params_
    LM parameters.
    Definition LevenbergMarquardtOptimizer.h:38
    │ │ │ │ -
    void print(const std::string &str="") const
    print
    Definition LevenbergMarquardtOptimizer.h:87
    │ │ │ │ -
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    const Matrix & matrix() const
    Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
    Definition VerticalBlockMatrix.h:188
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │ +
    Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
    Error function without the NoiseModel, .
    Definition ExpressionFactor.h:99
    │ │ │ │ +
    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::NonlinearFactor::shared_ptr clone() const override
    Definition ExpressionFactor.h:151
    │ │ │ │ +
    virtual Expression< T > expression() const
    Recreate expression from keys_ and measured_, used in load below.
    Definition ExpressionFactor.h:193
    │ │ │ │ +
    Expression< T > expression_
    the expression that is AD enabled
    Definition ExpressionFactor.h:53
    │ │ │ │ +
    T measured_
    the measurement to be compared with the expression
    Definition ExpressionFactor.h:52
    │ │ │ │ +
    FastVector< int > dims_
    dimensions of the Jacobian matrices
    Definition ExpressionFactor.h:54
    │ │ │ │ +
    const T & measured() const
    return the measurement
    Definition ExpressionFactor.h:77
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
    linearize to a GaussianFactor
    Definition ExpressionFactor.h:112
    │ │ │ │ +
    ~ExpressionFactor() override
    Destructor.
    Definition ExpressionFactor.h:74
    │ │ │ │ +
    bool equals(const NonlinearFactor &f, double tol) const override
    equals relies on Testable traits being defined for T
    Definition ExpressionFactor.h:87
    │ │ │ │ +
    ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement)
    Default constructor, for serialization.
    Definition ExpressionFactor.h:161
    │ │ │ │ +
    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
    │ │ │ │ +
    void initialize(const Expression< T > &expression)
    Initialize with constructor arguments.
    Definition ExpressionFactor.h:167
    │ │ │ │ +
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ +
    N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
    Definition ExpressionFactor.h:242
    │ │ │ │ +
    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
    │ │ │ │ +
    ~ExpressionFactorN() override=default
    Destructor.
    │ │ │ │ +
    ExpressionFactorN()=default
    Default constructor, for serialization.
    │ │ │ │ +
    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
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition NonlinearFactor.cpp:74
    │ │ │ │ +
    bool equals(const NonlinearFactor &f, double tol=1e-9) const override
    Check if two factors are equal.
    Definition NonlinearFactor.cpp:82
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    noise model to the factor, and calculates the error by asking the user to implement the method
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,480 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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()); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -79 │ │ │ │ │ -81 double lambda() const; │ │ │ │ │ -82 │ │ │ │ │ -84 int getInnerIterations() const; │ │ │ │ │ +_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 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 │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -122protected: │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ -_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. │ │ │ │ │ +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 │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ +_E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ +Expressions for Block Automatic Differentiation. │ │ │ │ │ +_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_:_:_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_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ +_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_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 │ │ │ │ │ + * 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/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/ISAM2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Variables
    │ │ │ │ -
    WhiteNoiseFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    ISAM2.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Binary white noise factor. │ │ │ │ +

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ 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
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    September 2011
    │ │ │ │ +

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -const double  ggttssaamm::::llooggSSqqrrtt22PPII = log(std::sqrt(2.0 * M_PI)) │ │ │ │ │ -  constant needed below │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Binary white noise factor. │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - September 2011 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/NonlinearISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,39 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    LevenbergMarquardtOptimizer.cpp File Reference
    │ │ │ │ +
    NonlinearISAM.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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +
    Date
    Jan 19, 2010
    │ │ │ │ +
    Author
    Viorela Ila and Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,20 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +NonlinearISAM.cpp File Reference │ │ │ │ │ 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. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Jan 19, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Viorela Ila and 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_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _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/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/ISAM2Result.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    NonlinearFactorGraph.h File Reference
    │ │ │ │ +
    ISAM2Result.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor Graph consisting of non-linear factors. │ │ │ │ +

    Class that stores detailed iSAM2 result. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::NonlinearFactorGraph
    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::traits< NonlinearFactorGraph >
     traits 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Factor Graph consisting of non-linear factors.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Carlos Nieto
    │ │ │ │ -
    │ │ │ │ -Christian Potthast
    │ │ │ │ +

    Class that stores detailed iSAM2 result.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -NonlinearFactorGraph.h File Reference │ │ │ │ │ -Factor Graph consisting of non-linear factors. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ - 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_:_:_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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ + 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_F_a_c_t_o_r_G_r_a_p_h_._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/a01094.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01094 = [ │ │ │ │ │ - ["gtsam::NonlinearFactorGraph", "a04540.html", "a04540"], │ │ │ │ │ - ["gtsam::traits< NonlinearFactorGraph >", "a04544.html", null] │ │ │ │ │ + ["gtsam::ISAM2Result::DetailedResults", "a04424.html", "a04424"], │ │ │ │ │ + ["gtsam::ISAM2Result::DetailedResults::VariableStatus", "a04428.html", "a04428"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01094_source.html │ │ │ │ @@ -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/ISAM2Result.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,245 +98,161 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearFactorGraph.h
    │ │ │ │ +
    ISAM2Result.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
    │ │ │ │ +
    18// \callgraph
    │ │ │ │ +
    19
    │ │ │ │ +
    20#pragma once
    │ │ │ │
    21
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    22#include <string>
    │ │ │ │ +
    23#include <vector>
    │ │ │ │ +
    24
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    61 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    62
    │ │ │ │ -
    65
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    80
    │ │ │ │ - │ │ │ │ -
    83
    │ │ │ │ -
    87
    │ │ │ │ -
    89 void print(
    │ │ │ │ -
    90 const std::string& str = "NonlinearFactorGraph: ",
    │ │ │ │ -
    91 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
    │ │ │ │ +
    30#include <boost/variant.hpp>
    │ │ │ │ +
    31
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    54 boost::optional<double> errorBefore;
    │ │ │ │ +
    55
    │ │ │ │ +
    66 boost::optional<double> errorAfter;
    │ │ │ │ +
    67
    │ │ │ │ + │ │ │ │ +
    77
    │ │ │ │ + │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    89
    │ │ │ │ +
    91 size_t cliques;
    │ │ │ │
    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;
    │ │ │ │ + │ │ │ │ +
    103
    │ │ │ │ + │ │ │ │ +
    106
    │ │ │ │ + │ │ │ │ +
    109
    │ │ │ │ + │ │ │ │ +
    112
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    133 bool isRelinearized;
    │ │ │ │ + │ │ │ │ +
    138 bool isNew;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    141 : isReeliminated(false),
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    144 isRelinearized(false),
    │ │ │ │ +
    145 isObserved(false),
    │ │ │ │ +
    146 isNew(false),
    │ │ │ │ +
    147 inRootClique(false) {}
    │ │ │ │ +
    148 };
    │ │ │ │ +
    │ │ │ │ +
    149
    │ │ │ │ +
    150 using StatusMap = FastMap<Key, VariableStatus>;
    │ │ │ │ +
    151
    │ │ │ │ + │ │ │ │ +
    154 };
    │ │ │ │ +
    │ │ │ │ +
    155
    │ │ │ │ +
    158 boost::optional<DetailedResults> detail;
    │ │ │ │ +
    159
    │ │ │ │ +
    160 explicit ISAM2Result(bool enableDetailedResults = false) {
    │ │ │ │ +
    161 if (enableDetailedResults) detail.reset(DetailedResults());
    │ │ │ │ +
    162 }
    │ │ │ │
    163
    │ │ │ │ -
    166 Values updateCholesky(const Values& values, const Ordering& ordering,
    │ │ │ │ -
    167 const Dampen& dampen = nullptr) const;
    │ │ │ │ -
    168
    │ │ │ │ -
    170 NonlinearFactorGraph clone() const;
    │ │ │ │ -
    171
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    2D Point
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Graphviz formatter for NonlinearFactorGraph.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    Parameters for iSAM 2.
    │ │ │ │ +
    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ -
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │ -
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ -
    Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
    Definition GraphvizFormatting.h:32
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    NonlinearFactorGraph()
    Default constructor.
    Definition NonlinearFactorGraph.h:67
    │ │ │ │ -
    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
    │ │ │ │ -
    NonlinearFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition NonlinearFactorGraph.h:75
    │ │ │ │ -
    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
    │ │ │ │ -
    void addPrior(Key key, const T &prior, const Matrix &covariance)
    Convenience method which adds a PriorFactor to the factor graph.
    Definition NonlinearFactorGraph.h:218
    │ │ │ │ -
    NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition NonlinearFactorGraph.h:71
    │ │ │ │ -
    std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
    typdef for dampen functions used below
    Definition NonlinearFactorGraph.h:136
    │ │ │ │ -
    NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition NonlinearFactorGraph.h:79
    │ │ │ │ -
    virtual ~NonlinearFactorGraph()
    Destructor.
    Definition NonlinearFactorGraph.h:82
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ - │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │ +
    This struct is returned from ISAM2::update() and contains information about the update that is useful...
    Definition ISAM2Result.h:41
    │ │ │ │ +
    size_t variablesRelinearized
    The number of variables that were relinearized because their linear deltas exceeded the reslinearizat...
    Definition ISAM2Result.h:76
    │ │ │ │ +
    size_t getVariablesRelinearized() const
    Getters and Setters.
    Definition ISAM2Result.h:176
    │ │ │ │ +
    size_t factorsRecalculated
    The number of factors that were included in reelimination of the Bayes' tree.
    Definition ISAM2Result.h:88
    │ │ │ │ +
    DetailedResults * details()
    Return pointer to detail, 0 if no detail requested.
    Definition ISAM2Result.h:165
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string str="") const
    Print results.
    Definition ISAM2Result.h:168
    │ │ │ │ +
    boost::optional< DetailedResults > detail
    Detailed results, if enabled by ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:158
    │ │ │ │ +
    size_t cliques
    The number of cliques in the Bayes' Tree.
    Definition ISAM2Result.h:91
    │ │ │ │ +
    KeySet keysWithRemovedFactors
    Keys of variables that had factors removed.
    Definition ISAM2Result.h:108
    │ │ │ │ +
    size_t variablesReeliminated
    The number of variables that were reeliminated as parts of the Bayes' Tree were recalculated,...
    Definition ISAM2Result.h:84
    │ │ │ │ +
    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
    │ │ │ │ +
    KeyVector observedKeys
    keys for variables that were observed, i.e., not unused.
    Definition ISAM2Result.h:105
    │ │ │ │ +
    KeySet markedKeys
    All keys that were marked during the update process.
    Definition ISAM2Result.h:111
    │ │ │ │ +
    FactorIndices newFactorsIndices
    The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
    Definition ISAM2Result.h:97
    │ │ │ │ +
    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
    │ │ │ │ +
    A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:117
    │ │ │ │ +
    StatusMap variableStatus
    The status of each variable during this update, see VariableStatus.
    Definition ISAM2Result.h:153
    │ │ │ │ +
    The status of a single variable, this struct is stored in DetailedResults::variableStatus.
    Definition ISAM2Result.h:120
    │ │ │ │ +
    bool isRelinearizeInvolved
    Whether the variable was below the relinearization threshold but was relinearized by being involved i...
    Definition ISAM2Result.h:128
    │ │ │ │ +
    bool isObserved
    Whether the variable was relinearized, either by being above the relinearization threshold or by invo...
    Definition ISAM2Result.h:136
    │ │ │ │ +
    bool isNew
    Whether the variable itself was just added.
    Definition ISAM2Result.h:138
    │ │ │ │ +
    bool isAboveRelinThreshold
    Whether the variable was just relinearized due to being above the relinearization threshold.
    Definition ISAM2Result.h:125
    │ │ │ │ +
    bool isReeliminated
    Whether the variable was just reeliminated, due to being relinearized, observed, new,...
    Definition ISAM2Result.h:124
    │ │ │ │ +
    bool inRootClique
    Whether the variable is in the root clique.
    Definition ISAM2Result.h:139
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,314 +1,248 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearFactorGraph.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 │ │ │ │ │ -20// \callgraph │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ 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> │ │ │ │ │ +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#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 │ │ │ │ │ -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() {} │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -{ │ │ │ │ │ -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. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_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. │ │ │ │ │ +_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) │ │ │ │ │ +_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_:_:_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. │ │ │ │ │ -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_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_:_:_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_F_a_c_t_o_r_G_r_a_p_h_._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/a01097_source.html │ │ │ │ @@ -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/NonlinearConjugateGradientOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,474 +98,247 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Values-inl.h
    │ │ │ │ +
    NonlinearConjugateGradientOptimizer.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
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/base/Manifold.h>
    │ │ │ │ + │ │ │ │ +
    23#include <boost/tuple/tuple.hpp>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    27#include <utility>
    │ │ │ │ -
    28#include <boost/bind/bind.hpp>
    │ │ │ │ - │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ -
    32
    │ │ │ │ -
    33
    │ │ │ │ -
    34 /* ************************************************************************* */
    │ │ │ │ -
    35 template<class ValueType>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    37 const Key key;
    │ │ │ │ -
    38 ValueType& value;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30 /* a class for the nonlinearConjugateGradient template */
    │ │ │ │ +
    31 class System {
    │ │ │ │ +
    32 public:
    │ │ │ │ +
    33 typedef Values State;
    │ │ │ │ +
    34 typedef VectorValues Gradient;
    │ │ │ │ + │ │ │ │ +
    36
    │ │ │ │ +
    37 protected:
    │ │ │ │ +
    38 const NonlinearFactorGraph &graph_;
    │ │ │ │
    39
    │ │ │ │ -
    40 _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
    │ │ │ │ -
    41 };
    │ │ │ │ -
    │ │ │ │ -
    42
    │ │ │ │ -
    43 /* ************************************************************************* */
    │ │ │ │ -
    44 template<class ValueType>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46 const Key key;
    │ │ │ │ -
    47 const ValueType& value;
    │ │ │ │ -
    48
    │ │ │ │ -
    49 _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
    │ │ │ │ -
    50 key(_key), value(_value) {
    │ │ │ │ -
    51 }
    │ │ │ │ - │ │ │ │ -
    53 key(rhs.key), value(rhs.value) {
    │ │ │ │ -
    54 }
    │ │ │ │ -
    55 };
    │ │ │ │ -
    │ │ │ │ -
    56
    │ │ │ │ -
    57 /* ************************************************************************* */
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    54 typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
    │ │ │ │ +
    55
    │ │ │ │ +
    56protected:
    │ │ │ │ +
    57 Parameters params_;
    │ │ │ │
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    59 const NonlinearOptimizerParams& _params() const override {
    │ │ │ │ +
    60 return params_;
    │ │ │ │ +
    61 }
    │ │ │ │ +
    62
    │ │ │ │ +
    63public:
    │ │ │ │ +
    64
    │ │ │ │ + │ │ │ │ +
    67 const Values& initialValues, const Parameters& params = Parameters());
    │ │ │ │ +
    68
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    196 ConstFiltered(const Filtered<ValueType>& rhs) :
    │ │ │ │ -
    197 begin_(rhs.beginConst()),
    │ │ │ │ -
    198 end_(rhs.endConst()) {}
    │ │ │ │ +
    195 alpha = lineSearch(system, currentValues, direction);
    │ │ │ │ +
    196
    │ │ │ │ +
    197 prevValues = currentValues;
    │ │ │ │ +
    198 prevError = currentError;
    │ │ │ │
    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
    │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    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) {
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Wraps any type T so it can play as a Value.
    Definition GenericValue.h:47
    │ │ │ │ -
    const T & value() const
    Return a constant value.
    Definition GenericValue.h:67
    │ │ │ │ -
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ -
    Definition Values-inl.h:36
    │ │ │ │ -
    const Key key
    The key.
    Definition Values-inl.h:37
    │ │ │ │ -
    ValueType & value
    The value.
    Definition Values-inl.h:38
    │ │ │ │ -
    Definition Values-inl.h:45
    │ │ │ │ -
    const Key key
    The key.
    Definition Values-inl.h:46
    │ │ │ │ -
    const ValueType & value
    The value.
    Definition Values-inl.h:47
    │ │ │ │ -
    Definition Values-inl.h:62
    │ │ │ │ -
    Definition Values-inl.h:291
    │ │ │ │ -
    Definition Values-inl.h:304
    │ │ │ │ -
    void update(Key j, const Value &val)
    single element change of existing element
    Definition Values.cpp:180
    │ │ │ │ -
    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
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    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
    │ │ │ │ -
    void insert_or_assign(Key j, const Value &val)
    If key j exists, update value, else perform an insert.
    Definition Values.cpp:203
    │ │ │ │ -
    Values()=default
    Default constructor creates an empty Values class.
    │ │ │ │ -
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │ -
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ -
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:101
    │ │ │ │ -
    Definition Values.h:475
    │ │ │ │ -
    Definition Values.h:498
    │ │ │ │ -
    Definition Values.h:542
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    double lineSearch(const S &system, const V currentValues, const W &gradient)
    Implement the golden-section line search algorithm.
    Definition NonlinearConjugateGradientOptimizer.h:88
    │ │ │ │ +
    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
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    An implementation of the nonlinear CG method using the template below.
    Definition NonlinearConjugateGradientOptimizer.h:28
    │ │ │ │ +
    ~NonlinearConjugateGradientOptimizer() override
    Destructor.
    Definition NonlinearConjugateGradientOptimizer.h:70
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    double absoluteErrorTol
    The maximum absolute error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:43
    │ │ │ │ +
    IterationHook iterationHook
    Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
    Definition NonlinearOptimizerParams.h:94
    │ │ │ │ +
    size_t maxIterations
    The maximum iterations to stop iterating (default 100)
    Definition NonlinearOptimizerParams.h:41
    │ │ │ │ +
    Verbosity verbosity
    The printing verbosity during optimization (default SILENT)
    Definition NonlinearOptimizerParams.h:45
    │ │ │ │ +
    double relativeErrorTol
    The maximum relative error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:42
    │ │ │ │ +
    double errorTol
    The maximum total error to stop iterating (default 0.0)
    Definition NonlinearOptimizerParams.h:44
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,525 +1,298 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Values-inl.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 │ │ │ │ │ -25#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +_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 ___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 │ │ │ │ │ -57 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ +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 // 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 │ │ │ │ │ -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; │ │ │ │ │ +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 // 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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -196 ConstFiltered(const Filtered& rhs) : │ │ │ │ │ -197 begin_(rhs.beginConst()), │ │ │ │ │ -198 end_(rhs.endConst()) {} │ │ │ │ │ +195 alpha = _l_i_n_e_S_e_a_r_c_h(system, currentValues, direction); │ │ │ │ │ +196 │ │ │ │ │ +197 prevValues = currentValues; │ │ │ │ │ +198 prevError = currentError; │ │ │ │ │ 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 │ │ │ │ │ -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 } │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_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 │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * VVaalluueess--iinnll..hh │ │ │ │ │ + * _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/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/NonlinearOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    nonlinearExceptions.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    NonlinearOptimizer.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ +

    Convergence functions not dependent on graph types. │ │ │ │ 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ 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

    │ │ │ │ -

    Exceptions that may be thrown by nonlinear optimization components.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Aug 17, 2012
    │ │ │ │ +

    Convergence functions not dependent on graph types.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Jul 17, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ │ +Convergence functions not dependent on graph types. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2012 │ │ │ │ │ + Jul 17, 2010 │ │ │ │ │ * _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_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/a01100.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01100 = [ │ │ │ │ │ - ["gtsam::MarginalizeNonleafException", "a04492.html", null] │ │ │ │ │ + ["checkConvergence", "a01100.html#a3382482454c164c6f5817dcbe6f932b3", null] │ │ │ │ │ ]; │ │ │ ├── ./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/LevenbergMarquardtParams.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    ISAM2Clique.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    LevenbergMarquardtParams.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Specialized iSAM2 Clique. │ │ │ │ +

    Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ 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, Frank Dellaert
    │ │ │ │ +

    Parameters for Levenberg-Marquardt trust-region scheme.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -ISAM2Clique.cpp File Reference │ │ │ │ │ -Specialized iSAM2 Clique. _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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/LevenbergMarquardtOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,59 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Expression-inl.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    LevenbergMarquardtOptimizer.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Internals for Expression.h, not for general consumption. │ │ │ │ +

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::internal::apply_compose< T >
     
    struct  gtsam::internal::apply_compose< double >
    class  gtsam::LevenbergMarquardtOptimizer
     This class performs Levenberg-Marquardt nonlinear optimization. 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
    │ │ │ │ +

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │ -Paul Furgale
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 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_-_i_n_l_._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/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::LevenbergMarquardtOptimizer", "a04436.html", "a04436"] │ │ │ │ │ ]; │ │ │ ├── ./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/LevenbergMarquardtOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,419 +98,131 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Expression-inl.h
    │ │ │ │ +
    LevenbergMarquardtOptimizer.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>
    │ │ │ │ -
    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>
    │ │ │ │ -
    35
    │ │ │ │ -
    36namespace gtsam {
    │ │ │ │ -
    37
    │ │ │ │ -
    38template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    40 root_(new internal::ConstantExpression<T>(value)) {
    │ │ │ │ -
    41}
    │ │ │ │ -
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26#include <boost/date_time/posix_time/posix_time.hpp>
    │ │ │ │ +
    27
    │ │ │ │ +
    28class NonlinearOptimizerMoreOptimizationTest;
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    36
    │ │ │ │ +
    37protected:
    │ │ │ │ + │ │ │ │ +
    39 boost::posix_time::ptime startTime_;
    │ │ │ │ +
    40
    │ │ │ │ +
    41 void initTime();
    │ │ │ │
    42
    │ │ │ │ -
    43template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    45 root_(new internal::LeafExpression<T>(key)) {
    │ │ │ │ -
    46}
    │ │ │ │ -
    │ │ │ │ -
    47
    │ │ │ │ -
    48template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ +
    43public:
    │ │ │ │ +
    44 typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
    │ │ │ │ +
    45
    │ │ │ │ +
    48
    │ │ │ │ +
    57 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
    │ │ │ │ + │ │ │ │ +
    59
    │ │ │ │ +
    67 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
    │ │ │ │ +
    68 const Ordering& ordering,
    │ │ │ │ + │ │ │ │ +
    70
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    73 }
    │ │ │ │ +
    │ │ │ │ +
    74
    │ │ │ │ +
    76
    │ │ │ │ +
    79
    │ │ │ │ +
    81 double lambda() const;
    │ │ │ │ +
    82
    │ │ │ │ +
    84 int getInnerIterations() const;
    │ │ │ │ +
    85
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    105 return params_;
    │ │ │ │ +
    106 }
    │ │ │ │ +
    │ │ │ │ +
    107
    │ │ │ │ +
    108 void writeLogFile(double currentError);
    │ │ │ │ +
    109
    │ │ │ │ +
    111 virtual GaussianFactorGraph::shared_ptr linearize() const;
    │ │ │ │
    112
    │ │ │ │ -
    114template<typename T>
    │ │ │ │ -
    │ │ │ │ -
    115template<typename A1, typename A2, typename A3>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    146template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    168 return root_->traceSize();
    │ │ │ │ -
    169}
    │ │ │ │ -
    │ │ │ │ -
    170
    │ │ │ │ -
    │ │ │ │ -
    171// Private methods:
    │ │ │ │ -
    172
    │ │ │ │ -
    173template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    175 const KeyVector& keys, const FastVector<int>& dims,
    │ │ │ │ -
    176 std::vector<Matrix>& H) const {
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    194 return result;
    │ │ │ │ -
    195}
    │ │ │ │ -
    196
    │ │ │ │ -
    │ │ │ │ -
    197template<typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    227 T value(this->traceExecution(values, trace, traceStorage));
    │ │ │ │ -
    228 trace.startReverseAD1(jacobians);
    │ │ │ │ -
    229
    │ │ │ │ -
    230 return value;
    │ │ │ │ -
    231}
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    232
    │ │ │ │ -
    233template<typename T>
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
    │ │ │ │ -
    304 return *this;
    │ │ │ │ -
    305}
    │ │ │ │ -
    │ │ │ │ -
    306
    │ │ │ │ -
    307} // namespace gtsam
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ +
    114 GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear,
    │ │ │ │ +
    115 const VectorValues& sqrtHessianDiagonal) const;
    │ │ │ │ +
    116
    │ │ │ │ +
    118 bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);
    │ │ │ │ +
    119
    │ │ │ │ +
    121
    │ │ │ │ +
    122protected:
    │ │ │ │ +
    123
    │ │ │ │ +
    │ │ │ │ +
    125 const NonlinearOptimizerParams& _params() const override {
    │ │ │ │ +
    126 return params_;
    │ │ │ │ +
    127 }
    │ │ │ │ +
    │ │ │ │ +
    128};
    │ │ │ │ +
    │ │ │ │ +
    129
    │ │ │ │ +
    130}
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │ +
    Parameters for Levenberg-Marquardt trust-region scheme.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    Key symbol(unsigned char c, std::uint64_t j)
    Create a symbol key from a character and index, i.e.
    Definition Symbol.h:135
    │ │ │ │ -
    std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
    Construct an array of leaves.
    Definition Expression-inl.h:284
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    : meta-function to generate JacobianTA optional reference Used mainly by Expressions
    Definition OptionalJacobian.h:261
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ -
    Definition Expression-inl.h:247
    │ │ │ │ -
    Definition Expression.h:40
    │ │ │ │ -
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ -
    Expression()
    Default constructor, for serialization.
    Definition Expression.h:182
    │ │ │ │ -
    std::set< Key > keys() const
    Return keys that play in this expression.
    Definition Expression-inl.h:132
    │ │ │ │ -
    std::pair< KeyVector, FastVector< int > > KeysAndDims
    Keys and dimensions in same order.
    Definition Expression.h:185
    │ │ │ │ -
    void dims(std::map< Key, int > &map) const
    Return dimensions for each argument, as a map.
    Definition Expression-inl.h:137
    │ │ │ │ -
    void print(const std::string &s) const
    Print.
    Definition Expression-inl.h:142
    │ │ │ │ -
    size_t traceSize() const
    Return size needed for memory buffer in traceExecution.
    Definition Expression-inl.h:167
    │ │ │ │ -
    T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
    trace execution, very unsafe
    Definition Expression-inl.h:198
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    This class performs Levenberg-Marquardt nonlinear optimization.
    Definition LevenbergMarquardtOptimizer.h:35
    │ │ │ │ +
    ~LevenbergMarquardtOptimizer() override
    Virtual destructor.
    Definition LevenbergMarquardtOptimizer.h:72
    │ │ │ │ +
    const LevenbergMarquardtParams & params() const
    Read-only access the parameters.
    Definition LevenbergMarquardtOptimizer.h:104
    │ │ │ │ +
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition LevenbergMarquardtOptimizer.h:125
    │ │ │ │ +
    const LevenbergMarquardtParams params_
    LM parameters.
    Definition LevenbergMarquardtOptimizer.h:38
    │ │ │ │ +
    void print(const std::string &str="") const
    print
    Definition LevenbergMarquardtOptimizer.h:87
    │ │ │ │ +
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,408 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Expression-inl.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 │ │ │ │ │ -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 │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ +79 │ │ │ │ │ +81 double lambda() const; │ │ │ │ │ +82 │ │ │ │ │ +84 int getInnerIterations() const; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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}; │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_:_:_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_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 │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_-_i_n_l_._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/a01112_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,213 +98,245 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GncParams.h
    │ │ │ │ +
    NonlinearFactorGraph.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
    │ │ │ │ -
    27#pragma once
    │ │ │ │ -
    28
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    31
    │ │ │ │ -
    32namespace gtsam {
    │ │ │ │ -
    33
    │ │ │ │ -
    34/* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    37 GM /*Geman McClure*/,
    │ │ │ │ -
    38 TLS /*Truncated least squares*/
    │ │ │ │ -
    39};
    │ │ │ │ -
    │ │ │ │ -
    40
    │ │ │ │ -
    41template<class BaseOptimizerParameters>
    │ │ │ │ -
    │ │ │ │ -
    42class GncParams {
    │ │ │ │ -
    43 public:
    │ │ │ │ -
    45 typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
    │ │ │ │ -
    46
    │ │ │ │ -
    │ │ │ │ -
    48 enum Verbosity {
    │ │ │ │ -
    49 SILENT = 0,
    │ │ │ │ -
    50 SUMMARY,
    │ │ │ │ -
    51 MU,
    │ │ │ │ -
    52 WEIGHTS,
    │ │ │ │ -
    53 VALUES
    │ │ │ │ -
    54 };
    │ │ │ │ -
    │ │ │ │ -
    55
    │ │ │ │ -
    │ │ │ │ -
    57 GncParams(const BaseOptimizerParameters& baseOptimizerParams)
    │ │ │ │ - │ │ │ │ -
    59 }
    │ │ │ │ -
    │ │ │ │ -
    60
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    64 }
    │ │ │ │ -
    │ │ │ │ +
    20// \callgraph
    │ │ │ │ +
    21
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    61 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    62
    │ │ │ │
    65
    │ │ │ │ -
    67 BaseOptimizerParameters baseOptimizerParams;
    │ │ │ │ - │ │ │ │ -
    70 size_t maxIterations = 100;
    │ │ │ │ -
    71 double muStep = 1.4;
    │ │ │ │ -
    72 double relativeCostTol = 1e-5;
    │ │ │ │ -
    73 double weightsTol = 1e-4;
    │ │ │ │ - │ │ │ │ -
    75
    │ │ │ │ -
    76 //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    82 IndexVector knownOutliers = IndexVector();
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    80
    │ │ │ │ + │ │ │ │
    83
    │ │ │ │ -
    │ │ │ │ -
    85 void setLossType(const GncLossType type) {
    │ │ │ │ -
    86 lossType = type;
    │ │ │ │ -
    87 }
    │ │ │ │ -
    │ │ │ │ -
    88
    │ │ │ │ -
    │ │ │ │ -
    90 void setMaxIterations(const size_t maxIter) {
    │ │ │ │ -
    91 std::cout
    │ │ │ │ -
    92 << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
    │ │ │ │ -
    93 << std::endl;
    │ │ │ │ -
    94 maxIterations = maxIter;
    │ │ │ │ -
    95 }
    │ │ │ │ -
    │ │ │ │ -
    96
    │ │ │ │ -
    │ │ │ │ -
    98 void setMuStep(const double step) {
    │ │ │ │ -
    99 muStep = step;
    │ │ │ │ -
    100 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    103 void setRelativeCostTol(double value) {
    │ │ │ │ -
    104 relativeCostTol = value;
    │ │ │ │ -
    105 }
    │ │ │ │ -
    │ │ │ │ -
    106
    │ │ │ │ -
    │ │ │ │ -
    108 void setWeightsTol(double value) {
    │ │ │ │ -
    109 weightsTol = value;
    │ │ │ │ -
    110 }
    │ │ │ │ -
    │ │ │ │ +
    105
    │ │ │ │ +
    107 double error(const Values& values) const;
    │ │ │ │ +
    108
    │ │ │ │ +
    110 double probPrime(const Values& values) const;
    │ │ │ │
    111
    │ │ │ │ -
    │ │ │ │ -
    113 void setVerbosityGNC(const Verbosity value) {
    │ │ │ │ -
    114 verbosity = value;
    │ │ │ │ -
    115 }
    │ │ │ │ -
    │ │ │ │ +
    115 boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
    │ │ │ │
    116
    │ │ │ │ -
    │ │ │ │ -
    123 void setKnownInliers(const IndexVector& knownIn) {
    │ │ │ │ -
    124 for (size_t i = 0; i < knownIn.size(); i++){
    │ │ │ │ -
    125 knownInliers.push_back(knownIn[i]);
    │ │ │ │ -
    126 }
    │ │ │ │ -
    127 std::sort(knownInliers.begin(), knownInliers.end());
    │ │ │ │ -
    128 }
    │ │ │ │ -
    │ │ │ │ -
    129
    │ │ │ │ -
    │ │ │ │ -
    134 void setKnownOutliers(const IndexVector& knownOut) {
    │ │ │ │ -
    135 for (size_t i = 0; i < knownOut.size(); i++){
    │ │ │ │ -
    136 knownOutliers.push_back(knownOut[i]);
    │ │ │ │ -
    137 }
    │ │ │ │ -
    138 std::sort(knownOutliers.begin(), knownOutliers.end());
    │ │ │ │ -
    139 }
    │ │ │ │ -
    │ │ │ │ -
    140
    │ │ │ │ -
    │ │ │ │ -
    142 bool equals(const GncParams& other, double tol = 1e-9) const {
    │ │ │ │ -
    143 return baseOptimizerParams.equals(other.baseOptimizerParams)
    │ │ │ │ -
    144 && lossType == other.lossType && maxIterations == other.maxIterations
    │ │ │ │ -
    145 && std::fabs(muStep - other.muStep) <= tol
    │ │ │ │ -
    146 && verbosity == other.verbosity && knownInliers == other.knownInliers
    │ │ │ │ -
    147 && knownOutliers == other.knownOutliers;
    │ │ │ │ -
    148 }
    │ │ │ │ -
    │ │ │ │ -
    149
    │ │ │ │ -
    │ │ │ │ -
    151 void print(const std::string& str) const {
    │ │ │ │ -
    152 std::cout << str << "\n";
    │ │ │ │ -
    153 switch (lossType) {
    │ │ │ │ -
    154 case GM:
    │ │ │ │ -
    155 std::cout << "lossType: Geman McClure" << "\n";
    │ │ │ │ -
    156 break;
    │ │ │ │ -
    157 case TLS:
    │ │ │ │ -
    158 std::cout << "lossType: Truncated Least-squares" << "\n";
    │ │ │ │ -
    159 break;
    │ │ │ │ -
    160 default:
    │ │ │ │ -
    161 throw std::runtime_error("GncParams::print: unknown loss type.");
    │ │ │ │ -
    162 }
    │ │ │ │ -
    163 std::cout << "maxIterations: " << maxIterations << "\n";
    │ │ │ │ -
    164 std::cout << "muStep: " << muStep << "\n";
    │ │ │ │ -
    165 std::cout << "relativeCostTol: " << relativeCostTol << "\n";
    │ │ │ │ -
    166 std::cout << "weightsTol: " << weightsTol << "\n";
    │ │ │ │ -
    167 std::cout << "verbosity: " << verbosity << "\n";
    │ │ │ │ -
    168 for (size_t i = 0; i < knownInliers.size(); i++)
    │ │ │ │ -
    169 std::cout << "knownInliers: " << knownInliers[i] << "\n";
    │ │ │ │ -
    170 for (size_t i = 0; i < knownOutliers.size(); i++)
    │ │ │ │ -
    171 std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
    │ │ │ │ -
    172 baseOptimizerParams.print("Base optimizer params: ");
    │ │ │ │ -
    173 }
    │ │ │ │ -
    │ │ │ │ -
    174};
    │ │ │ │ -
    │ │ │ │ -
    175
    │ │ │ │ -
    176}
    │ │ │ │ - │ │ │ │ -
    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
    │ │ │ │ -
    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
    │ │ │ │ +
    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;
    │ │ │ │ +
    168
    │ │ │ │ +
    170 NonlinearFactorGraph clone() const;
    │ │ │ │ +
    171
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    2D Point
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    Graphviz formatter for NonlinearFactorGraph.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    GncLossType
    Choice of robust loss function for GNC.
    Definition GncParams.h:36
    │ │ │ │ -
    Definition GncParams.h:42
    │ │ │ │ -
    BaseOptimizerParameters baseOptimizerParams
    GNC parameters.
    Definition GncParams.h:67
    │ │ │ │ -
    IndexVector knownInliers
    Slots in the factor graph corresponding to measurements that we know are outliers.
    Definition GncParams.h:80
    │ │ │ │ -
    GncParams()
    Default constructor.
    Definition GncParams.h:62
    │ │ │ │ -
    BaseOptimizerParameters::OptimizerType OptimizerType
    For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
    Definition GncParams.h:45
    │ │ │ │ -
    void setKnownInliers(const IndexVector &knownIn)
    (Optional) Provide a vector of measurements that must be considered inliers.
    Definition GncParams.h:123
    │ │ │ │ -
    void print(const std::string &str) const
    Print.
    Definition GncParams.h:151
    │ │ │ │ -
    FastVector< uint64_t > IndexVector
    Slots in the factor graph corresponding to measurements that we know are inliers.
    Definition GncParams.h:79
    │ │ │ │ -
    GncParams(const BaseOptimizerParameters &baseOptimizerParams)
    Constructor.
    Definition GncParams.h:57
    │ │ │ │ -
    double muStep
    Multiplicative factor to reduce/increase the mu in gnc.
    Definition GncParams.h:71
    │ │ │ │ -
    double weightsTol
    If the weights are within weightsTol from being binary, stop iterating (only for TLS)
    Definition GncParams.h:73
    │ │ │ │ -
    void setKnownOutliers(const IndexVector &knownOut)
    (Optional) Provide a vector of measurements that must be considered outliers.
    Definition GncParams.h:134
    │ │ │ │ -
    Verbosity verbosity
    Verbosity level.
    Definition GncParams.h:74
    │ │ │ │ -
    bool equals(const GncParams &other, double tol=1e-9) const
    Equals.
    Definition GncParams.h:142
    │ │ │ │ -
    void setRelativeCostTol(double value)
    Set the maximum relative difference in mu values to stop iterating.
    Definition GncParams.h:103
    │ │ │ │ -
    GncLossType lossType
    any other specific GNC parameters:
    Definition GncParams.h:69
    │ │ │ │ -
    size_t maxIterations
    Maximum number of iterations.
    Definition GncParams.h:70
    │ │ │ │ -
    void setLossType(const GncLossType type)
    Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
    Definition GncParams.h:85
    │ │ │ │ -
    void setMuStep(const double step)
    Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
    Definition GncParams.h:98
    │ │ │ │ -
    double relativeCostTol
    If relative cost change is below this threshold, stop iterating.
    Definition GncParams.h:72
    │ │ │ │ -
    void setVerbosityGNC(const Verbosity value)
    Set the verbosity level.
    Definition GncParams.h:113
    │ │ │ │ -
    void setMaxIterations(const size_t maxIter)
    Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate...
    Definition GncParams.h:90
    │ │ │ │ -
    Verbosity
    Verbosity levels.
    Definition GncParams.h:48
    │ │ │ │ -
    void setWeightsTol(double value)
    Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
    Definition GncParams.h:108
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ +
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │ +
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ +
    Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
    Definition GraphvizFormatting.h:32
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    NonlinearFactorGraph()
    Default constructor.
    Definition NonlinearFactorGraph.h:67
    │ │ │ │ +
    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
    │ │ │ │ +
    NonlinearFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition NonlinearFactorGraph.h:75
    │ │ │ │ +
    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
    │ │ │ │ +
    void addPrior(Key key, const T &prior, const Matrix &covariance)
    Convenience method which adds a PriorFactor to the factor graph.
    Definition NonlinearFactorGraph.h:218
    │ │ │ │ +
    NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition NonlinearFactorGraph.h:71
    │ │ │ │ +
    std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
    typdef for dampen functions used below
    Definition NonlinearFactorGraph.h:136
    │ │ │ │ +
    NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition NonlinearFactorGraph.h:79
    │ │ │ │ +
    virtual ~NonlinearFactorGraph()
    Destructor.
    Definition NonlinearFactorGraph.h:82
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ + │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,265 +1,314 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GncParams.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 │ │ │ │ │ -27#pragma once │ │ │ │ │ -28 │ │ │ │ │ -29#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> │ │ │ │ │ -30#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> │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_6enum _G_n_c_L_o_s_s_T_y_p_e { │ │ │ │ │ -37 GM /*Geman McClure*/, │ │ │ │ │ -38 TLS /*Truncated least squares*/ │ │ │ │ │ -39}; │ │ │ │ │ -40 │ │ │ │ │ -41template │ │ │ │ │ -_4_2class _G_n_c_P_a_r_a_m_s { │ │ │ │ │ -43 public: │ │ │ │ │ -_4_5 typedef typename BaseOptimizerParameters::OptimizerType _O_p_t_i_m_i_z_e_r_T_y_p_e; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 enum _V_e_r_b_o_s_i_t_y { │ │ │ │ │ -49 SILENT = 0, │ │ │ │ │ -50 SUMMARY, │ │ │ │ │ -51 MU, │ │ │ │ │ -52 WEIGHTS, │ │ │ │ │ -53 VALUES │ │ │ │ │ -54 }; │ │ │ │ │ -55 │ │ │ │ │ -_5_7 _G_n_c_P_a_r_a_m_s(const BaseOptimizerParameters& _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) │ │ │ │ │ -58 : _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s(_b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) { │ │ │ │ │ -59 } │ │ │ │ │ -60 │ │ │ │ │ -_6_2 _G_n_c_P_a_r_a_m_s() │ │ │ │ │ -63 : _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() { │ │ │ │ │ -64 } │ │ │ │ │ +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 │ │ │ │ │ 65 │ │ │ │ │ -_6_7 BaseOptimizerParameters _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s; │ │ │ │ │ -_6_9 _G_n_c_L_o_s_s_T_y_p_e _l_o_s_s_T_y_p_e = TLS; │ │ │ │ │ -_7_0 size_t _m_a_x_I_t_e_r_a_t_i_o_n_s = 100; │ │ │ │ │ -_7_1 double _m_u_S_t_e_p = 1.4; │ │ │ │ │ -_7_2 double _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l = 1e-5; │ │ │ │ │ -_7_3 double _w_e_i_g_h_t_s_T_o_l = 1e-4; │ │ │ │ │ -_7_4 _V_e_r_b_o_s_i_t_y _v_e_r_b_o_s_i_t_y = SILENT; │ │ │ │ │ -75 │ │ │ │ │ -76 //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is │ │ │ │ │ -globally enabled. │ │ │ │ │ -_7_8 using _I_n_d_e_x_V_e_c_t_o_r = _F_a_s_t_V_e_c_t_o_r_<_u_i_n_t_6_4___t_>; │ │ │ │ │ -_8_0 _I_n_d_e_x_V_e_c_t_o_r _k_n_o_w_n_I_n_l_i_e_r_s = _I_n_d_e_x_V_e_c_t_o_r(); │ │ │ │ │ -82 _I_n_d_e_x_V_e_c_t_o_r knownOutliers = _I_n_d_e_x_V_e_c_t_o_r(); │ │ │ │ │ +_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 │ │ │ │ │ -_8_5 void _s_e_t_L_o_s_s_T_y_p_e(const _G_n_c_L_o_s_s_T_y_p_e type) { │ │ │ │ │ -86 _l_o_s_s_T_y_p_e = type; │ │ │ │ │ -87 } │ │ │ │ │ -88 │ │ │ │ │ -_9_0 void _s_e_t_M_a_x_I_t_e_r_a_t_i_o_n_s(const size_t maxIter) { │ │ │ │ │ -91 std::cout │ │ │ │ │ -92 << "setMaxIterations: changing the max nr of iters might lead to less │ │ │ │ │ -accurate solutions and is not recommended! " │ │ │ │ │ -93 << std::endl; │ │ │ │ │ -94 _m_a_x_I_t_e_r_a_t_i_o_n_s = maxIter; │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_9_8 void _s_e_t_M_u_S_t_e_p(const double step) { │ │ │ │ │ -99 _m_u_S_t_e_p = step; │ │ │ │ │ -100 } │ │ │ │ │ +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 │ │ │ │ │ -_1_0_3 void _s_e_t_R_e_l_a_t_i_v_e_C_o_s_t_T_o_l(double value) { │ │ │ │ │ -104 _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l = value; │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 void _s_e_t_W_e_i_g_h_t_s_T_o_l(double value) { │ │ │ │ │ -109 _w_e_i_g_h_t_s_T_o_l = value; │ │ │ │ │ -110 } │ │ │ │ │ +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 │ │ │ │ │ -_1_1_3 void _s_e_t_V_e_r_b_o_s_i_t_y_G_N_C(const _V_e_r_b_o_s_i_t_y value) { │ │ │ │ │ -114 _v_e_r_b_o_s_i_t_y = value; │ │ │ │ │ -115 } │ │ │ │ │ +115 boost::shared_ptr symbolic() const; │ │ │ │ │ 116 │ │ │ │ │ -_1_2_3 void _s_e_t_K_n_o_w_n_I_n_l_i_e_r_s(const _I_n_d_e_x_V_e_c_t_o_r& knownIn) { │ │ │ │ │ -124 for (size_t i = 0; i < knownIn.size(); i++){ │ │ │ │ │ -125 _k_n_o_w_n_I_n_l_i_e_r_s.push_back(knownIn[i]); │ │ │ │ │ -126 } │ │ │ │ │ -127 std::sort(_k_n_o_w_n_I_n_l_i_e_r_s.begin(), _k_n_o_w_n_I_n_l_i_e_r_s.end()); │ │ │ │ │ -128 } │ │ │ │ │ -129 │ │ │ │ │ -_1_3_4 void _s_e_t_K_n_o_w_n_O_u_t_l_i_e_r_s(const _I_n_d_e_x_V_e_c_t_o_r& knownOut) { │ │ │ │ │ -135 for (size_t i = 0; i < knownOut.size(); i++){ │ │ │ │ │ -136 knownOutliers.push_back(knownOut[i]); │ │ │ │ │ -137 } │ │ │ │ │ -138 std::sort(knownOutliers.begin(), knownOutliers.end()); │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 bool _e_q_u_a_l_s(const _G_n_c_P_a_r_a_m_s& other, double tol = 1e-9) const { │ │ │ │ │ -143 return _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s.equals(other._b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) │ │ │ │ │ -144 && _l_o_s_s_T_y_p_e == other._l_o_s_s_T_y_p_e && _m_a_x_I_t_e_r_a_t_i_o_n_s == other._m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -145 && std::fabs(_m_u_S_t_e_p - other._m_u_S_t_e_p) <= tol │ │ │ │ │ -146 && _v_e_r_b_o_s_i_t_y == other._v_e_r_b_o_s_i_t_y && _k_n_o_w_n_I_n_l_i_e_r_s == other._k_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ -147 && knownOutliers == other.knownOutliers; │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 void _p_r_i_n_t(const std::string& str) const { │ │ │ │ │ -152 std::cout << str << "\n"; │ │ │ │ │ -153 switch (_l_o_s_s_T_y_p_e) { │ │ │ │ │ -154 case GM: │ │ │ │ │ -155 std::cout << "lossType: Geman McClure" << "\n"; │ │ │ │ │ -156 break; │ │ │ │ │ -157 case TLS: │ │ │ │ │ -158 std::cout << "lossType: Truncated Least-squares" << "\n"; │ │ │ │ │ -159 break; │ │ │ │ │ -160 default: │ │ │ │ │ -161 throw std::runtime_error("GncParams::print: unknown loss type."); │ │ │ │ │ -162 } │ │ │ │ │ -163 std::cout << "maxIterations: " << _m_a_x_I_t_e_r_a_t_i_o_n_s << "\n"; │ │ │ │ │ -164 std::cout << "muStep: " << _m_u_S_t_e_p << "\n"; │ │ │ │ │ -165 std::cout << "relativeCostTol: " << _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l << "\n"; │ │ │ │ │ -166 std::cout << "weightsTol: " << _w_e_i_g_h_t_s_T_o_l << "\n"; │ │ │ │ │ -167 std::cout << "verbosity: " << _v_e_r_b_o_s_i_t_y << "\n"; │ │ │ │ │ -168 for (size_t i = 0; i < _k_n_o_w_n_I_n_l_i_e_r_s.size(); i++) │ │ │ │ │ -169 std::cout << "knownInliers: " << _k_n_o_w_n_I_n_l_i_e_r_s[i] << "\n"; │ │ │ │ │ -170 for (size_t i = 0; i < knownOutliers.size(); i++) │ │ │ │ │ -171 std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; │ │ │ │ │ -172 _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s.print("Base optimizer params: "); │ │ │ │ │ -173 } │ │ │ │ │ -174}; │ │ │ │ │ -175 │ │ │ │ │ -176} │ │ │ │ │ -_G_a_u_s_s_N_e_w_t_o_n_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 │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_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 │ │ │ │ │ +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; │ │ │ │ │ +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)); │ │ │ │ │ +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 │ │ │ │ │ +{ │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_G_n_c_L_o_s_s_T_y_p_e │ │ │ │ │ -GncLossType │ │ │ │ │ -Choice of robust loss function for GNC. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -BaseOptimizerParameters baseOptimizerParams │ │ │ │ │ -GNC parameters. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_k_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ -IndexVector knownInliers │ │ │ │ │ -Slots in the factor graph corresponding to measurements that we know are │ │ │ │ │ -outliers. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ -GncParams() │ │ │ │ │ +_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_:_:_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_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 GncParams.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_O_p_t_i_m_i_z_e_r_T_y_p_e │ │ │ │ │ -BaseOptimizerParameters::OptimizerType OptimizerType │ │ │ │ │ -For each parameter, specify the corresponding optimizer: e.g., │ │ │ │ │ -GaussNewtonParams -> GaussNewtonOptimi... │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_K_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ -void setKnownInliers(const IndexVector &knownIn) │ │ │ │ │ -(Optional) Provide a vector of measurements that must be considered inliers. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str) const │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_I_n_d_e_x_V_e_c_t_o_r │ │ │ │ │ -FastVector< uint64_t > IndexVector │ │ │ │ │ -Slots in the factor graph corresponding to measurements that we know are │ │ │ │ │ -inliers. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ -GncParams(const BaseOptimizerParameters &baseOptimizerParams) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_m_u_S_t_e_p │ │ │ │ │ -double muStep │ │ │ │ │ -Multiplicative factor to reduce/increase the mu in gnc. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_w_e_i_g_h_t_s_T_o_l │ │ │ │ │ -double weightsTol │ │ │ │ │ -If the weights are within weightsTol from being binary, stop iterating (only │ │ │ │ │ -for TLS) │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_K_n_o_w_n_O_u_t_l_i_e_r_s │ │ │ │ │ -void setKnownOutliers(const IndexVector &knownOut) │ │ │ │ │ -(Optional) Provide a vector of measurements that must be considered outliers. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y │ │ │ │ │ -Verbosity verbosity │ │ │ │ │ -Verbosity level. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const GncParams &other, double tol=1e-9) const │ │ │ │ │ -Equals. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_R_e_l_a_t_i_v_e_C_o_s_t_T_o_l │ │ │ │ │ -void setRelativeCostTol(double value) │ │ │ │ │ -Set the maximum relative difference in mu values to stop iterating. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_l_o_s_s_T_y_p_e │ │ │ │ │ -GncLossType lossType │ │ │ │ │ -any other specific GNC parameters: │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -size_t maxIterations │ │ │ │ │ -Maximum number of iterations. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_L_o_s_s_T_y_p_e │ │ │ │ │ -void setLossType(const GncLossType type) │ │ │ │ │ -Set the robust loss function to be used in GNC (chosen among the ones in │ │ │ │ │ -GncLossType). │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_M_u_S_t_e_p │ │ │ │ │ -void setMuStep(const double step) │ │ │ │ │ -Set the graduated non-convexity step: at each GNC iteration, mu is updated as │ │ │ │ │ -mu <- mu * muStep. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_C_o_s_t_T_o_l │ │ │ │ │ -double relativeCostTol │ │ │ │ │ -If relative cost change is below this threshold, stop iterating. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_V_e_r_b_o_s_i_t_y_G_N_C │ │ │ │ │ -void setVerbosityGNC(const Verbosity value) │ │ │ │ │ -Set the verbosity level. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_M_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -void setMaxIterations(const size_t maxIter) │ │ │ │ │ -Set the maximum number of iterations in GNC (changing the max nr of iters might │ │ │ │ │ -lead to less accurate... │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ -Verbosity │ │ │ │ │ -Verbosity levels. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_W_e_i_g_h_t_s_T_o_l │ │ │ │ │ -void setWeightsTol(double value) │ │ │ │ │ -Set the maximum difference between the weights and their rounding in {0,1} to │ │ │ │ │ -stop iterating. │ │ │ │ │ -DDeeffiinniittiioonn GncParams.h:108 │ │ │ │ │ +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 │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * GGnnccPPaarraammss..hh │ │ │ │ │ + * _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/a01115.html │ │ │ │ @@ -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/nonlinear/ISAM2-impl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM2Result.h File Reference
    │ │ │ │ +
    ISAM2-impl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class that stores detailed iSAM2 result. │ │ │ │ +

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ 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...
    class  gtsam::ISAM2BayesTree
     
    struct  gtsam::ISAM2Result::DetailedResults
     A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults. More...
    class  gtsam::ISAM2JunctionTree
     
    struct  gtsam::ISAM2Result::DetailedResults::VariableStatus
     The status of a single variable, this struct is stored in DetailedResults::variableStatus. More...
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Class that stores detailed iSAM2 result.

    │ │ │ │ +

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

    │ │ │ │
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +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 │ │ │ │ │ -ISAM2Result.h File Reference │ │ │ │ │ -Class that stores detailed iSAM2 result. _M_o_r_e_._._. │ │ │ │ │ +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 │ │ │ │ │ -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_._._. │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class that stores detailed iSAM2 result. │ │ │ │ │ +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_R_e_s_u_l_t_._h │ │ │ │ │ + * _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/a01115.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,8 @@ │ │ │ │ │ var a01115 = [ │ │ │ │ │ - ["gtsam::ISAM2Result::DetailedResults", "a04424.html", "a04424"], │ │ │ │ │ - ["gtsam::ISAM2Result::DetailedResults::VariableStatus", "a04428.html", "a04428"] │ │ │ │ │ + ["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/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/ISAM2-impl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,161 +98,566 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2Result.h
    │ │ │ │ +
    ISAM2-impl.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 <string>
    │ │ │ │ -
    23#include <vector>
    │ │ │ │ -
    24
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30#include <boost/variant.hpp>
    │ │ │ │ -
    31
    │ │ │ │ -
    32namespace gtsam {
    │ │ │ │ -
    33
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    54 boost::optional<double> errorBefore;
    │ │ │ │ -
    55
    │ │ │ │ -
    66 boost::optional<double> errorAfter;
    │ │ │ │ -
    67
    │ │ │ │ - │ │ │ │ -
    77
    │ │ │ │ - │ │ │ │ -
    85
    │ │ │ │ - │ │ │ │ -
    89
    │ │ │ │ -
    91 size_t cliques;
    │ │ │ │ -
    92
    │ │ │ │ - │ │ │ │ -
    98
    │ │ │ │ - │ │ │ │ -
    103
    │ │ │ │ - │ │ │ │ -
    106
    │ │ │ │ - │ │ │ │ -
    109
    │ │ │ │ - │ │ │ │ -
    112
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    133 bool isRelinearized;
    │ │ │ │ - │ │ │ │ -
    138 bool isNew;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    141 : isReeliminated(false),
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    144 isRelinearized(false),
    │ │ │ │ -
    145 isObserved(false),
    │ │ │ │ -
    146 isNew(false),
    │ │ │ │ -
    147 inRootClique(false) {}
    │ │ │ │ -
    148 };
    │ │ │ │ -
    │ │ │ │ -
    149
    │ │ │ │ -
    150 using StatusMap = FastMap<Key, VariableStatus>;
    │ │ │ │ -
    151
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ -
    Parameters for iSAM 2.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    37
    │ │ │ │ +
    38#include <algorithm>
    │ │ │ │ +
    39#include <limits>
    │ │ │ │ +
    40#include <string>
    │ │ │ │ +
    41#include <utility>
    │ │ │ │ +
    42
    │ │ │ │ +
    43namespace gtsam {
    │ │ │ │ +
    44
    │ │ │ │ +
    45/* ************************************************************************* */
    │ │ │ │ +
    46// Special BayesTree class that uses ISAM2 cliques - this is the result of
    │ │ │ │ +
    47// reeliminating ISAM2 subtrees.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    49 public:
    │ │ │ │ +
    50 typedef ISAM2::Base Base;
    │ │ │ │ +
    51 typedef ISAM2BayesTree This;
    │ │ │ │ +
    52 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    53
    │ │ │ │ + │ │ │ │ +
    55};
    │ │ │ │ +
    │ │ │ │ +
    56
    │ │ │ │ +
    57/* ************************************************************************* */
    │ │ │ │ +
    58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
    │ │ │ │ +
    59// reeliminating ISAM2 subtrees.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    61 : public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
    │ │ │ │ +
    62 public:
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ +
    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());
    │ │ │ │ +
    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.");
    │ │ │ │ +
    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 }
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Global debugging flags.
    │ │ │ │ +
    The junction tree, template bodies.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    Class that stores detailed iSAM2 result.
    │ │ │ │ +
    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ │ │ │ │ +
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ +
    void remove(size_t i)
    delete factor without re-arranging indexes by inserting a nullptr pointer
    Definition FactorGraph.h:385
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ +
    FastVector< sharedClique > Roots
    Root cliques.
    Definition BayesTree.h:95
    │ │ │ │ +
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ +
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ +
    unsigned char chr() const
    Retrieve key character.
    Definition Symbol.h:73
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ +
    void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
    Remove entries corresponding to the specified factors.
    Definition VariableIndex-inl.h:54
    │ │ │ │ +
    bool empty(Key variable) const
    Return true if no factors associated with a variable.
    Definition VariableIndex.h:98
    │ │ │ │ +
    Definition GaussianEliminationTree.h:29
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Definition ISAM2-impl.h:48
    │ │ │ │ +
    Definition ISAM2-impl.h:61
    │ │ │ │ +
    Definition ISAM2-impl.h:72
    │ │ │ │ +
    Definition ISAM2-impl.h:73
    │ │ │ │ +
    Definition ISAM2-impl.h:77
    │ │ │ │ +
    Implementation functions for update method All of the methods below have clear inputs and outputs,...
    Definition ISAM2-impl.h:114
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
    Definition ISAM2.h:45
    │ │ │ │ +
    Base::sharedClique sharedClique
    Shared pointer to a clique.
    Definition ISAM2.h:103
    │ │ │ │ +
    Definition ISAM2Params.h:135
    │ │ │ │ +
    boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
    Either a constant relinearization threshold or a per-variable-type set of thresholds.
    Definition ISAM2Params.h:140
    │ │ │ │
    This struct is returned from ISAM2::update() and contains information about the update that is useful...
    Definition ISAM2Result.h:41
    │ │ │ │ -
    size_t variablesRelinearized
    The number of variables that were relinearized because their linear deltas exceeded the reslinearizat...
    Definition ISAM2Result.h:76
    │ │ │ │ -
    size_t getVariablesRelinearized() const
    Getters and Setters.
    Definition ISAM2Result.h:176
    │ │ │ │ -
    size_t factorsRecalculated
    The number of factors that were included in reelimination of the Bayes' tree.
    Definition ISAM2Result.h:88
    │ │ │ │ -
    DetailedResults * details()
    Return pointer to detail, 0 if no detail requested.
    Definition ISAM2Result.h:165
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string str="") const
    Print results.
    Definition ISAM2Result.h:168
    │ │ │ │ -
    boost::optional< DetailedResults > detail
    Detailed results, if enabled by ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:158
    │ │ │ │ -
    size_t cliques
    The number of cliques in the Bayes' Tree.
    Definition ISAM2Result.h:91
    │ │ │ │ -
    KeySet keysWithRemovedFactors
    Keys of variables that had factors removed.
    Definition ISAM2Result.h:108
    │ │ │ │ -
    size_t variablesReeliminated
    The number of variables that were reeliminated as parts of the Bayes' Tree were recalculated,...
    Definition ISAM2Result.h:84
    │ │ │ │ -
    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
    │ │ │ │ -
    KeyVector observedKeys
    keys for variables that were observed, i.e., not unused.
    Definition ISAM2Result.h:105
    │ │ │ │ -
    KeySet markedKeys
    All keys that were marked during the update process.
    Definition ISAM2Result.h:111
    │ │ │ │ -
    FactorIndices newFactorsIndices
    The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
    Definition ISAM2Result.h:97
    │ │ │ │ -
    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
    │ │ │ │ -
    A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:117
    │ │ │ │ -
    StatusMap variableStatus
    The status of each variable during this update, see VariableStatus.
    Definition ISAM2Result.h:153
    │ │ │ │ -
    The status of a single variable, this struct is stored in DetailedResults::variableStatus.
    Definition ISAM2Result.h:120
    │ │ │ │ -
    bool isRelinearizeInvolved
    Whether the variable was below the relinearization threshold but was relinearized by being involved i...
    Definition ISAM2Result.h:128
    │ │ │ │ -
    bool isObserved
    Whether the variable was relinearized, either by being above the relinearization threshold or by invo...
    Definition ISAM2Result.h:136
    │ │ │ │ -
    bool isNew
    Whether the variable itself was just added.
    Definition ISAM2Result.h:138
    │ │ │ │ -
    bool isAboveRelinThreshold
    Whether the variable was just relinearized due to being above the relinearization threshold.
    Definition ISAM2Result.h:125
    │ │ │ │ -
    bool isReeliminated
    Whether the variable was just reeliminated, due to being relinearized, observed, new,...
    Definition ISAM2Result.h:124
    │ │ │ │ -
    bool inRootClique
    Whether the variable is in the root clique.
    Definition ISAM2Result.h:139
    │ │ │ │ +
    This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
    Definition ISAM2UpdateParams.h:32
    │ │ │ │ +
    bool force_relinearize
    Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
    Definition ISAM2UpdateParams.h:54
    │ │ │ │ +
    FactorIndices removeFactorIndices
    Indices of factors to remove from system (default: empty)
    Definition ISAM2UpdateParams.h:36
    │ │ │ │ +
    bool forceFullSolve
    By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
    Definition ISAM2UpdateParams.h:71
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::optional< FastList< Key > > noRelinKeys
    An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
    Definition ISAM2UpdateParams.h:44
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactorGraph.cpp:55
    │ │ │ │ +
    double error(const Values &values) const
    unnormalized error, in the most common case
    Definition NonlinearFactorGraph.cpp:170
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ + │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,248 +1,689 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Result.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 │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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); │ │ │ │ │ +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()); │ │ │ │ │ +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."); │ │ │ │ │ +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 } │ │ │ │ │ +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_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) │ │ │ │ │ +_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_:_:_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_:_:_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_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_:_:_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 │ │ │ │ │ +_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_R_e_s_u_l_t_._h │ │ │ │ │ + * _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/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/Expression-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,40 +95,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    NonlinearISAM.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Expression-inl.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Internals for Expression.h, not for general consumption. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::NonlinearISAM
     Wrapper class to manage ISAM in a nonlinear context. More...
    struct  gtsam::internal::apply_compose< T >
     
    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

    │ │ │ │ -
    Date
    Jan 19, 2010
    │ │ │ │ -
    Author
    Viorela Ila and Richard Roberts
    │ │ │ │ +

    Internals for Expression.h, not for general consumption.

    │ │ │ │ +
    Date
    September 18, 2014
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Paul Furgale
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -NonlinearISAM.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 │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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_ _> │ │ │ │ │   │ │ │ │ │ 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 │ │ │ │ │ - Jan 19, 2010 │ │ │ │ │ + September 18, 2014 │ │ │ │ │ Author │ │ │ │ │ - Viorela Ila and Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Paul Furgale │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01118.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a01118 = [ │ │ │ │ │ - ["gtsam::NonlinearISAM", "a04548.html", "a04548"] │ │ │ │ │ + ["gtsam::internal::apply_compose< T >", "a04272.html", null], │ │ │ │ │ + ["gtsam::internal::apply_compose< double >", "a04276.html", null], │ │ │ │ │ + ["createUnknowns", "a01118.html#a51881c20dd5ecdd129cb993ea8374846", null], │ │ │ │ │ + ["operator*", "a01118.html#a3db519caa1b7e43412c9a7e13acf8329", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01118_source.html │ │ │ │ @@ -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/nonlinear/Expression-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,118 +98,419 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearISAM.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    │ │ │ │ -
    27class GTSAM_EXPORT NonlinearISAM {
    │ │ │ │ -
    28protected:
    │ │ │ │ -
    29
    │ │ │ │ - │ │ │ │ -
    32
    │ │ │ │ - │ │ │ │ +
    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/bind/bind.hpp>
    │ │ │ │ +
    32#include <boost/tuple/tuple.hpp>
    │ │ │ │ +
    33#include <boost/range/adaptor/map.hpp>
    │ │ │ │ +
    34#include <boost/range/algorithm.hpp>
    │ │ │ │
    35
    │ │ │ │ - │ │ │ │ -
    38
    │ │ │ │ - │ │ │ │ -
    41 int reorderCounter_;
    │ │ │ │ +
    36namespace gtsam {
    │ │ │ │ +
    37
    │ │ │ │ +
    38template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    40 root_(new internal::ConstantExpression<T>(value)) {
    │ │ │ │ +
    41}
    │ │ │ │ +
    │ │ │ │
    42
    │ │ │ │ - │ │ │ │ -
    45
    │ │ │ │ -
    46public:
    │ │ │ │ +
    43template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    45 root_(new internal::LeafExpression<T>(key)) {
    │ │ │ │ +
    46}
    │ │ │ │ +
    │ │ │ │
    47
    │ │ │ │ -
    50
    │ │ │ │ -
    │ │ │ │ -
    58 NonlinearISAM(int reorderInterval = 1,
    │ │ │ │ -
    59 const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
    │ │ │ │ -
    60 reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
    │ │ │ │ +
    48template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │
    │ │ │ │ -
    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_; }
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    88template<typename T>
    │ │ │ │ +
    89template<typename A>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    146template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    168 return root_->traceSize();
    │ │ │ │ +
    169}
    │ │ │ │ +
    │ │ │ │ +
    170
    │ │ │ │ +
    │ │ │ │ +
    171// Private methods:
    │ │ │ │ +
    172
    │ │ │ │ +
    173template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    175 const KeyVector& keys, const FastVector<int>& dims,
    │ │ │ │ +
    176 std::vector<Matrix>& H) const {
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    194 return result;
    │ │ │ │ +
    195}
    │ │ │ │ +
    196
    │ │ │ │ +
    │ │ │ │ +
    197template<typename T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    227 T value(this->traceExecution(values, trace, traceStorage));
    │ │ │ │ +
    228 trace.startReverseAD1(jacobians);
    │ │ │ │ +
    229
    │ │ │ │ +
    230 return value;
    │ │ │ │ +
    231}
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    232
    │ │ │ │ +
    233template<typename T>
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
    │ │ │ │ +
    304 return *this;
    │ │ │ │ +
    305}
    │ │ │ │ +
    │ │ │ │ +
    306
    │ │ │ │ +
    307} // namespace gtsam
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    Key symbol(unsigned char c, std::uint64_t j)
    Create a symbol key from a character and index, i.e.
    Definition Symbol.h:135
    │ │ │ │ +
    std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
    Construct an array of leaves.
    Definition Expression-inl.h:284
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition GaussianISAM.h:28
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    Wrapper class to manage ISAM in a nonlinear context.
    Definition NonlinearISAM.h:27
    │ │ │ │ -
    GaussianFactorGraph::Eliminate eliminationFunction_
    The elimination function.
    Definition NonlinearISAM.h:44
    │ │ │ │ -
    NonlinearFactorGraph factors_
    The original factors, used when relinearizing.
    Definition NonlinearISAM.h:37
    │ │ │ │ -
    const GaussianISAM & bayesTree() const
    access the underlying bayes tree
    Definition NonlinearISAM.h:75
    │ │ │ │ -
    int reorderInterval() const
    get counters
    Definition NonlinearISAM.h:84
    │ │ │ │ -
    NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate &eliminationFunction=GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
    Periodically reorder and relinearize.
    Definition NonlinearISAM.h:58
    │ │ │ │ -
    int reorderCounter() const
    TODO: comment.
    Definition NonlinearISAM.h:85
    │ │ │ │ -
    const NonlinearFactorGraph & getFactorsUnsafe() const
    get underlying nonlinear graph
    Definition NonlinearISAM.h:81
    │ │ │ │ -
    int reorderInterval_
    The reordering interval and counter.
    Definition NonlinearISAM.h:40
    │ │ │ │ -
    Values linPoint_
    The current linearization point.
    Definition NonlinearISAM.h:34
    │ │ │ │ -
    gtsam::GaussianISAM isam_
    The internal iSAM object.
    Definition NonlinearISAM.h:31
    │ │ │ │ -
    const Values & getLinearizationPoint() const
    Return the current linearization point.
    Definition NonlinearISAM.h:78
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    : meta-function to generate JacobianTA optional reference Used mainly by Expressions
    Definition OptionalJacobian.h:261
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ +
    Definition Expression-inl.h:247
    │ │ │ │ +
    Definition Expression.h:40
    │ │ │ │ +
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ +
    Expression()
    Default constructor, for serialization.
    Definition Expression.h:182
    │ │ │ │ +
    std::set< Key > keys() const
    Return keys that play in this expression.
    Definition Expression-inl.h:132
    │ │ │ │ +
    std::pair< KeyVector, FastVector< int > > KeysAndDims
    Keys and dimensions in same order.
    Definition Expression.h:185
    │ │ │ │ +
    void dims(std::map< Key, int > &map) const
    Return dimensions for each argument, as a map.
    Definition Expression-inl.h:137
    │ │ │ │ +
    void print(const std::string &s) const
    Print.
    Definition Expression-inl.h:142
    │ │ │ │ +
    size_t traceSize() const
    Return size needed for memory buffer in traceExecution.
    Definition Expression-inl.h:167
    │ │ │ │ +
    T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
    trace execution, very unsafe
    Definition Expression-inl.h:198
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,166 +1,408 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearISAM.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 │ │ │ │ │ -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__; │ │ │ │ │ +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 │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ 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_; │ │ │ │ │ +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 │ │ │ │ │ -_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: │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_I_S_A_M_._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/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/DoglegOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,39 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -
    LevenbergMarquardtParams.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    DoglegOptimizer.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::LevenbergMarquardtParams
     Parameters for Levenberg-Marquardt optimization. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    │ │ │ │ +typedef internal::DoglegState gtsam::State
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Parameters for Levenberg-Marquardt trust-region scheme.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ Date │ │ │ │ │ Feb 26, 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_P_a_r_a_m_s_._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/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/ExtendedKalmanFilter-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    DoglegOptimizerImpl.h File Reference
    │ │ │ │ +
    ExtendedKalmanFilter-inl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation) │ │ │ │ +

    Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ 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

    │ │ │ │ -

    Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)

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

    Class to perform generic Kalman Filtering using nonlinear factor graphs.

    │ │ │ │ +
    Author
    Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -DoglegOptimizerImpl.h File Reference │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) _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 │ │ │ │ │ -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 ********** │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Chris Beall │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01124_source.html │ │ │ │ @@ -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/nonlinear/ExtendedKalmanFilter-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,203 +98,164 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DoglegOptimizerImpl.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
    │ │ │ │ -
    17#pragma once
    │ │ │ │ -
    18
    │ │ │ │ -
    19#include <iomanip>
    │ │ │ │ +
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ +
    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,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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();
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    Class to perform generic Kalman Filtering using nonlinear factor graphs.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    static VectorValues Zero(const VectorValues &other)
    Create a VectorValues with the same structure as other, but filled with zeros.
    Definition VectorValues.cpp:78
    │ │ │ │ -
    This class contains the implementation of the Dogleg algorithm.
    Definition DoglegOptimizerImpl.h:32
    │ │ │ │ -
    TrustRegionAdaptationMode
    Specifies how the trust region is adapted at each Dogleg iteration.
    Definition DoglegOptimizerImpl.h:53
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition DoglegOptimizerImpl.h:34
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ +
    This is a generic Extended Kalman Filter class implemented using nonlinear factors.
    Definition ExtendedKalmanFilter.h:45
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
    Linearize a non-linearFactorN to get a GaussianFactor, Hence .
    Definition NonlinearFactor.cpp:152
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,205 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DoglegOptimizerImpl.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 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include │ │ │ │ │ +19#pragma once │ │ │ │ │ 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 { │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -_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/* ************************************************************************* │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ +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(); │ │ │ │ │ +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 │ │ │ │ │ +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_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._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/a01127.html │ │ │ │ @@ -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/nonlinear/CustomFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,55 +97,50 @@ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs
    │ │ │ │ -
    NonlinearOptimizerParams.h File Reference
    │ │ │ │ +
    CustomFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Parameters for nonlinear optimization. │ │ │ │ +

    Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::NonlinearOptimizerParams
     The common parameters for Nonlinear optimizers. More...
    class  gtsam::CustomFactor
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ Typedefs

    │ │ │ │ -typedef NonlinearOptimizerParams gtsam::SuccessiveLinearizationParams
     
    │ │ │ │ +using gtsam::JacobianVector = std::vector< Matrix >
     
    │ │ │ │ +using gtsam::CustomErrorFunction = std::function< Vector(const CustomFactor &, const Values &, const JacobianVector *)>
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Parameters for nonlinear optimization.

    │ │ │ │ -
    Author
    Yong-Dian Jian
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Andrew Melim
    │ │ │ │ -
    Date
    Apr 1, 2012
    │ │ │ │ +

    Class to enable arbitrary factors with runtime swappable error function.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -NonlinearOptimizerParams.h File Reference │ │ │ │ │ -Parameters for nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Andrew Melim │ │ │ │ │ - Date │ │ │ │ │ - Apr 1, 2012 │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01127.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01127 = [ │ │ │ │ │ - ["gtsam::NonlinearOptimizerParams", "a04556.html", "a04556"] │ │ │ │ │ + ["gtsam::CustomFactor", "a04248.html", "a04248"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01127_source.html │ │ │ │ @@ -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/CustomFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,196 +98,116 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearOptimizerParams.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    21
    │ │ │ │ +
    22namespace gtsam {
    │ │ │ │
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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); }
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46protected:
    │ │ │ │ +
    47 CustomErrorFunction error_function_;
    │ │ │ │ +
    48
    │ │ │ │ +
    49protected:
    │ │ │ │ +
    50
    │ │ │ │ +
    51 using Base = NoiseModelFactor;
    │ │ │ │ +
    52 using This = CustomFactor;
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    95
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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 */
    │ │ │ │ -
    Subgraph Solver from IROS 2010.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    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
    │ │ │ │ +
    54public:
    │ │ │ │ +
    55
    │ │ │ │ +
    59 CustomFactor() = default;
    │ │ │ │ +
    60
    │ │ │ │ +
    │ │ │ │ +
    67 CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    OrderingType
    Type of ordering to use.
    Definition Ordering.h:41
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ -
    IterationHook iterationHook
    Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
    Definition NonlinearOptimizerParams.h:94
    │ │ │ │ -
    IterativeOptimizationParameters::shared_ptr iterativeParams
    The container for iterativeOptimization parameters. used in CG Solvers.
    Definition NonlinearOptimizerParams.h:108
    │ │ │ │ -
    Verbosity
    See NonlinearOptimizerParams::verbosity.
    Definition NonlinearOptimizerParams.h:37
    │ │ │ │ -
    LinearSolverType
    See NonlinearOptimizerParams::linearSolverType.
    Definition NonlinearOptimizerParams.h:97
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< Ordering > ordering
    The optional variable elimination ordering, or empty to use COLAMD (default: empty)
    Definition NonlinearOptimizerParams.h:107
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    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
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    Definition CustomFactor.h:45
    │ │ │ │ +
    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
    │ │ │ │ +
    CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction)
    Constructor.
    Definition CustomFactor.h:67
    │ │ │ │ +
    CustomFactor()=default
    Default Constructor for I/O.
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition CustomFactor.h:94
    │ │ │ │ +
    bool sendable() const override
    Mark not sendable.
    Definition CustomFactor.h:87
    │ │ │ │ +
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition CustomFactor.cpp:59
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    NoiseModelFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:189
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,233 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearOptimizerParams.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 │ │ │ │ │ -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 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ 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); } │ │ │ │ │ +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; │ │ │ │ │ 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; │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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... │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,31 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    Marginals.cpp File Reference
    │ │ │ │ +
    GaussNewtonOptimizer.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    May 14, 2012
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Marginals.cpp File Reference │ │ │ │ │ +GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ 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_._c_p_p │ │ │ │ │ + * _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/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/LevenbergMarquardtOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM2-impl.h File Reference
    │ │ │ │ +
    LevenbergMarquardtOptimizer.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ +

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

    │ │ │ │ -
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │ +

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_-_i_m_p_l_._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_._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/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/CustomFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ISAM2.h File Reference
    │ │ │ │ +
    CustomFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ +

    Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ 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
    │ │ │ │ +

    Class to enable arbitrary factors with runtime swappable error function.

    │ │ │ │ +
    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_._h │ │ │ │ │ + * _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/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/NonlinearEquality.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,67 +98,402 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ExpressionFactorGraph.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ -
    21#include <gtsam/nonlinear/ExpressionFactor.h>
    │ │ │ │ - │ │ │ │ +
    12/*
    │ │ │ │ +
    13 * @file NonlinearEquality.h
    │ │ │ │ +
    14 * @brief Factor to handle enforced equality between factors
    │ │ │ │ +
    15 * @author Alex Cunningham
    │ │ │ │ +
    16 */
    │ │ │ │ +
    17
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    21#include <gtsam/base/Testable.h>
    │ │ │ │ +
    22#include <gtsam/base/Manifold.h>
    │ │ │ │
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ +
    24#include <boost/bind/bind.hpp>
    │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    26#include <limits>
    │ │ │ │ +
    27#include <iostream>
    │ │ │ │ +
    28#include <cmath>
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    44template<class 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
    │ │ │ │ + │ │ │ │ +
    66
    │ │ │ │ +
    67public:
    │ │ │ │ +
    68
    │ │ │ │ +
    70 using CompareFunction = std::function<bool(const T&, const T&)>;
    │ │ │ │ +
    71 CompareFunction compare_;
    │ │ │ │ +
    72
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    75 }
    │ │ │ │
    │ │ │ │ -
    48
    │ │ │ │ -
    50};
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    51
    │ │ │ │ -
    52}
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    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());
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    180
    │ │ │ │ +
    181private:
    │ │ │ │ +
    182
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    208
    │ │ │ │ +
    209public:
    │ │ │ │ +
    210 typedef VALUE X;
    │ │ │ │ +
    211
    │ │ │ │ +
    212protected:
    │ │ │ │ + │ │ │ │ +
    214 typedef NonlinearEquality1<VALUE> This;
    │ │ │ │ +
    215
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    269
    │ │ │ │ +
    270private:
    │ │ │ │ +
    271
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    287 : Testable<NonlinearEquality1<VALUE> > {};
    │ │ │ │ +
    │ │ │ │ +
    288
    │ │ │ │ +
    289/* ************************************************************************* */
    │ │ │ │ +
    294template <class T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    296 protected:
    │ │ │ │ + │ │ │ │ +
    298 using This = NonlinearEquality2<T>;
    │ │ │ │ +
    299
    │ │ │ │ +
    300 GTSAM_CONCEPT_MANIFOLD_TYPE(T)
    │ │ │ │ +
    301
    │ │ │ │ +
    302
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    336
    │ │ │ │ +
    337 private:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ -
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │ -
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ -
    Factor graph that supports adding ExpressionFactors directly.
    Definition ExpressionFactorGraph.h:29
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    static shared_ptr All(size_t dim)
    Fully constrained variations.
    Definition NoiseModel.h:457
    │ │ │ │ +
    An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
    Definition NonlinearEquality.h:45
    │ │ │ │ +
    double error(const Values &c) const override
    Actual error function calculation.
    Definition NonlinearEquality.h:130
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition NonlinearEquality.h:109
    │ │ │ │ +
    std::function< bool(const T &, const T &)> CompareFunction
    Function that compares two values.
    Definition NonlinearEquality.h:70
    │ │ │ │ +
    NonlinearEquality()
    Default constructor - only for serialization.
    Definition NonlinearEquality.h:74
    │ │ │ │ +
    bool equals(const NonlinearFactor &f, double tol=1e-9) const override
    Check if two factors are equal.
    Definition NonlinearEquality.h:119
    │ │ │ │ +
    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::NonlinearFactor::shared_ptr clone() const override
    Definition NonlinearEquality.h:172
    │ │ │ │ +
    Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
    Error function.
    Definition NonlinearEquality.h:141
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition NonlinearEquality.h:184
    │ │ │ │ +
    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
    │ │ │ │ +
    GaussianFactor::shared_ptr linearize(const Values &x) const override
    Linearize is over-written, because base linearization tries to whiten.
    Definition NonlinearEquality.h:162
    │ │ │ │ +
    Simple unary equality constraint - fixes a value for a variable.
    Definition NonlinearEquality.h:207
    │ │ │ │ +
    Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
    g(x) with optional derivative
    Definition NonlinearEquality.h:251
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition NonlinearEquality.h:245
    │ │ │ │ +
    boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
    fixed value for variable
    Definition NonlinearEquality.h:227
    │ │ │ │ +
    NonlinearEquality1(const X &value, Key key, double mu=1000.0)
    Constructor.
    Definition NonlinearEquality.h:235
    │ │ │ │ +
    NonlinearEquality1()
    Default constructor to allow for serialization.
    Definition NonlinearEquality.h:217
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition NonlinearEquality.h:260
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition NonlinearEquality.h:273
    │ │ │ │ +
    Simple binary equality constraint - this constraint forces two variables to be the same.
    Definition NonlinearEquality.h:295
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition NonlinearEquality.h:320
    │ │ │ │ +
    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
    │ │ │ │ +
    NonlinearEquality2(Key key1, Key key2, double mu=1e4)
    Constructor.
    Definition NonlinearEquality.h:314
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition NonlinearEquality.h:339
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    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
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,76 +1,494 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ +24#include │ │ │ │ │ 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}; │ │ │ │ │ +26#include │ │ │ │ │ +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; │ │ │ │ │ +49 │ │ │ │ │ +50private: │ │ │ │ │ 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. │ │ │ │ │ +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. │ │ │ │ │ +_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_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold 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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_._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/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/ISAM2Clique.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,54 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    CustomFactor.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    ISAM2Clique.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ +

    Specialized iSAM2 Clique. │ │ │ │ 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

    │ │ │ │ -

    Class to enable arbitrary factors with runtime swappable error function.

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

    Specialized iSAM2 Clique.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,34 @@ │ │ │ │ │ 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. │ │ │ │ │ -_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_:_:_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 ********** │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Michael Kaess, Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/LevenbergMarquardtParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,61 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    FunctorizedFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    LevenbergMarquardtParams.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ +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...
    class  gtsam::LevenbergMarquardtParams
     Parameters for Levenberg-Marquardt optimization. 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

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

    Parameters for Levenberg-Marquardt trust-region scheme.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │ +
    Date
    Feb 26, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,51 +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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -FunctorizedFactor.h File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_o_ _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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ - Date │ │ │ │ │ - May 31, 2020 │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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] │ │ │ │ │ + ["gtsam::LevenbergMarquardtParams", "a04440.html", "a04440"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01157_source.html │ │ │ │ @@ -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/nonlinear/LevenbergMarquardtParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,250 +98,188 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    FunctorizedFactor.h
    │ │ │ │ +
    LevenbergMarquardtParams.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#pragma once
    │ │ │ │
    22
    │ │ │ │ -
    23#include <cmath>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    58template <typename R, typename T>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    60 private:
    │ │ │ │ - │ │ │ │ -
    62
    │ │ │ │ -
    63 R measured_;
    │ │ │ │ -
    64 SharedNoiseModel noiseModel_;
    │ │ │ │ -
    65 std::function<R(T, boost::optional<Matrix &>)> func_;
    │ │ │ │ -
    66
    │ │ │ │ -
    67 public:
    │ │ │ │ - │ │ │ │ -
    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 {}
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    28class LevenbergMarquardtOptimizer;
    │ │ │ │ +
    29
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    36
    │ │ │ │ +
    37public:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 double lambdaFactor;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    55 std::string logFile;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    58 double minDiagonal;
    │ │ │ │ +
    59 double maxDiagonal;
    │ │ │ │ +
    60
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    134 : public Testable<FunctorizedFactor<R, T>> {};
    │ │ │ │ -
    │ │ │ │ -
    135
    │ │ │ │ -
    142template <typename T, typename R, typename FUNC>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    169 public:
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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 }
    │ │ │ │ +
    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}
    │ │ │ │ +
    Parameters for nonlinear optimization.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
    Definition FunctorizedFactor.h:59
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equals(const NonlinearFactor &other, double tol=1e-9) const override
    Check if two factors are equal.
    Definition FunctorizedFactor.h:110
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition FunctorizedFactor.h:99
    │ │ │ │ -
    FunctorizedFactor()
    default constructor - only use for serialization
    Definition FunctorizedFactor.h:69
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition FunctorizedFactor.h:120
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition FunctorizedFactor.h:85
    │ │ │ │ -
    Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
    Definition FunctorizedFactor.h:159
    │ │ │ │ -
    FunctorizedFactor2()
    default constructor - only use for serialization
    Definition FunctorizedFactor.h:171
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition FunctorizedFactor.h:190
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition FunctorizedFactor.h:206
    │ │ │ │ -
    bool equals(const NonlinearFactor &other, double tol=1e-9) const override
    Check if two factors are equal.
    Definition FunctorizedFactor.h:218
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition FunctorizedFactor.h:228
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    This class performs Levenberg-Marquardt nonlinear optimization.
    Definition LevenbergMarquardtOptimizer.h:35
    │ │ │ │ +
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ +
    double lambdaFactor
    The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
    Definition LevenbergMarquardtParams.h:50
    │ │ │ │ +
    double minDiagonal
    when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
    Definition LevenbergMarquardtParams.h:58
    │ │ │ │ +
    double lambdaUpperBound
    The maximum lambda to try before assuming the optimization has failed (default: 1e5)
    Definition LevenbergMarquardtParams.h:51
    │ │ │ │ +
    double lambdaInitial
    The initial Levenberg-Marquardt damping term (default: 1e-5)
    Definition LevenbergMarquardtParams.h:49
    │ │ │ │ +
    double maxDiagonal
    when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
    Definition LevenbergMarquardtParams.h:59
    │ │ │ │ +
    double minModelFidelity
    Lower bound for the modelFidelity to accept the result of an LM iteration.
    Definition LevenbergMarquardtParams.h:54
    │ │ │ │ +
    double lambdaLowerBound
    The minimum lambda used in LM (default: 0)
    Definition LevenbergMarquardtParams.h:52
    │ │ │ │ +
    boost::shared_ptr< NonlinearOptimizerParams > clone() const
    Definition LevenbergMarquardtParams.h:152
    │ │ │ │ +
    bool diagonalDamping
    if true, use diagonal of Hessian
    Definition LevenbergMarquardtParams.h:56
    │ │ │ │ +
    bool useFixedLambdaFactor
    if true applies constant increase (or decrease) to lambda according to lambdaFactor
    Definition LevenbergMarquardtParams.h:57
    │ │ │ │ +
    std::string logFile
    an optional CSV log file, with [iteration, time, error, lambda]
    Definition LevenbergMarquardtParams.h:55
    │ │ │ │ +
    VerbosityLM
    See LevenbergMarquardtParams::verbosityLM.
    Definition LevenbergMarquardtParams.h:39
    │ │ │ │ +
    VerbosityLM verbosityLM
    The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
    Definition LevenbergMarquardtParams.h:53
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    double absoluteErrorTol
    The maximum absolute error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:43
    │ │ │ │ +
    size_t maxIterations
    The maximum iterations to stop iterating (default 100)
    Definition NonlinearOptimizerParams.h:41
    │ │ │ │ +
    double relativeErrorTol
    The maximum relative error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:42
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,306 +1,247 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FunctorizedFactor.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 │ │ │ │ │ -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> │ │ │ │ │ +21#pragma once │ │ │ │ │ 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 │ │ │ │ │ -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 } │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ { │ │ │ │ │ -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. │ │ │ │ │ +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 } │ │ │ │ │ +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_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_:_:_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 │ │ │ │ │ -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_:_:_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_:_:_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/ISAM2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ExtendedKalmanFilter-inl.h File Reference
    │ │ │ │ +
    ISAM2.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ +

    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

    │ │ │ │ -

    Class to perform generic Kalman Filtering using nonlinear factor graphs.

    │ │ │ │ -
    Author
    Stephen Williams
    │ │ │ │ -
    │ │ │ │ -Chris Beall
    │ │ │ │ +

    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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. │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Chris Beall │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/ISAM2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,164 +98,254 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ExtendedKalmanFilter-inl.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    60 return x;
    │ │ │ │ -
    61 }
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    29
    │ │ │ │ +
    30#include <vector>
    │ │ │ │ +
    31
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ +
    45class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    │ │ │ │ +
    46 protected:
    │ │ │ │ + │ │ │ │ +
    49
    │ │ │ │ + │ │ │ │ +
    53
    │ │ │ │ + │ │ │ │
    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();
    │ │ │ │ -
    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));
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    82
    │ │ │ │ + │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    88
    │ │ │ │ +
    90 mutable boost::optional<double> doglegDelta_;
    │ │ │ │ +
    91
    │ │ │ │ + │ │ │ │
    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();
    │ │ │ │ + │ │ │ │ +
    98
    │ │ │ │ +
    99 public:
    │ │ │ │ +
    100 using This = ISAM2;
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    104 using Cliques = Base::Cliques;
    │ │ │ │ +
    105
    │ │ │ │ +
    107 explicit ISAM2(const ISAM2Params& params);
    │ │ │ │
    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
    │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    Class to perform generic Kalman Filtering using nonlinear factor graphs.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    Parameters for iSAM 2.
    │ │ │ │ +
    Class that stores extra params for ISAM2::update()
    │ │ │ │ +
    Class that stores detailed iSAM2 result.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    Specialized iSAM2 Clique.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ -
    This is a generic Extended Kalman Filter class implemented using nonlinear factors.
    Definition ExtendedKalmanFilter.h:45
    │ │ │ │ -
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
    Linearize a non-linearFactorN to get a GaussianFactor, Hence .
    Definition NonlinearFactor.cpp:152
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
    Definition ISAM2.h:45
    │ │ │ │ +
    KeySet fixedVariables_
    Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
    Definition ISAM2.h:94
    │ │ │ │ +
    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
    │ │ │ │ +
    int update_count_
    Counter incremented every update(), used to determine periodic relinearization.
    Definition ISAM2.h:96
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual ~ISAM2()
    default virtual destructor
    Definition ISAM2.h:114
    │ │ │ │ +
    const KeySet & getFixedVariables() const
    Access the nonlinear variable index.
    Definition ISAM2.h:265
    │ │ │ │ +
    Base::Cliques Cliques
    List of Cliques.
    Definition ISAM2.h:104
    │ │ │ │ +
    void printStats() const
    prints out clique statistics
    Definition ISAM2.h:270
    │ │ │ │ +
    NonlinearFactorGraph nonlinearFactors_
    All original nonlinear factors are stored here to use during relinearization.
    Definition ISAM2.h:81
    │ │ │ │ +
    const VariableIndex & getVariableIndex() const
    Access the nonlinear variable index.
    Definition ISAM2.h:262
    │ │ │ │ +
    Base::sharedClique sharedClique
    Shared pointer to a clique.
    Definition ISAM2.h:103
    │ │ │ │ +
    VectorValues delta_
    The linear delta from the last linear solution, an update to the estimate in theta.
    Definition ISAM2.h:61
    │ │ │ │ +
    VariableIndex variableIndex_
    VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
    Definition ISAM2.h:52
    │ │ │ │ +
    const Values & getLinearizationPoint() const
    Access the current linearization point.
    Definition ISAM2.h:205
    │ │ │ │ +
    Values theta_
    The current linearization point.
    Definition ISAM2.h:48
    │ │ │ │ +
    ISAM2Params params_
    The current parameters.
    Definition ISAM2.h:87
    │ │ │ │ +
    boost::optional< double > doglegDelta_
    The current Dogleg Delta (trust region radius)
    Definition ISAM2.h:90
    │ │ │ │ +
    GaussianFactorGraph linearFactors_
    The current linear factors, which are only updated as needed.
    Definition ISAM2.h:84
    │ │ │ │ +
    const NonlinearFactorGraph & getFactorsUnsafe() const
    Access the set of nonlinear factors.
    Definition ISAM2.h:257
    │ │ │ │ +
    bool valueExists(Key key) const
    Check whether variable with given key exists in linearization point.
    Definition ISAM2.h:208
    │ │ │ │ +
    Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
    Definition ISAM2Clique.h:37
    │ │ │ │ +
    Definition ISAM2Params.h:135
    │ │ │ │ +
    This struct is returned from ISAM2::update() and contains information about the update that is useful...
    Definition ISAM2Result.h:41
    │ │ │ │ +
    A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:117
    │ │ │ │ +
    This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
    Definition ISAM2UpdateParams.h:32
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,205 +1,373 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExtendedKalmanFilter-inl.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 <_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()); │ │ │ │ │ -59 │ │ │ │ │ -60 return x; │ │ │ │ │ -61 } │ │ │ │ │ +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 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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(); │ │ │ │ │ -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)); │ │ │ │ │ +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 │ │ │ │ │ -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(); │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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_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_R_e_s_u_l_t_._h │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ +_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_C_l_i_q_u_e_._h │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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_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_:_:_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_:_:_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_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._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/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/NonlinearFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GncOptimizer.h File Reference
    │ │ │ │ +
    NonlinearFactorGraph.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    The GncOptimizer class. │ │ │ │ +

    Factor Graph Consisting of non-linear factors. │ │ │ │ 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

    │ │ │ │ -

    The GncOptimizer class.

    │ │ │ │ -
    Author
    Jingnan Shi
    │ │ │ │ +

    Factor Graph Consisting of non-linear factors.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Luca Carlone
    │ │ │ │ +Carlos Nieto │ │ │ │
    │ │ │ │ -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.

    │ │ │ │ +Christian Potthast
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -The GncOptimizer class. │ │ │ │ │ +Factor Graph Consisting of non-linear factors. │ │ │ │ │ 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: _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. │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/expressionTesting.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,79 +94,107 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros
    │ │ │ │ -
    NonlinearFactor.h File Reference
    │ │ │ │ +Macros | │ │ │ │ +Functions
    │ │ │ │ +
    expressionTesting.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Non-linear factor base classes. │ │ │ │ +

    Test harness methods for expressions. │ │ │ │ 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
     
    #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

    │ │ │ │ -

    Non-linear factor base classes.

    │ │ │ │ +

    Test harness methods for expressions.

    │ │ │ │ +
    Date
    September 18, 2014
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Gerry Chen
    │ │ │ │ -
    │ │ │ │ +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,53 +1,58 @@ │ │ │ │ │ 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 | _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_. │ │ │ │ │ -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 │ │ │ │ │ +#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 ********** │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +Test harness methods for expressions. │ │ │ │ │ + Date │ │ │ │ │ + September 18, 2014 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Gerry Chen │ │ │ │ │ + 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_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_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/a01169.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a01169 = [ │ │ │ │ │ - ["gtsam::NonlinearFactor", "a04496.html", "a04496"], │ │ │ │ │ - ["gtsam::traits< NonlinearFactor >", "a04500.html", null], │ │ │ │ │ - ["gtsam::NoiseModelFactor", "a04504.html", "a04504"], │ │ │ │ │ - ["gtsam::NoiseModelFactorN< ValueTypes >", "a04536.html", "a04536"] │ │ │ │ │ + ["EXPECT_CORRECT_EXPRESSION_JACOBIANS", "a01169.html#a701d0cd12b81a725f7f9cd2432fe9e2a", null] │ │ │ │ │ ]; │ │ │ ├── ./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/expressionTesting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,469 +98,66 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearFactor.h
    │ │ │ │ +
    expressionTesting.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
    │ │ │ │ +
    20#pragma once
    │ │ │ │
    21
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    59
    │ │ │ │ -
    63 template<typename CONTAINER>
    │ │ │ │ -
    │ │ │ │ -
    64 NonlinearFactor(const CONTAINER& keys) :
    │ │ │ │ -
    65 Base(keys) {}
    │ │ │ │ +
    22#include <gtsam/nonlinear/ExpressionFactor.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)); }
    │ │ │ │
    │ │ │ │ -
    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;
    │ │ │ │ -
    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; }
    │ │ │ │ -
    119
    │ │ │ │ -
    121 virtual boost::shared_ptr<GaussianFactor>
    │ │ │ │ -
    122 linearize(const Values& c) const = 0;
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    470
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    Special class for optional Jacobian arguments.
    │ │ │ │ -
    The base class for all factors.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Evaluate derivatives of a nonlinear factor numerically.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
    linearize to a GaussianFactor
    │ │ │ │ -
    virtual size_t dim() const =0
    get the dimension of the factor (number of rows on linearization)
    │ │ │ │ -
    NonlinearFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:58
    │ │ │ │ -
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ -
    NonlinearFactor(const CONTAINER &keys)
    Constructor from a collection of the keys involved in this factor.
    Definition NonlinearFactor.h:64
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual ~NonlinearFactor()
    Destructor.
    Definition NonlinearFactor.h:83
    │ │ │ │ -
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ -
    NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
    Constructor.
    Definition NonlinearFactor.h:198
    │ │ │ │ -
    ~NoiseModelFactor() override
    Destructor.
    Definition NonlinearFactor.h:192
    │ │ │ │ -
    NoiseModelFactor(const SharedNoiseModel &noiseModel)
    Constructor - only for subclasses, as this does not set keys.
    Definition NonlinearFactor.h:206
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Noise model.
    Definition NonlinearFactor.h:186
    │ │ │ │ -
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition NonlinearFactor.h:218
    │ │ │ │ -
    NoiseModelFactor()
    Default constructor for I/O only.
    Definition NonlinearFactor.h:189
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ -
    virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
    Error function without the NoiseModel, .
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Vector evaluateError(const ValueTypes &... x) const
    No-Jacobians requested function overload.
    Definition NonlinearFactor.h:589
    │ │ │ │ -
    NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys)
    Constructor.
    Definition NonlinearFactor.h:491
    │ │ │ │ -
    NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys)
    Constructor.
    Definition NonlinearFactor.h:478
    │ │ │ │ -
    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
    │ │ │ │ -
    NoiseModelFactorN()
    Default Constructor for I/O.
    Definition NonlinearFactor.h:469
    │ │ │ │ -
    virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< ValueTypes >... H) const =0
    Override evaluateError to finish implementing an n-way factor.
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition NonlinearFactor.h:632
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const
    Some (but not all) optional Jacobians are omitted (function overload)
    Definition NonlinearFactor.h:599
    │ │ │ │ +
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ +
    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
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,574 +1,74 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearFactor.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 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20// \callgraph │ │ │ │ │ +20#pragma once │ │ │ │ │ 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/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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() {} │ │ │ │ │ -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; │ │ │ │ │ -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; } │ │ │ │ │ -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; │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/a01172.html │ │ │ │ @@ -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/nonlinear/ISAM2Clique.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    LinearContainerFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    ISAM2Clique.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph. │ │ │ │ +

    Specialized iSAM2 Clique. │ │ │ │ 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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +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

    │ │ │ │ -

    Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.

    │ │ │ │ -
    Date
    Jul 6, 2012
    │ │ │ │ -
    Author
    Alex Cunningham
    │ │ │ │ +

    Specialized iSAM2 Clique.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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_._._. │ │ │ │ │ -_G_o_ _t_o_ _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_ _> │ │ │ │ │ -  │ │ │ │ │ +_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 │ │ │ │ │ +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 ********** │ │ │ │ │ -Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ -nonlinear graph. │ │ │ │ │ - Date │ │ │ │ │ - Jul 6, 2012 │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01172.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01172 = [ │ │ │ │ │ - ["gtsam::LinearContainerFactor", "a04444.html", "a04444"], │ │ │ │ │ - ["gtsam::traits< LinearContainerFactor >", "a04448.html", null] │ │ │ │ │ + ["optimizeWildfire", "a01172.html#a2b0857edd76f8d63eeee0ce9944e28d4", null] │ │ │ │ │ ]; │ │ │ ├── ./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.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,95 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    TranslationFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    DsfTrackGenerator.cpp 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    │ │ │ │ +typedef DSFMap< IndexPairgtsam::gtsfm::DSFMapIndexPair
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Functions

    std::vector< SfmTrack2dgtsam::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
    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,28 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 factor for a relative translation direction measurement. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +Identifies connected components in the keypoint matches graph. │ │ │ │ │ Date │ │ │ │ │ - March 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 │ │ │ │ │ - * _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_._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/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/BinaryMeasurement.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,67 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    SfmData.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    BinaryMeasurement.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Data structure for dealing with Structure from Motion data. │ │ │ │ +

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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
    │ │ │ │ +

    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,55 +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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ - Frank dellaert │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ + Date │ │ │ │ │ + July 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_f_m_D_a_t_a_._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/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::BinaryMeasurement< T >", "a04688.html", "a04688"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01196_source.html │ │ │ │ @@ -114,18 +114,18 @@ │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ +
    21#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    22#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    23#include <gtsam/geometry/SOn.h>
    │ │ │ │ + │ │ │ │
    25
    │ │ │ │
    26#include <type_traits>
    │ │ │ │
    27
    │ │ │ │
    28namespace gtsam {
    │ │ │ │
    29
    │ │ │ │
    35template <size_t d>
    │ │ │ │
    │ │ │ │ @@ -165,18 +165,18 @@ │ │ │ │
    │ │ │ │
    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
    │ │ │ │ -
    N*N matrix representation of SO(N).
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    N*N matrix representation of SO(N).
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -65,20 +65,20 @@ │ │ │ │ │ 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 │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ _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) │ │ │ ├── ./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/DsfTrackGenerator.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,107 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    MFAS.h File Reference
    │ │ │ │ +Typedefs | │ │ │ │ +Functions
    │ │ │ │ +
    DsfTrackGenerator.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ +

    Identifies connected components in the keypoint matches graph. │ │ │ │ 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...
    struct  gtsam::gtsfm::Keypoints
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ Typedefs

    │ │ │ │ -typedef std::map< std::pair< Key, Key >, double > gtsam::KeyPairDoubleMap
     
    │ │ │ │ +typedef Eigen::MatrixX2i gtsam::gtsfm::CorrespondenceIndices
     
    │ │ │ │ +using gtsam::gtsfm::KeypointsVector = std::vector< Keypoints >
     
    │ │ │ │ +using gtsam::gtsfm::MatchIndicesMap = std::map< IndexPair, CorrespondenceIndices >
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Functions

    std::vector< SfmTrack2dgtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
     Creates a list of tracks from 2d point correspondences.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    MFAS class to solve Minimum Feedback Arc Set graph problem.

    │ │ │ │ -
    Author
    Akshay Krishnan
    │ │ │ │ -
    Date
    September 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,31 +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 │ │ │ │ │ -MFAS.h File Reference │ │ │ │ │ -MFAS class to solve Minimum Feedback Arc Set graph problem. _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_:_:_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_._._. │ │ │ │ │ +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 std::map< std::pair< _K_e_y, _K_e_y >, double >  ggttssaamm::::KKeeyyPPaaiirrDDoouubblleeMMaapp │ │ │ │ │ +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 ********** │ │ │ │ │ -MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ │ - Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ +Identifies connected components in the keypoint matches graph. │ │ │ │ │ Date │ │ │ │ │ - September 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 │ │ │ │ │ - * _M_F_A_S_._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/a01199_source.html │ │ │ │ @@ -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/sfm/DsfTrackGenerator.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,82 +98,87 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    MFAS.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/Unit3.h>
    │ │ │ │ -
    22#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20#include <gtsam/base/DSFMap.h>
    │ │ │ │ +
    21#include <gtsam/sfm/SfmTrack.h>
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <boost/optional.hpp>
    │ │ │ │
    24
    │ │ │ │ -
    25#include <memory>
    │ │ │ │ -
    26#include <unordered_map>
    │ │ │ │ +
    25#include <Eigen/Core>
    │ │ │ │ +
    26#include <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
    │ │ │ │ - │ │ │ │ -
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ +
    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
    │ │ │ │ +
    Allow for arbitrary type in DSF.
    │ │ │ │ +
    A simple data structure for a track in Structure from Motion.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    The MFAS class to solve a Minimum feedback arc set (MFAS) problem.
    Definition MFAS.h:51
    │ │ │ │ -
    MFAS(const std::map< KeyPair, double > &edgeWeights)
    Construct from the weighted directed edges between the nodes.
    Definition MFAS.h:69
    │ │ │ │ +
    Definition DsfTrackGenerator.h:41
    │ │ │ │ +
    boost::optional< gtsam::Vector > responses
    Optional confidences/responses for each detection, of shape N.
    Definition DsfTrackGenerator.h:50
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,84 +1,88 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MFAS.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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ +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... │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _M_F_A_S_._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/a01202.html │ │ │ │ @@ -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/sfm/SfmTrack.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,64 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    SfmTrack.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SfmTrack.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │

    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, Point2gtsam::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

    │ │ │ │

    A simple data structure for a track in Structure from Motion.

    │ │ │ │
    Date
    January 2022
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +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 │ │ │ │ │ -SfmTrack.h File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ -_G_o_ _t_o_ _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 ********** │ │ │ │ │ A simple data structure for a track in Structure from Motion. │ │ │ │ │ Date │ │ │ │ │ January 2022 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ + * _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/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/SfmData.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,95 +94,84 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    DsfTrackGenerator.cpp File Reference
    │ │ │ │ +
    SfmData.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Identifies connected components in the keypoint matches graph. │ │ │ │ +

    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 DSFMap< IndexPairgtsam::gtsfm::DSFMapIndexPair
     
    │ │ │ │ +typedef PinholeCamera< Cal3Bundlergtsam::SfmCamera
     Define the structure for the camera poses.
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ Functions

    std::vector< SfmTrack2dgtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
     Creates a list of tracks from 2d point correspondences.
     
    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

    │ │ │ │ -

    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.
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +

    Data structure for dealing with Structure from Motion data.

    │ │ │ │ +
    Date
    January 2022
    │ │ │ │ +
    Author
    Frank dellaert
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,66 @@ │ │ │ │ │ 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 | _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 _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::ggttssffmm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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 ********** │ │ │ │ │ -Identifies connected components in the keypoint matches graph. │ │ │ │ │ +Data structure for dealing with Structure from Motion data. │ │ │ │ │ Date │ │ │ │ │ - October 2022 │ │ │ │ │ + January 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. │ │ │ │ │ + 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_._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/a01208.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,12 @@ │ │ │ │ │ var a01208 = [ │ │ │ │ │ - ["tracksFromPairwiseMatches", "a01208.html#ac50218675ff25e1e9202d335ecfa6b3a", null] │ │ │ │ │ + ["gtsam::traits< SfmData >", "a04708.html", null], │ │ │ │ │ + ["SfmCamera", "a01208.html#acab0207730cf78822bdf274b50739d31", null], │ │ │ │ │ + ["gtsam2openGL", "a01208.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ + ["gtsam2openGL", "a01208.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ + ["initialCamerasAndPointsEstimate", "a01208.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ + ["initialCamerasEstimate", "a01208.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ + ["openGL2gtsam", "a01208.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ + ["readBal", "a01208.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ + ["writeBAL", "a01208.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ + ["writeBALfromValues", "a01208.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ ]; │ │ │ ├── ./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/SfmTrack.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,43 +95,63 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    BinaryMeasurement.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    SfmTrack.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. │ │ │ │ +

    A simple data structure for a track in Structure from Motion. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::BinaryMeasurement< T >
    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, Point2gtsam::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

    │ │ │ │ -

    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
    │ │ │ │ +

    A simple data structure for a track in Structure from Motion.

    │ │ │ │ +
    Date
    January 2022
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ +_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 │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_ _T_ _> │ │ │ │ │ +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 ********** │ │ │ │ │ -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 │ │ │ │ │ +A simple data structure for a track in Structure from Motion. │ │ │ │ │ Date │ │ │ │ │ - July 2020 │ │ │ │ │ + January 2022 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01211.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,7 @@ │ │ │ │ │ var a01211 = [ │ │ │ │ │ - ["gtsam::BinaryMeasurement< T >", "a04688.html", "a04688"] │ │ │ │ │ + ["gtsam::SfmTrack2d", "a04712.html", "a04712"], │ │ │ │ │ + ["gtsam::SfmTrack", "a04716.html", "a04716"], │ │ │ │ │ + ["gtsam::traits< SfmTrack >", "a04720.html", null], │ │ │ │ │ + ["SfmMeasurement", "a01211.html#a9ad7940245bbbf4bf25d1dc4690b0108", null], │ │ │ │ │ + ["SiftIndex", "a01211.html#a21a02e01a9608ce6cade24c1366b208c", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01211_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/BinaryMeasurement.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,199 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BinaryMeasurement.h
    │ │ │ │ +
    SfmTrack.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
    │ │ │ │ -
    26#include <gtsam/base/Testable.h>
    │ │ │ │ - │ │ │ │ -
    28#include <gtsam/inference/Key.h>
    │ │ │ │ - │ │ │ │ -
    30
    │ │ │ │ -
    31#include <iostream>
    │ │ │ │ -
    32#include <vector>
    │ │ │ │ -
    33
    │ │ │ │ -
    34namespace gtsam {
    │ │ │ │ -
    35
    │ │ │ │ -
    │ │ │ │ -
    36template <class T> class BinaryMeasurement : public Factor {
    │ │ │ │ -
    37 // Check that T type is testable
    │ │ │ │ -
    38 BOOST_CONCEPT_ASSERT((IsTestable<T>));
    │ │ │ │ -
    39
    │ │ │ │ -
    40public:
    │ │ │ │ -
    41 // shorthand for a smart pointer to a measurement
    │ │ │ │ -
    42 using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;
    │ │ │ │ -
    43
    │ │ │ │ -
    44private:
    │ │ │ │ -
    45 T measured_;
    │ │ │ │ -
    46 SharedNoiseModel noiseModel_;
    │ │ │ │ -
    47
    │ │ │ │ -
    48 public:
    │ │ │ │ -
    49 BinaryMeasurement(Key key1, Key key2, const T &measured,
    │ │ │ │ -
    50 const SharedNoiseModel &model = nullptr)
    │ │ │ │ -
    51 : Factor(std::vector<Key>({key1, key2})),
    │ │ │ │ -
    52 measured_(measured),
    │ │ │ │ -
    53 noiseModel_(model) {}
    │ │ │ │ -
    54
    │ │ │ │ -
    56 virtual ~BinaryMeasurement() {}
    │ │ │ │ -
    57
    │ │ │ │ -
    60
    │ │ │ │ -
    61 Key key1() const { return keys_[0]; }
    │ │ │ │ -
    62 Key key2() const { return keys_[1]; }
    │ │ │ │ -
    63 const T &measured() const { return measured_; }
    │ │ │ │ -
    64 const SharedNoiseModel &noiseModel() const { return noiseModel_; }
    │ │ │ │ -
    65
    │ │ │ │ -
    69
    │ │ │ │ -
    │ │ │ │ -
    70 void print(const std::string &s, const KeyFormatter &keyFormatter =
    │ │ │ │ -
    71 DefaultKeyFormatter) const override {
    │ │ │ │ -
    72 std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
    │ │ │ │ -
    73 << keyFormatter(this->key2()) << ")\n";
    │ │ │ │ -
    74 traits<T>::Print(measured_, " measured: ");
    │ │ │ │ -
    75 this->noiseModel_->print(" noise model: ");
    │ │ │ │ -
    76 }
    │ │ │ │ -
    │ │ │ │ -
    77
    │ │ │ │ -
    78 bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
    │ │ │ │ -
    79 const BinaryMeasurement<T> *e =
    │ │ │ │ -
    80 dynamic_cast<const BinaryMeasurement<T> *>(&expected);
    │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ - │ │ │ │ -
    The base class for all factors.
    │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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};
    │ │ │ │ +
    │ │ │ │ +
    119
    │ │ │ │ +
    120using SfmTrack2dVector = std::vector<SfmTrack2d>;
    │ │ │ │ +
    121
    │ │ │ │ +
    │ │ │ │ +
    126struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ +
    std::pair< size_t, size_t > SiftIndex
    Sift index for SfmTrack.
    Definition SfmTrack.h:36
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    std::pair< size_t, Point2 > SfmMeasurement
    A measurement with its camera index.
    Definition SfmTrack.h:33
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    Factor()
    Default constructor for I/O.
    Definition Factor.h:91
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Definition BinaryMeasurement.h:36
    │ │ │ │ -
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition BinaryMeasurement.h:70
    │ │ │ │ -
    virtual ~BinaryMeasurement()
    Destructor.
    Definition BinaryMeasurement.h:56
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Track containing 2D measurements associated with a single 3D point.
    Definition SfmTrack.h:43
    │ │ │ │ +
    void addMeasurement(size_t idx, const gtsam::Point2 &m)
    Add measurement (camera_idx, Point2) to track.
    Definition SfmTrack.h:65
    │ │ │ │ +
    const SfmMeasurement & measurement(size_t idx) const
    Get the measurement (camera index, Point2) at pose index idx
    Definition SfmTrack.h:73
    │ │ │ │ +
    std::vector< SiftIndex > siftIndices
    The feature descriptors (optional)
    Definition SfmTrack.h:48
    │ │ │ │ +
    const SiftIndex & siftIndex(size_t idx) const
    Get the SIFT feature index corresponding to the measurement at idx
    Definition SfmTrack.h:78
    │ │ │ │ +
    bool hasUniqueCameras() const
    Check that no two measurements are from the same camera.
    Definition SfmTrack.h:84
    │ │ │ │ +
    Eigen::MatrixX2d measurementMatrix() const
    Return the measurements as a 2D matrix.
    Definition SfmTrack.h:100
    │ │ │ │ +
    size_t numberMeasurements() const
    Total number of measurements in this track.
    Definition SfmTrack.h:70
    │ │ │ │ +
    std::vector< SfmMeasurement > measurements
    The 2D image projections (id,(u,v))
    Definition SfmTrack.h:45
    │ │ │ │ +
    Eigen::VectorXi indexVector() const
    Return the camera indices of the measurements.
    Definition SfmTrack.h:109
    │ │ │ │ +
    Definition SfmTrack.h:126
    │ │ │ │ +
    Point3 rgb() const
    Get RGB values describing 3d point.
    Definition SfmTrack.h:148
    │ │ │ │ +
    float b
    RGB color of the 3D point.
    Definition SfmTrack.h:128
    │ │ │ │ +
    const Point3 & point3() const
    Get 3D point.
    Definition SfmTrack.h:145
    │ │ │ │ +
    Point3 p
    3D position of the point
    Definition SfmTrack.h:127
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,138 +1,248 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BinaryMeasurement.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-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 │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._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_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._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 │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33 │ │ │ │ │ -34namespace _g_t_s_a_m { │ │ │ │ │ -35 │ │ │ │ │ -_3_6template class _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t : public _F_a_c_t_o_r { │ │ │ │ │ -37 // Check that T type is testable │ │ │ │ │ -38 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_T_>)); │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -41 // shorthand for a smart pointer to a measurement │ │ │ │ │ -42 using shared_ptr = typename boost::shared_ptr; │ │ │ │ │ -43 │ │ │ │ │ -44private: │ │ │ │ │ -45 T measured_; │ │ │ │ │ -46 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ -49 _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t(_K_e_y key1, _K_e_y key2, const T &measured, │ │ │ │ │ -50 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model = nullptr) │ │ │ │ │ -51 : _F_a_c_t_o_r(std::vector({key1, key2})), │ │ │ │ │ -52 measured_(measured), │ │ │ │ │ -53 noiseModel_(model) {} │ │ │ │ │ -54 │ │ │ │ │ -_5_6 virtual _~_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t() {} │ │ │ │ │ -57 │ │ │ │ │ -60 │ │ │ │ │ -61 _K_e_y key1() const { return _k_e_y_s__[0]; } │ │ │ │ │ -62 _K_e_y key2() const { return _k_e_y_s__[1]; } │ │ │ │ │ -63 const T &measured() const { return measured_; } │ │ │ │ │ -64 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &noiseModel() const { return noiseModel_; } │ │ │ │ │ -65 │ │ │ │ │ -69 │ │ │ │ │ -_7_0 void _p_r_i_n_t(const std::string &s, const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = │ │ │ │ │ -71 DefaultKeyFormatter) const override { │ │ │ │ │ -72 std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," │ │ │ │ │ -73 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ -74 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(measured_, " measured: "); │ │ │ │ │ -75 this->noiseModel_->print(" noise model: "); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 bool _e_q_u_a_l_s(const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t &expected, double tol = 1e-9) const { │ │ │ │ │ -79 const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_T_> *e = │ │ │ │ │ -80 dynamic_cast *>(&expected); │ │ │ │ │ -81 return e != nullptr && _F_a_c_t_o_r_:_:_e_q_u_a_l_s(*e) && │ │ │ │ │ -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. │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +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}; │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_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_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_:_:_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_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn BinaryMeasurement.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_:_:_~_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -virtual ~BinaryMeasurement() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn BinaryMeasurement.h:56 │ │ │ │ │ +_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 │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ + * _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/TranslationRecovery.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,84 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    SfmData.h File Reference
    │ │ │ │ +
    TranslationRecovery.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Data structure for dealing with Structure from Motion data. │ │ │ │ +

    Source code for recovering translations when rotations are given. │ │ │ │ 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< Cal3Bundlergtsam::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.
     
    │ │ │ │ +DSFMap< KeygetSameTranslationDSFMap (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

    │ │ │ │ -

    Data structure for dealing with Structure from Motion data.

    │ │ │ │ -
    Date
    January 2022
    │ │ │ │ -
    Author
    Frank dellaert
    │ │ │ │ +

    Source code for recovering translations when rotations are given.

    │ │ │ │ +
    Author
    Frank Dellaert, Akshay Krishnan
    │ │ │ │ +
    Date
    March 2020
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,66 +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 │ │ │ │ │ -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. │ │ │ │ │ -  │ │ │ │ │ +_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 │ │ │ │ │ -_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. │ │ │ │ │ + _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) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Data structure for dealing with Structure from Motion data. │ │ │ │ │ - Date │ │ │ │ │ - January 2022 │ │ │ │ │ +Source code for recovering translations 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_._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/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/ShonanAveraging.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,107 +96,60 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    DsfTrackGenerator.h File Reference
    │ │ │ │ +Typedefs
    │ │ │ │ +
    ShonanAveraging.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Identifies connected components in the keypoint matches graph. │ │ │ │ +

    Shonan Averaging algorithm. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::gtsfm::Keypoints
    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

    │ │ │ │ -typedef Eigen::MatrixX2i gtsam::gtsfm::CorrespondenceIndices
     
    │ │ │ │ -using gtsam::gtsfm::KeypointsVector = std::vector< Keypoints >
     
    │ │ │ │ -using gtsam::gtsfm::MatchIndicesMap = std::map< IndexPair, CorrespondenceIndices >
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ -Functions

    std::vector< SfmTrack2dgtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
     Creates a list of tracks from 2d point correspondences.
     
    │ │ │ │ +using gtsam::ShonanAveragingParameters2 = ShonanAveragingParameters< 2 >
     
    │ │ │ │ +using gtsam::ShonanAveragingParameters3 = ShonanAveragingParameters< 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.
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +

    Shonan Averaging algorithm.

    │ │ │ │ +
    Date
    March 2019 - August 2020
    │ │ │ │ +
    Author
    Frank Dellaert, David Rosen, and Jing Wu
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,57 +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 │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s │ │ │ │ │ +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 │ │ │ │ │ -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 > │ │ │ │ │ +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 > │ │ │ │ │   │ │ │ │ │ -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::::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 ********** │ │ │ │ │ -Identifies connected components in the keypoint matches graph. │ │ │ │ │ +Shonan Averaging algorithm. │ │ │ │ │ Date │ │ │ │ │ - July 2022 │ │ │ │ │ + March 2019 - August 2020 │ │ │ │ │ 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, David Rosen, and Jing Wu │ │ │ │ │ * _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_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/a01220.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,6 @@ │ │ │ │ │ var a01220 = [ │ │ │ │ │ - ["gtsam::gtsfm::Keypoints", "a04692.html", "a04692"], │ │ │ │ │ - ["tracksFromPairwiseMatches", "a01220.html#ac50218675ff25e1e9202d335ecfa6b3a", null] │ │ │ │ │ + ["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/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/ShonanAveraging.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,87 +98,390 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DsfTrackGenerator.h
    │ │ │ │ +
    ShonanAveraging.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 {
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    31#include <gtsam/slam/dataset.h>
    │ │ │ │
    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
    │ │ │ │ -
    Allow for arbitrary type in DSF.
    │ │ │ │ -
    A simple data structure for a track in Structure from Motion.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    54 Anchor anchor;
    │ │ │ │ +
    55 double alpha;
    │ │ │ │ +
    56 double beta;
    │ │ │ │ +
    57 double gamma;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    62
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    accelerated power method for fast eigenvalue and eigenvector computation
    │ │ │ │ +
    Power method for fast eigenvalue and eigenvector computation.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Parameters for Levenberg-Marquardt trust-region scheme.
    │ │ │ │ +
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ +
    utility functions for loading datasets
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition DsfTrackGenerator.h:41
    │ │ │ │ -
    boost::optional< gtsam::Vector > responses
    Optional confidences/responses for each detection, of shape N.
    Definition DsfTrackGenerator.h:50
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition BinaryMeasurement.h:36
    │ │ │ │ +
    Parameters governing optimization etc.
    Definition ShonanAveraging.h:46
    │ │ │ │ +
    double alpha
    weight of anchor-based prior (default 0)
    Definition ShonanAveraging.h:55
    │ │ │ │ +
    void print(const std::string &s="") const
    Print the parameters and flags used for rotation averaging.
    Definition ShonanAveraging.h:94
    │ │ │ │ +
    LevenbergMarquardtParams lm
    LM parameters.
    Definition ShonanAveraging.h:52
    │ │ │ │ +
    bool useHuber
    if enabled, the Huber loss is used (default false)
    Definition ShonanAveraging.h:59
    │ │ │ │ +
    double optimalityThreshold
    threshold used in checkOptimality
    Definition ShonanAveraging.h:53
    │ │ │ │ +
    double beta
    weight of Karcher-based prior (default 1)
    Definition ShonanAveraging.h:56
    │ │ │ │ +
    double gamma
    weight of gauge-fixing factors (default 0)
    Definition ShonanAveraging.h:57
    │ │ │ │ +
    Anchor anchor
    pose to use as anchor if not Karcher
    Definition ShonanAveraging.h:54
    │ │ │ │ +
    bool certifyOptimality
    if enabled solution optimality is certified (default true)
    Definition ShonanAveraging.h:61
    │ │ │ │ +
    Class that implements Shonan Averaging from our ECCV'20 paper.
    Definition ShonanAveraging.h:123
    │ │ │ │ +
    Sparse D() const
    Sparse version of D.
    Definition ShonanAveraging.h:215
    │ │ │ │ +
    const BinaryMeasurement< Rot > & measurement(size_t k) const
    k^th binary measurement
    Definition ShonanAveraging.h:171
    │ │ │ │ +
    Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
    Update factors to use robust Huber loss.
    Definition ShonanAveraging.h:181
    │ │ │ │ +
    Values roundSolutionS(const Matrix &S) const
    Project pxdN Stiefel manifold matrix S to Rot3^N.
    │ │ │ │ +
    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
    │ │ │ │ +
    Sparse L() const
    Sparse version of L.
    Definition ShonanAveraging.h:219
    │ │ │ │ +
    const Rot & measured(size_t k) const
    k^th measurement, as a Rot.
    Definition ShonanAveraging.h:206
    │ │ │ │ +
    Sparse Q() const
    Sparse version of Q.
    Definition ShonanAveraging.h:217
    │ │ │ │ +
    Matrix denseD() const
    Dense version of D.
    Definition ShonanAveraging.h:216
    │ │ │ │ +
    size_t nrUnknowns() const
    Return number of unknowns.
    Definition ShonanAveraging.h:165
    │ │ │ │ +
    Values projectFrom(size_t p, const Values &values) const
    Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
    │ │ │ │ +
    size_t numberMeasurements() const
    Return number of measurements.
    Definition ShonanAveraging.h:168
    │ │ │ │ +
    Matrix denseL() const
    Dense version of L.
    Definition ShonanAveraging.h:220
    │ │ │ │ +
    Matrix computeLambda_(const Matrix &S) const
    Dense versions of computeLambda for wrapper/testing.
    Definition ShonanAveraging.h:231
    │ │ │ │ +
    Matrix denseQ() const
    Dense version of Q.
    Definition ShonanAveraging.h:218
    │ │ │ │ +
    const KeyVector & keys(size_t k) const
    Keys for k^th measurement, as a vector of Key values.
    Definition ShonanAveraging.h:209
    │ │ │ │ +
    Matrix computeLambda_(const Values &values) const
    Dense versions of computeLambda for wrapper/testing.
    Definition ShonanAveraging.h:226
    │ │ │ │ +
    Matrix computeA_(const Values &values) const
    Dense version of computeA for wrapper/testing.
    Definition ShonanAveraging.h:242
    │ │ │ │ +
    static Values LiftTo(size_t p, const Values &values)
    Lift Values of type T to SO(p)
    Definition ShonanAveraging.h:367
    │ │ │ │ +
    Definition ShonanAveraging.h:428
    │ │ │ │ +
    Definition ShonanAveraging.h:438
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,88 +1,473 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DsfTrackGenerator.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, 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 { │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ +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()); │ │ │ │ │ +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 │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_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 │ │ │ │ │ +_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. │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ * _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_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/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/MFAS.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,59 +97,49 @@ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs
    │ │ │ │ -
    ShonanAveraging.h File Reference
    │ │ │ │ +
    MFAS.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Shonan Averaging algorithm. │ │ │ │ +

    MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ 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
    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::ShonanAveragingParameters2 = ShonanAveragingParameters< 2 >
     
    │ │ │ │ -using gtsam::ShonanAveragingParameters3 = ShonanAveragingParameters< 3 >
     
    │ │ │ │ +typedef std::map< std::pair< Key, Key >, double > gtsam::KeyPairDoubleMap
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Shonan Averaging algorithm.

    │ │ │ │ -
    Date
    March 2019 - August 2020
    │ │ │ │ -
    Author
    Frank Dellaert, David Rosen, and Jing Wu
    │ │ │ │ +

    MFAS class to solve Minimum Feedback Arc Set graph problem.

    │ │ │ │ +
    Author
    Akshay Krishnan
    │ │ │ │ +
    Date
    September 2020
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _T_y_p_e_d_e_f_s │ │ │ │ │ -ShonanAveraging.h File Reference │ │ │ │ │ -Shonan Averaging algorithm. _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 │ │ │ │ │ -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 │ │ │ │ │ +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::::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 > │ │ │ │ │ +typedef std::map< std::pair< _K_e_y, _K_e_y >, double >  ggttssaamm::::KKeeyyPPaaiirrDDoouubblleeMMaapp │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Shonan Averaging algorithm. │ │ │ │ │ - Date │ │ │ │ │ - March 2019 - August 2020 │ │ │ │ │ +MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, David Rosen, and Jing Wu │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ + Date │ │ │ │ │ + September 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._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/a01223_source.html │ │ │ │ @@ -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/sfm/MFAS.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,390 +98,82 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ShonanAveraging.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/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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    54 Anchor anchor;
    │ │ │ │ -
    55 double alpha;
    │ │ │ │ -
    56 double beta;
    │ │ │ │ -
    57 double gamma;
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    12#pragma once
    │ │ │ │ +
    13
    │ │ │ │ +
    21#include <gtsam/geometry/Unit3.h>
    │ │ │ │ +
    22#include <gtsam/inference/Key.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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    63 public:
    │ │ │ │ +
    │ │ │ │ +
    69 MFAS(const std::map<KeyPair, double> &edgeWeights)
    │ │ │ │ +
    70 : edgeWeights_(edgeWeights) {}
    │ │ │ │
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    Power method for fast eigenvalue and eigenvector computation.
    │ │ │ │ -
    accelerated power method for fast eigenvalue and eigenvector computation
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Parameters for Levenberg-Marquardt trust-region scheme.
    │ │ │ │ -
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ -
    utility functions for loading datasets
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition BinaryMeasurement.h:36
    │ │ │ │ -
    Parameters governing optimization etc.
    Definition ShonanAveraging.h:46
    │ │ │ │ -
    double alpha
    weight of anchor-based prior (default 0)
    Definition ShonanAveraging.h:55
    │ │ │ │ -
    void print(const std::string &s="") const
    Print the parameters and flags used for rotation averaging.
    Definition ShonanAveraging.h:94
    │ │ │ │ -
    LevenbergMarquardtParams lm
    LM parameters.
    Definition ShonanAveraging.h:52
    │ │ │ │ -
    bool useHuber
    if enabled, the Huber loss is used (default false)
    Definition ShonanAveraging.h:59
    │ │ │ │ -
    double optimalityThreshold
    threshold used in checkOptimality
    Definition ShonanAveraging.h:53
    │ │ │ │ -
    double beta
    weight of Karcher-based prior (default 1)
    Definition ShonanAveraging.h:56
    │ │ │ │ -
    double gamma
    weight of gauge-fixing factors (default 0)
    Definition ShonanAveraging.h:57
    │ │ │ │ -
    Anchor anchor
    pose to use as anchor if not Karcher
    Definition ShonanAveraging.h:54
    │ │ │ │ -
    bool certifyOptimality
    if enabled solution optimality is certified (default true)
    Definition ShonanAveraging.h:61
    │ │ │ │ -
    Class that implements Shonan Averaging from our ECCV'20 paper.
    Definition ShonanAveraging.h:123
    │ │ │ │ -
    Sparse D() const
    Sparse version of D.
    Definition ShonanAveraging.h:215
    │ │ │ │ -
    const BinaryMeasurement< Rot > & measurement(size_t k) const
    k^th binary measurement
    Definition ShonanAveraging.h:171
    │ │ │ │ -
    Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
    Update factors to use robust Huber loss.
    Definition ShonanAveraging.h:181
    │ │ │ │ -
    Values roundSolutionS(const Matrix &S) const
    Project pxdN Stiefel manifold matrix S to Rot3^N.
    │ │ │ │ -
    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
    │ │ │ │ -
    Sparse L() const
    Sparse version of L.
    Definition ShonanAveraging.h:219
    │ │ │ │ -
    const Rot & measured(size_t k) const
    k^th measurement, as a Rot.
    Definition ShonanAveraging.h:206
    │ │ │ │ -
    Sparse Q() const
    Sparse version of Q.
    Definition ShonanAveraging.h:217
    │ │ │ │ -
    Matrix denseD() const
    Dense version of D.
    Definition ShonanAveraging.h:216
    │ │ │ │ -
    size_t nrUnknowns() const
    Return number of unknowns.
    Definition ShonanAveraging.h:165
    │ │ │ │ -
    Values projectFrom(size_t p, const Values &values) const
    Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
    │ │ │ │ -
    size_t numberMeasurements() const
    Return number of measurements.
    Definition ShonanAveraging.h:168
    │ │ │ │ -
    Matrix denseL() const
    Dense version of L.
    Definition ShonanAveraging.h:220
    │ │ │ │ -
    Matrix computeLambda_(const Matrix &S) const
    Dense versions of computeLambda for wrapper/testing.
    Definition ShonanAveraging.h:231
    │ │ │ │ -
    Matrix denseQ() const
    Dense version of Q.
    Definition ShonanAveraging.h:218
    │ │ │ │ -
    const KeyVector & keys(size_t k) const
    Keys for k^th measurement, as a vector of Key values.
    Definition ShonanAveraging.h:209
    │ │ │ │ -
    Matrix computeLambda_(const Values &values) const
    Dense versions of computeLambda for wrapper/testing.
    Definition ShonanAveraging.h:226
    │ │ │ │ -
    Matrix computeA_(const Values &values) const
    Dense version of computeA for wrapper/testing.
    Definition ShonanAveraging.h:242
    │ │ │ │ -
    static Values LiftTo(size_t p, const Values &values)
    Lift Values of type T to SO(p)
    Definition ShonanAveraging.h:367
    │ │ │ │ -
    Definition ShonanAveraging.h:428
    │ │ │ │ -
    Definition ShonanAveraging.h:438
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    The MFAS class to solve a Minimum feedback arc set (MFAS) problem.
    Definition MFAS.h:51
    │ │ │ │ +
    MFAS(const std::map< KeyPair, double > &edgeWeights)
    Construct from the weighted directed edges between the nodes.
    Definition MFAS.h:69
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,473 +1,84 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ShonanAveraging.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_/_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; │ │ │ │ │ +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 _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; } │ │ │ │ │ +63 public: │ │ │ │ │ +_6_9 _M_F_A_S(const std::map &edgeWeights) │ │ │ │ │ +70 : edgeWeights_(edgeWeights) {} │ │ │ │ │ 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()); │ │ │ │ │ -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. │ │ │ │ │ -_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. │ │ │ │ │ +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... │ │ │ │ │ -_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_:_:_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_:_:_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_A_v_e_r_a_g_i_n_g_._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/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/TranslationRecovery.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Functions
    │ │ │ │ -
    TranslationRecovery.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    TranslationRecovery.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Source code for recovering translations when rotations are given. │ │ │ │ +

    Recovering translations in an epipolar graph when rotations are given. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ -Functions

    │ │ │ │ -DSFMap< KeygetSameTranslationDSFMap (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)
     

    │ │ │ │ +Classes

    class  gtsam::TranslationRecovery
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +Namespaces

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

    Detailed Description

    │ │ │ │ -

    Source code for recovering translations when rotations are given.

    │ │ │ │ +

    Recovering translations in an epipolar graph when rotations are given.

    │ │ │ │
    Author
    Frank Dellaert, Akshay Krishnan
    │ │ │ │
    Date
    March 2020
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,26 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ -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) │ │ │ │ │ +_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 │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Source code for recovering translations when rotations are given. │ │ │ │ │ +Recovering translations in an epipolar graph when rotations are given. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert, Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ March 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_._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/a01229_source.html │ │ │ │ @@ -114,17 +114,17 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ -
    21#include <gtsam/geometry/SOn.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    21#include <gtsam/geometry/SOn.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
    │ │ │ │ @@ -177,17 +177,17 @@ │ │ │ │
    102 }
    │ │ │ │
    │ │ │ │
    103};
    │ │ │ │ │ │ │ │
    104// \ShonanGaugeFactor
    │ │ │ │
    105
    │ │ │ │
    106} // namespace gtsam
    │ │ │ │ -
    N*N matrix representation of SO(N).
    │ │ │ │ - │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    N*N matrix representation of SO(N).
    │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
    Definition ShonanGaugeFactor.h:45
    │ │ │ │
    double error(const Values &c) const override
    Calculate the error of the factor: always zero.
    Definition ShonanGaugeFactor.h:94
    │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01238_source.html │ │ │ │ @@ -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/slam/StereoFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,94 +98,218 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    KarcherMeanFactor.h
    │ │ │ │ +
    StereoFactor.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 KarcherMeanFactor.h
    │ │ │ │ -
    14 * @author Frank Dellaert
    │ │ │ │ -
    15 * @date March 2019
    │ │ │ │ -
    16 */
    │ │ │ │ -
    17
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ -
    20#include <gtsam/base/Matrix.h>
    │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23#include <map>
    │ │ │ │ -
    24#include <vector>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │
    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
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    84 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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_;
    │ │ │ │ +
    160 }
    │ │ │ │ +
    │ │ │ │ +
    161
    │ │ │ │ +
    163 inline bool verboseCheirality() const { return verboseCheirality_; }
    │ │ │ │ +
    164
    │ │ │ │ +
    166 inline bool throwCheirality() const { return throwCheirality_; }
    │ │ │ │ +
    167
    │ │ │ │ +
    168private:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A Stereo Camera based on two Simple Cameras.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Definition StereoCamera.h:26
    │ │ │ │ +
    A stereo camera class, parameterize by left camera pose and stereo calibration.
    Definition StereoCamera.h:47
    │ │ │ │ +
    StereoPoint2 project(const Point3 &point) const
    Project 3D point to StereoPoint2 (uL,uR,v)
    Definition StereoCamera.cpp:32
    │ │ │ │ +
    A 2D stereo point, v will be same for rectified images.
    Definition StereoPoint2.h:32
    │ │ │ │ +
    Vector3 vector() const
    convert to vector
    Definition StereoPoint2.h:115
    │ │ │ │ +
    bool equals(const StereoPoint2 &q, double tol=1e-9) const
    equals
    Definition StereoPoint2.h:64
    │ │ │ │ +
    void print(const std::string &s="") const
    print
    Definition StereoPoint2.cpp:26
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher me...
    Definition KarcherMeanFactor.h:45
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor: always zero.
    Definition KarcherMeanFactor.h:69
    │ │ │ │ -
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition KarcherMeanFactor.h:72
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
    linearize to a GaussianFactor
    Definition KarcherMeanFactor.h:75
    │ │ │ │ -
    ~KarcherMeanFactor() override
    Destructor.
    Definition KarcherMeanFactor.h:66
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    A Generic Stereo Factor.
    Definition StereoFactor.h:31
    │ │ │ │ +
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition StereoFactor.h:166
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equals(const NonlinearFactor &f, double tol=1e-9) const override
    equals
    Definition StereoFactor.h:113
    │ │ │ │ +
    const Cal3_S2Stereo::shared_ptr calibration() const
    return the calibration object
    Definition StereoFactor.h:158
    │ │ │ │ +
    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
    │ │ │ │ +
    GenericStereoFactor< POSE, LANDMARK > This
    typedef for this class (with templates)
    Definition StereoFactor.h:47
    │ │ │ │ +
    ~GenericStereoFactor() override
    Virtual destructor.
    Definition StereoFactor.h:91
    │ │ │ │ +
    bool verboseCheirality() const
    return verbosity
    Definition StereoFactor.h:163
    │ │ │ │ +
    boost::shared_ptr< GenericStereoFactor > shared_ptr
    typedef for shared pointer to this object
    Definition StereoFactor.h:48
    │ │ │ │ +
    const StereoPoint2 & measured() const
    return the measured
    Definition StereoFactor.h:153
    │ │ │ │ +
    GenericStereoFactor()
    Default constructor.
    Definition StereoFactor.h:54
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition StereoFactor.h:170
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition StereoFactor.h:103
    │ │ │ │ +
    POSE CamPose
    typedef for Pose Lie Value type
    Definition StereoFactor.h:49
    │ │ │ │ +
    NoiseModelFactorN< POSE, LANDMARK > Base
    typedef for base class
    Definition StereoFactor.h:46
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition StereoFactor.h:94
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,111 +1,317 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KarcherMeanFactor.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 │ │ │ │ │ -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 │ │ │ │ │ +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 { │ │ │ │ │ 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 │ │ │ │ │ +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_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. │ │ │ │ │ +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_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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) │ │ │ │ │ +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 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 │ │ │ │ │ - * KKaarrcchheerrMMeeaannFFaaccttoorr..hh │ │ │ │ │ + * _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/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/BetweenFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,47 +95,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    RegularImplicitSchurFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Macros
    │ │ │ │ +
    BetweenFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::RegularImplicitSchurFactor< CAMERA >
     RegularImplicitSchurFactor. 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< RegularImplicitSchurFactor< CAMERA > >
    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

    │ │ │ │ -

    A subclass of GaussianFactor specialized to structureless SFM.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ +
    Author
    Frank Dellaert, Viorela Ila
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -RegularImplicitSchurFactor.h File Reference │ │ │ │ │ -A subclass of GaussianFactor specialized to structureless SFM. _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_:_:_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_:_:_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_<_ _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_<_ _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 ********** │ │ │ │ │ -A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Frank Dellaert, Viorela Ila │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01241.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a01241 = [ │ │ │ │ │ - ["gtsam::RegularImplicitSchurFactor< CAMERA >", "a04912.html", "a04912"], │ │ │ │ │ - ["gtsam::traits< RegularImplicitSchurFactor< CAMERA > >", "a04916.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/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/BetweenFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,587 +98,215 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    RegularImplicitSchurFactor.h
    │ │ │ │ +
    BetweenFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -Go to the documentation of this file.
    1
    │ │ │ │ -
    8#pragma once
    │ │ │ │ -
    9
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    13
    │ │ │ │ -
    14#include <iosfwd>
    │ │ │ │ -
    15#include <map>
    │ │ │ │ -
    16#include <string>
    │ │ │ │ -
    17#include <vector>
    │ │ │ │ -
    18
    │ │ │ │ -
    19namespace gtsam {
    │ │ │ │ -
    20
    │ │ │ │ -
    38template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    40
    │ │ │ │ -
    41public:
    │ │ │ │ - │ │ │ │ -
    43 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    44
    │ │ │ │ -
    45protected:
    │ │ │ │ -
    46
    │ │ │ │ -
    47 // This factor is closely related to a CameraSet
    │ │ │ │ -
    48 typedef CameraSet<CAMERA> Set;
    │ │ │ │ +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
    │ │ │ │ +
    16#pragma once
    │ │ │ │ +
    17
    │ │ │ │ +
    18#include <ostream>
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/base/Testable.h>
    │ │ │ │ +
    21#include <gtsam/base/Lie.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
    │ │ │ │ +
    32namespace gtsam {
    │ │ │ │ +
    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:
    │ │ │ │ +
    47
    │ │ │ │ +
    48 typedef VALUE T;
    │ │ │ │
    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:
    │ │ │ │ +
    50 private:
    │ │ │ │ +
    51
    │ │ │ │ +
    52 typedef BetweenFactor<VALUE> This;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ - │ │ │ │ -
    129 && equal_with_abs_tol(E_, f->E_, tol)
    │ │ │ │ -
    130 && equal_with_abs_tol(b_, f->b_, tol);
    │ │ │ │ -
    131 }
    │ │ │ │ + │ │ │ │ +
    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 {}
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    134 DenseIndex getDim(const_iterator variable) const override {
    │ │ │ │ -
    135 return D;
    │ │ │ │ -
    136 }
    │ │ │ │ -
    │ │ │ │ -
    137
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    133 private:
    │ │ │ │ +
    134
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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 {}
    │ │ │ │
    │ │ │ │ -
    169
    │ │ │ │ - │ │ │ │ +
    170
    │ │ │ │ +
    171 private:
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    257 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
    │ │ │ │ - │ │ │ │ -
    259 throw std::runtime_error(
    │ │ │ │ -
    260 "RegularImplicitSchurFactor::clone non implemented");
    │ │ │ │ -
    261 }
    │ │ │ │ -
    │ │ │ │ -
    262
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    264 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    509
    │ │ │ │ -
    510template<class CAMERA>
    │ │ │ │ - │ │ │ │ -
    512
    │ │ │ │ -
    513// traits
    │ │ │ │ -
    │ │ │ │ -
    514template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
    │ │ │ │ -
    515 RegularImplicitSchurFactor<CAMERA> > {
    │ │ │ │ -
    516};
    │ │ │ │ -
    │ │ │ │ -
    517
    │ │ │ │ -
    518}
    │ │ │ │ -
    519
    │ │ │ │ -
    Base class to create smart factors on poses or cameras.
    │ │ │ │ - │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    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
    │ │ │ │ +
    Lie Group Concept.
    Definition Lie.h:260
    │ │ │ │ +
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    VectorValues hessianDiagonal() const
    Return the diagonal of the Hessian for this factor.
    Definition GaussianFactor.cpp:35
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    iterator insert(const std::pair< Key, Vector > &key_value)
    Insert a vector value with key j.
    Definition VectorValues.cpp:91
    │ │ │ │ -
    std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
    Emplace a vector value with key j.
    Definition VectorValues.h:185
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    RegularImplicitSchurFactor.
    Definition RegularImplicitSchurFactor.h:39
    │ │ │ │ -
    const Matrix E_
    The 2m*3 E Jacobian with respect to the point.
    Definition RegularImplicitSchurFactor.h:60
    │ │ │ │ -
    void projectError(const Error2s &e1, Error2s &e2) const
    Calculate corrected error Q*e = (I - E*P*E')*e.
    Definition RegularImplicitSchurFactor.h:357
    │ │ │ │ -
    GaussianFactor::shared_ptr clone() const override
    Clone a factor (make a deep copy)
    Definition RegularImplicitSchurFactor.h:256
    │ │ │ │ -
    void hessianDiagonalAdd(VectorValues &d) const override
    Add the diagonal of the Hessian for this factor to existing VectorValues.
    Definition RegularImplicitSchurFactor.h:174
    │ │ │ │ -
    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
    │ │ │ │ -
    RegularImplicitSchurFactor()
    Constructor.
    Definition RegularImplicitSchurFactor.h:66
    │ │ │ │ -
    const Vector b_
    2m-dimensional RHS vector
    Definition RegularImplicitSchurFactor.h:61
    │ │ │ │ -
    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
    │ │ │ │ -
    Eigen::Matrix< double, D, D > MatrixDD
    camera Hessian
    Definition RegularImplicitSchurFactor.h:55
    │ │ │ │ -
    void gradientAtZero(double *d) const override
    Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
    Definition RegularImplicitSchurFactor.h:479
    │ │ │ │ -
    static const int ZDim
    Measurement dimension.
    Definition RegularImplicitSchurFactor.h:52
    │ │ │ │ -
    Matrix information() const override
    Compute full information matrix
    Definition RegularImplicitSchurFactor.h:163
    │ │ │ │ -
    ~RegularImplicitSchurFactor() override
    Destructor.
    Definition RegularImplicitSchurFactor.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    FBlocks FBlocks_
    All ZDim*D F blocks (one for each camera)
    Definition RegularImplicitSchurFactor.h:58
    │ │ │ │ -
    VectorValues gradientAtZero() const override
    Calculate gradient, which is -F'Q*b, see paper.
    Definition RegularImplicitSchurFactor.h:457
    │ │ │ │ -
    Eigen::Matrix< double, ZDim, D > MatrixZD
    type of an F block
    Definition RegularImplicitSchurFactor.h:54
    │ │ │ │ -
    const Matrix PointCovariance_
    the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
    Definition RegularImplicitSchurFactor.h:59
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    Hessian-vector multiply, i.e.
    Definition RegularImplicitSchurFactor.h:413
    │ │ │ │ -
    std::map< Key, Matrix > hessianBlockDiagonal() const override
    Return the block diagonal of the Hessian for this factor.
    Definition RegularImplicitSchurFactor.h:231
    │ │ │ │ -
    Matrix augmentedInformation() const override
    Compute full augmented information matrix
    Definition RegularImplicitSchurFactor.h:155
    │ │ │ │ -
    GaussianFactor::shared_ptr negate() const override
    Construct the corresponding anti-factor to negate information stored stored in this factor.
    Definition RegularImplicitSchurFactor.h:263
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition RegularImplicitSchurFactor.h:43
    │ │ │ │ -
    Error2s e1
    Scratch space for multiplyHessianAdd.
    Definition RegularImplicitSchurFactor.h:374
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RegularImplicitSchurFactor.h:105
    │ │ │ │ -
    DenseIndex getDim(const_iterator variable) const override
    Degrees of freedom of camera.
    Definition RegularImplicitSchurFactor.h:134
    │ │ │ │ -
    bool equals(const GaussianFactor &lf, double tol) const override
    equals
    Definition RegularImplicitSchurFactor.h:118
    │ │ │ │ -
    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
    │ │ │ │ -
    RegularImplicitSchurFactor This
    Typedef to this class.
    Definition RegularImplicitSchurFactor.h:42
    │ │ │ │ -
    static const int D
    Camera dimension.
    Definition RegularImplicitSchurFactor.h:51
    │ │ │ │ -
    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
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const double *x, double *y) const
    double* Hessian-vector multiply, i.e.
    Definition RegularImplicitSchurFactor.h:380
    │ │ │ │ -
    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
    │ │ │ │ -
    void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
    Dummy version to measure overhead of key access.
    Definition RegularImplicitSchurFactor.h:442
    │ │ │ │ -
    Vector gradient(Key key, const VectorValues &x) const override
    Gradient wrt a key at any values.
    Definition RegularImplicitSchurFactor.h:499
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    A class for a measurement predicted by "between(config[key1],config[key2])".
    Definition BetweenFactor.h:40
    │ │ │ │ +
    BetweenFactor()
    default constructor - only use for serialization
    Definition BetweenFactor.h:66
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    assert equality up to a tolerance
    Definition BetweenFactor.h:98
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition BetweenFactor.h:79
    │ │ │ │ +
    boost::shared_ptr< BetweenFactor > shared_ptr
    The measurement.
    Definition BetweenFactor.h:60
    │ │ │ │ +
    const VALUE & measured() const
    return the measurement
    Definition BetweenFactor.h:128
    │ │ │ │ +
    BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition BetweenFactor.h:69
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BetweenFactor.h:136
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print with optional string
    Definition BetweenFactor.h:87
    │ │ │ │ +
    Binary between constraint - forces between to a given value This constraint requires the underlying t...
    Definition BetweenFactor.h:161
    │ │ │ │ +
    BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
    Syntactic sugar for constrained version.
    Definition BetweenFactor.h:166
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BetweenFactor.h:174
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,729 +1,278 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularImplicitSchurFactor.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 │ │ │ │ │ -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: │ │ │ │ │ -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; │ │ │ │ │ +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> │ │ │ │ │ +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 │ │ │ │ │ +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 typedef VALUE T; │ │ │ │ │ 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: │ │ │ │ │ +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 _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 } │ │ │ │ │ +_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 {} │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -_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 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; │ │ │ │ │ +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 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. │ │ │ │ │ +_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) │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +_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_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_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_:_:_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_:_:_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_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_:_:_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() │ │ │ │ │ +_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_:_:_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 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 │ │ │ │ │ +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 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. │ │ │ │ │ +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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/dataset.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,170 +95,152 @@ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ Typedefs | │ │ │ │ +Enumerations | │ │ │ │ Functions
    │ │ │ │ -
    dataset.cpp File Reference
    │ │ │ │ +
    dataset.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    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 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 >
     
    │ │ │ │ +typedef std::pair< size_t, Pose2gtsam::IndexedPose
     Return type for auxiliary functions.
     
    │ │ │ │ +typedef std::pair< size_t, Point2gtsam::IndexedLandmark
     
    │ │ │ │ +typedef std::pair< std::pair< size_t, size_t >, Pose2gtsam::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 >
    map< size_t, T > gtsam::parseToMap (const string &filename, Parser< pair< size_t, T > > parse, size_t maxIndex)
     
    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< IndexedPosegtsam::parseVertexPose (std::istream &is, const std::string &tag)
     Parse TORO/G2O vertex "id x y yaw".
     
    │ │ │ │ -template<>
    GTSAM_EXPORT std::map< size_t, Pose2gtsam::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, Point2gtsam::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< Samplergtsam::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)
     
    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.
     
    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, Pose3gtsam::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, Point3gtsam::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
    │ │ │ │ +
    Author
    Ni Kai
    │ │ │ │
    │ │ │ │ Luca Carlone
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ Varun Agrawal
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,188 +1,164 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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_. │ │ │ │ │ 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)> │ │ │ │ │ + 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  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 > │ │ │ │ │ + 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. │ │ │ │ │ + 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 │ │ │ │ │ - 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) │ │ │ │ │ + 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 ********** │ │ │ │ │ utility functions for loading datasets │ │ │ │ │ Date │ │ │ │ │ Jan 22, 2010 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Ni Kai │ │ │ │ │ Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _d_a_t_a_s_e_t_._c_p_p │ │ │ │ │ + * _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/a01247.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,13 +1,30 @@ │ │ │ │ │ var a01247 = [ │ │ │ │ │ + ["GraphAndValues", "a01247.html#abcc5d1ec05ddc954d5de28929acabbdf", null], │ │ │ │ │ + ["IndexedPose", "a01247.html#a6ab764d13f9aaeafc2dc60763b05ec0e", null], │ │ │ │ │ + ["KernelFunctionType", "a01247.html#a8be373f34edc0a9d28b1bfab5dd62ba0", [ │ │ │ │ │ + ["KernelFunctionTypeNONE", "a01247.html#a8be373f34edc0a9d28b1bfab5dd62ba0a406af886b3b4b113dfe666c8ad95e38b", null], │ │ │ │ │ + ["KernelFunctionTypeHUBER", "a01247.html#a8be373f34edc0a9d28b1bfab5dd62ba0a37cef4c071c8c4f77f31fa617e91da53", null], │ │ │ │ │ + ["KernelFunctionTypeTUKEY", "a01247.html#a8be373f34edc0a9d28b1bfab5dd62ba0aacc5d659fca66977c8fe6f341802fae9", null] │ │ │ │ │ + ]], │ │ │ │ │ + ["NoiseFormat", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18", [ │ │ │ │ │ + ["NoiseFormatG2O", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18ad0aaa5c51a45cb7c9151aae60a603209", null], │ │ │ │ │ + ["NoiseFormatTORO", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18a3790bc2c6f11cd7da84730b33173de5a", null], │ │ │ │ │ + ["NoiseFormatGRAPH", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18a1f8edeb25c58a249789556c686265a4c", null], │ │ │ │ │ + ["NoiseFormatCOV", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18a5450a16f3522214ccdfa23c461e7d05a", null], │ │ │ │ │ + ["NoiseFormatAUTO", "a01247.html#a90552b70a2da9c74595cae4b05c2ce18a2f9067b8f5bd2d0dd38c49326a4c070e", null] │ │ │ │ │ + ]], │ │ │ │ │ ["createRewrittenFileName", "a01247.html#adbe09e573771fd72b08069353d537725", null], │ │ │ │ │ ["findExampleDataFile", "a01247.html#aef971b2c69845e4fcce707306703c7b8", null], │ │ │ │ │ ["load2D", "a01247.html#a635ec096b94adc2b05f081cd07eb2c0f", null], │ │ │ │ │ - ["load2D", "a01247.html#a79f6cde1c018ea0f4ca3c7fc83123a14", null], │ │ │ │ │ + ["load2D", "a01247.html#a6f8d49b13ccc0e007a5e7572286d5854", null], │ │ │ │ │ ["load3D", "a01247.html#ac96511703a91071d7b2033976f9739c3", null], │ │ │ │ │ ["parseEdge", "a01247.html#abc8bb9db0893652e8b5e7b47782ec990", null], │ │ │ │ │ + ["parseFactors", "a01247.html#a8a2136a163e31faaefe2db498a9b9b43", null], │ │ │ │ │ + ["parseMeasurements", "a01247.html#a6eb4817b97658f00e426b753c52361ed", null], │ │ │ │ │ + ["parseVariables", "a01247.html#a683ab6a83b5b3e508e734a7089986838", 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] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ │ @@ -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/slam/GeneralSFMFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,63 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Enumerations
    │ │ │ │ -
    SmartFactorParams.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    GeneralSFMFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Collect common parameters for SmartProjection and SmartStereoProjection factors. │ │ │ │ +

    a general SFM factor with an unknown calibration │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::SmartProjectionParams
    class  gtsam::GeneralSFMFactor< CAMERA, LANDMARK >
     Non-linear factor for a constraint derived from a 2D measurement. 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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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

    │ │ │ │ -

    Collect common parameters for SmartProjection and SmartStereoProjection factors.

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Zsolt Kira
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +

    a general SFM factor with an unknown calibration

    │ │ │ │ +
    Date
    Dec 15, 2010
    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +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 | _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 │ │ │ │ │ +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 │ │ │ │ │ -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_:_:_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_<_ _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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ -factors. │ │ │ │ │ +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_F_a_c_t_o_r_P_a_r_a_m_s_._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/a01250.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,14 +1,5 @@ │ │ │ │ │ var a01250 = [ │ │ │ │ │ - ["gtsam::SmartProjectionParams", "a04932.html", "a04932"], │ │ │ │ │ - ["DegeneracyMode", "a01250.html#ac3d021280c8a8b84903a8ce81f73ea0f", [ │ │ │ │ │ - ["IGNORE_DEGENERACY", "a01250.html#ac3d021280c8a8b84903a8ce81f73ea0fa7377887fa83ae502adf7f477205c0273", null], │ │ │ │ │ - ["ZERO_ON_DEGENERACY", "a01250.html#ac3d021280c8a8b84903a8ce81f73ea0fa88da97b27cb213cdf714af781ae76b99", null], │ │ │ │ │ - ["HANDLE_INFINITY", "a01250.html#ac3d021280c8a8b84903a8ce81f73ea0fa0836b9ba9572539be631139b0785a200", null] │ │ │ │ │ - ]], │ │ │ │ │ - ["LinearizationMode", "a01250.html#adf2020f3f6087064dde501c99794aac3", [ │ │ │ │ │ - ["HESSIAN", "a01250.html#adf2020f3f6087064dde501c99794aac3a94d642727a23c8b5a1b59b8f74553e80", null], │ │ │ │ │ - ["IMPLICIT_SCHUR", "a01250.html#adf2020f3f6087064dde501c99794aac3a7a3f02bec869fc563591ecb84ea2eb7a", null], │ │ │ │ │ - ["JACOBIAN_Q", "a01250.html#adf2020f3f6087064dde501c99794aac3a672276bd72f64f6b462004c89c57297b", null], │ │ │ │ │ - ["JACOBIAN_SVD", "a01250.html#adf2020f3f6087064dde501c99794aac3aca6b517d49e41094c865c321ddff5b81", 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/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/GeneralSFMFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,157 +98,346 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SmartFactorParams.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
    │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
    │ │ │ │ -
    32};
    │ │ │ │ -
    │ │ │ │ -
    33
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
    │ │ │ │ -
    37};
    │ │ │ │ -
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    30#include <gtsam/base/concepts.h>
    │ │ │ │ +
    31#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    32#include <gtsam/base/Matrix.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/*
    │ │ │ │ -
    40 * Parameters for the smart (stereo) projection factors
    │ │ │ │ -
    41 */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    43
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    46
    │ │ │ │ -
    49 TriangulationParameters triangulation;
    │ │ │ │ - │ │ │ │ -
    52
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    58
    │ │ │ │ -
    59 // Constructor
    │ │ │ │ - │ │ │ │ -
    61 DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
    │ │ │ │ -
    62 bool verboseCheirality = false, double retriangulationTh = 1e-5) :
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    73
    │ │ │ │ +
    74public:
    │ │ │ │ +
    75
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    78
    │ │ │ │ +
    79 // shorthand for a smart pointer to a factor
    │ │ │ │ +
    80 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    81
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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) {}
    │ │ │ │ + │ │ │ │ +
    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 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
    │ │ │ │ -
    116 triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
    │ │ │ │ -
    117 }
    │ │ │ │ -
    118
    │ │ │ │ -
    119private:
    │ │ │ │ -
    120
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Functions for triangulation.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    212
    │ │ │ │ +
    213public:
    │ │ │ │ +
    214
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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) {}
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    2D Point
    │ │ │ │ +
    A binary JacobianFactor specialization that uses fixed matrix math for speed.
    │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    DegeneracyMode
    How to manage degeneracy.
    Definition SmartFactorParams.h:35
    │ │ │ │ -
    LinearizationMode
    SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStere...
    Definition SmartFactorParams.h:30
    │ │ │ │ -
    Definition triangulation.h:556
    │ │ │ │ -
    double dynamicOutlierRejectionThreshold
    If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
    Definition triangulation.h:573
    │ │ │ │ -
    double rankTolerance
    threshold to decide whether triangulation is result.degenerate
    Definition triangulation.h:558
    │ │ │ │ -
    double landmarkDistanceThreshold
    if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
    Definition triangulation.h:566
    │ │ │ │ -
    bool enableEPI
    if set to true, will refine triangulation using LM
    Definition triangulation.h:560
    │ │ │ │ -
    Definition SmartFactorParams.h:42
    │ │ │ │ -
    bool throwCheirality
    If true, re-throws Cheirality exceptions (default: false)
    Definition SmartFactorParams.h:55
    │ │ │ │ -
    LinearizationMode linearizationMode
    How to linearize the factor.
    Definition SmartFactorParams.h:44
    │ │ │ │ -
    double retriangulationThreshold
    threshold to decide whether to re-triangulate
    Definition SmartFactorParams.h:50
    │ │ │ │ -
    DegeneracyMode degeneracyMode
    How to linearize the factor.
    Definition SmartFactorParams.h:45
    │ │ │ │ -
    bool verboseCheirality
    If true, prints text for Cheirality exceptions (default: false)
    Definition SmartFactorParams.h:56
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition SmartFactorParams.h:122
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Definition CalibratedCamera.h:32
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition GeneralSFMFactor.h:60
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
    Linearize using fixed-size matrices.
    Definition GeneralSFMFactor.h:138
    │ │ │ │ +
    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::NonlinearFactor::shared_ptr clone() const override
    Definition GeneralSFMFactor.h:101
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition GeneralSFMFactor.h:118
    │ │ │ │ +
    const Point2 measured() const
    return the measured
    Definition GeneralSFMFactor.h:177
    │ │ │ │ +
    GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
    Constructor.
    Definition GeneralSFMFactor.h:89
    │ │ │ │ +
    GeneralSFMFactor()
    default constructor
    Definition GeneralSFMFactor.h:93
    │ │ │ │ +
    GeneralSFMFactor< CAMERA, LANDMARK > This
    typedef for this object
    Definition GeneralSFMFactor.h:76
    │ │ │ │ +
    ~GeneralSFMFactor() override
    destructor
    Definition GeneralSFMFactor.h:98
    │ │ │ │ +
    NoiseModelFactorN< CAMERA, LANDMARK > Base
    typedef for the base class
    Definition GeneralSFMFactor.h:77
    │ │ │ │ +
    void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition GeneralSFMFactor.h:110
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition GeneralSFMFactor.h:183
    │ │ │ │ +
    Point2 measured_
    the 2D measurement
    Definition GeneralSFMFactor.h:72
    │ │ │ │ +
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition GeneralSFMFactor.h:204
    │ │ │ │ +
    GeneralSFMFactor2()
    default constructor
    Definition GeneralSFMFactor.h:232
    │ │ │ │ +
    ~GeneralSFMFactor2() override
    destructor
    Definition GeneralSFMFactor.h:234
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition GeneralSFMFactor.h:237
    │ │ │ │ +
    NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
    typedef for the base class
    Definition GeneralSFMFactor.h:217
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition GeneralSFMFactor.h:246
    │ │ │ │ +
    Point2 measured_
    the 2D measurement
    Definition GeneralSFMFactor.h:211
    │ │ │ │ +
    const Point2 measured() const
    return the measured
    Definition GeneralSFMFactor.h:280
    │ │ │ │ +
    PinholeCamera< CALIBRATION > Camera
    typedef for camera type
    Definition GeneralSFMFactor.h:216
    │ │ │ │ +
    GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
    Constructor.
    Definition GeneralSFMFactor.h:230
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition GeneralSFMFactor.h:254
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition GeneralSFMFactor.h:286
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,188 +1,477 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartFactorParams.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_/_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}; │ │ │ │ │ +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/* │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +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: │ │ │ │ │ +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_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 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. │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_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. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_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 │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_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_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_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_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_:_:_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 │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:122 │ │ │ │ │ +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 │ │ │ │ │ +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... │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01253_source.html │ │ │ │ @@ -114,15 +114,15 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    25#pragma once
    │ │ │ │
    26
    │ │ │ │ - │ │ │ │ + │ │ │ │
    28
    │ │ │ │
    29namespace gtsam {
    │ │ │ │
    51template <class CAMERA>
    │ │ │ │
    │ │ │ │ │ │ │ │
    53 private:
    │ │ │ │ │ │ │ │ @@ -391,15 +391,15 @@ │ │ │ │
    368template <class CAMERA>
    │ │ │ │
    │ │ │ │ │ │ │ │
    370 : public Testable<SmartProjectionRigFactor<CAMERA> > {};
    │ │ │ │
    │ │ │ │
    371
    │ │ │ │
    372} // namespace gtsam
    │ │ │ │ -
    Smart factor on cameras (pose + calibration)
    │ │ │ │ +
    Smart factor on cameras (pose + calibration)
    │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    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/a01256.html │ │ │ │ @@ -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/slam/EssentialMatrixConstraint.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,38 +96,41 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    AntiFactor.h File Reference
    │ │ │ │ +
    EssentialMatrixConstraint.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::AntiFactor
     A class for downdating an existing factor from a graph. 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Stephen Williams
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Pablo Alcantarilla
    │ │ │ │ +
    Date
    Jan 5, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +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 │ │ │ │ │ -AntiFactor.h File Reference │ │ │ │ │ +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_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ -  A class for downdating an existing factor from a graph. _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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Pablo Alcantarilla │ │ │ │ │ + Date │ │ │ │ │ + Jan 5, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _A_n_t_i_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/a01256_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,134 +98,127 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    AntiFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    32
    │ │ │ │ -
    33 private:
    │ │ │ │ -
    34
    │ │ │ │ -
    35 typedef AntiFactor This;
    │ │ │ │ -
    36 typedef NonlinearFactor Base;
    │ │ │ │ -
    37 typedef NonlinearFactor::shared_ptr sharedFactor;
    │ │ │ │ -
    38
    │ │ │ │ -
    39 sharedFactor factor_;
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/geometry/EssentialMatrix.h>
    │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    │ │ │ │ +
    30class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
    │ │ │ │ +
    31
    │ │ │ │ +
    32private:
    │ │ │ │ +
    33
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    36
    │ │ │ │ +
    37 EssentialMatrix measuredE_;
    │ │ │ │ +
    39public:
    │ │ │ │
    40
    │ │ │ │ -
    41 public:
    │ │ │ │ -
    42
    │ │ │ │ -
    43 // shorthand for a smart pointer to a factor
    │ │ │ │ -
    44 typedef boost::shared_ptr<AntiFactor> shared_ptr;
    │ │ │ │ -
    45
    │ │ │ │ - │ │ │ │ -
    48
    │ │ │ │ -
    50 AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {}
    │ │ │ │ -
    51
    │ │ │ │ -
    52 ~AntiFactor() override {}
    │ │ │ │ -
    53
    │ │ │ │ +
    41 // shorthand for a smart pointer to a factor
    │ │ │ │ +
    42 typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
    │ │ │ │ +
    43
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46 }
    │ │ │ │ +
    │ │ │ │ +
    47
    │ │ │ │
    │ │ │ │ -
    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 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(); }
    │ │ │ │ + │ │ │ │ +
    56 const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
    │ │ │ │ +
    57 Base(model, key1, key2), measuredE_(measuredE) {
    │ │ │ │ +
    58 }
    │ │ │ │ +
    │ │ │ │ +
    59
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    105};
    │ │ │ │ +
    │ │ │ │ +
    106// \class EssentialMatrixConstraint
    │ │ │ │ +
    107
    │ │ │ │ +
    108}
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    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
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
    Check if two factors are equal.
    Definition NonlinearFactor.cpp:47
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    A class for downdating an existing factor from a graph.
    Definition AntiFactor.h:31
    │ │ │ │ -
    AntiFactor(NonlinearFactor::shared_ptr factor)
    constructor - Creates the equivalent AntiFactor from an existing factor
    Definition AntiFactor.h:50
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals
    Definition AntiFactor.h:68
    │ │ │ │ -
    AntiFactor()
    default constructor - only use for serialization
    Definition AntiFactor.h:47
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
    Linearize to a GaussianFactor.
    Definition AntiFactor.h:97
    │ │ │ │ -
    bool active(const Values &c) const override
    Checks whether this factor should be used based on a set of values.
    Definition AntiFactor.h:89
    │ │ │ │ -
    size_t dim() const override
    get the dimension of the factor (same as the original factor)
    Definition AntiFactor.h:83
    │ │ │ │ -
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    implement functions needed for Testable
    Definition AntiFactor.h:62
    │ │ │ │ -
    double error(const Values &c) const override
    implement functions needed to derive from Factor
    Definition AntiFactor.h:80
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AntiFactor.h:55
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition AntiFactor.h:110
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
    Definition EssentialMatrixConstraint.h:30
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixConstraint.h:64
    │ │ │ │ +
    const EssentialMatrix & measured() const
    return the measured
    Definition EssentialMatrixConstraint.h:86
    │ │ │ │ +
    boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
    The measurement is an essential matrix.
    Definition EssentialMatrixConstraint.h:42
    │ │ │ │ +
    EssentialMatrixConstraint()
    default constructor - only use for serialization
    Definition EssentialMatrixConstraint.h:45
    │ │ │ │ +
    EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixConstraint.h:55
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,173 +1,159 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AntiFactor.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_/_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_; │ │ │ │ │ +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; │ │ │ │ │ +36 │ │ │ │ │ +37 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x measuredE_; │ │ │ │ │ +39public: │ │ │ │ │ 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 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(); } │ │ │ │ │ +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_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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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_:_:_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_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_:_:_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 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 │ │ │ │ │ -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 │ │ │ │ │ +_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 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 AntiFactor.h:110 │ │ │ │ │ +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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _A_n_t_i_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/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/AntiFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,49 +96,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SmartProjectionFactor.h File Reference
    │ │ │ │ +
    AntiFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Smart factor on cameras (pose + 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...
     
    struct  gtsam::traits< SmartProjectionFactor< 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 cameras (pose + calibration)

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Zsolt Kira
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +
    Author
    Stephen Williams
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -SmartProjectionFactor.h File Reference │ │ │ │ │ -Smart factor on cameras (pose + calibration) _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_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_<_ _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_._._. │ │ │ │ │ +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 cameras (pose + calibration) │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Zsolt Kira │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + 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_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/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/AntiFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,539 +98,134 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SmartProjectionFactor.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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    16#pragma once
    │ │ │ │ +
    17
    │ │ │ │ +
    18#include <ostream>
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │
    24
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    27#include <gtsam/slam/dataset.h>
    │ │ │ │ -
    28
    │ │ │ │ -
    29#include <boost/optional.hpp>
    │ │ │ │ -
    30#include <boost/make_shared.hpp>
    │ │ │ │ -
    31#include <vector>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    32
    │ │ │ │ -
    33namespace gtsam {
    │ │ │ │ +
    33 private:
    │ │ │ │
    34
    │ │ │ │ -
    44template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46
    │ │ │ │ -
    47public:
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │
    48
    │ │ │ │ -
    49private:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    52 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
    │ │ │ │ +
    50 AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {}
    │ │ │ │ +
    51
    │ │ │ │ +
    52 ~AntiFactor() override {}
    │ │ │ │
    53
    │ │ │ │ -
    54protected:
    │ │ │ │ -
    55
    │ │ │ │ - │ │ │ │ -
    60
    │ │ │ │ - │ │ │ │ -
    64 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
    │ │ │ │ - │ │ │ │ -
    67
    │ │ │ │ -
    68 public:
    │ │ │ │ -
    69
    │ │ │ │ -
    71 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    │ │ │ │ +
    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 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
    │ │ │ │ -
    74 typedef CAMERA Camera;
    │ │ │ │ - │ │ │ │ -
    76
    │ │ │ │ - │ │ │ │ +
    80 double error(const Values& c) const override { return -factor_->error(c); }
    │ │ │ │
    81
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    88 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ - │ │ │ │ -
    90 : Base(sharedNoiseModel),
    │ │ │ │ -
    91 params_(params),
    │ │ │ │ -
    92 result_(TriangulationResult::Degenerate()) {}
    │ │ │ │ -
    │ │ │ │ -
    93
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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) {
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    183 params_.triangulation);
    │ │ │ │ -
    184 return result_;
    │ │ │ │ -
    185 }
    │ │ │ │ -
    │ │ │ │ -
    186
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ - │ │ │ │ -
    352 }
    │ │ │ │ -
    │ │ │ │ -
    353
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    370 }
    │ │ │ │ -
    371 }
    │ │ │ │ -
    │ │ │ │ -
    372
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    381 return nonDegenerate;
    │ │ │ │ -
    382 }
    │ │ │ │ -
    │ │ │ │ -
    383
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    401 else
    │ │ │ │ -
    402 return Vector::Zero(cameras.size() * 2);
    │ │ │ │ -
    403 }
    │ │ │ │ -
    │ │ │ │ -
    404
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    412 boost::optional<Point3> externalPoint = boost::none) const {
    │ │ │ │ -
    413
    │ │ │ │ -
    414 if (externalPoint)
    │ │ │ │ -
    415 result_ = TriangulationResult(*externalPoint);
    │ │ │ │ -
    416 else
    │ │ │ │ - │ │ │ │ -
    418
    │ │ │ │ -
    419 if (result_)
    │ │ │ │ -
    420 // All good, just use version in base class
    │ │ │ │ - │ │ │ │ -
    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)) {
    │ │ │ │ - │ │ │ │ -
    436 } else { // else of active flag
    │ │ │ │ -
    437 return 0.0;
    │ │ │ │ -
    438 }
    │ │ │ │ -
    439 }
    │ │ │ │ -
    │ │ │ │ -
    440
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Functions for triangulation.
    │ │ │ │ -
    Collect common parameters for SmartProjection and SmartStereoProjection factors.
    │ │ │ │ -
    Base class to create smart factors on poses or cameras.
    │ │ │ │ -
    utility functions for loading datasets
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
    Check if two factors are equal.
    Definition NonlinearFactor.cpp:47
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    Base class for smart factors.
    Definition SmartFactorBase.h:50
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static const int Dim
    Camera dimension.
    Definition SmartFactorBase.h:60
    │ │ │ │ -
    virtual Cameras cameras(const Values &values) const
    Collect all cameras: important that in key order.
    Definition SmartFactorBase.h:162
    │ │ │ │ -
    double totalReprojectionError(const Cameras &cameras, const POINT &point) const
    Calculate the error of the factor.
    Definition SmartFactorBase.h:267
    │ │ │ │ -
    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
    │ │ │ │ -
    ZVector measured_
    Measurements for each of the m views.
    Definition SmartFactorBase.h:79
    │ │ │ │ -
    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
    │ │ │ │ -
    void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
    Whiten the Jacobians computed by computeJacobians using noiseModel_.
    Definition SmartFactorBase.h:347
    │ │ │ │ -
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
    Return Jacobians as JacobianFactorSVD.
    Definition SmartFactorBase.h:386
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartFactorBase.h:174
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartFactorBase.h:187
    │ │ │ │ -
    Definition SmartFactorParams.h:42
    │ │ │ │ -
    LinearizationMode linearizationMode
    How to linearize the factor.
    Definition SmartFactorParams.h:44
    │ │ │ │ -
    double retriangulationThreshold
    threshold to decide whether to re-triangulate
    Definition SmartFactorParams.h:50
    │ │ │ │ -
    DegeneracyMode degeneracyMode
    How to linearize the factor.
    Definition SmartFactorParams.h:45
    │ │ │ │ -
    SmartProjectionFactor: triangulates point and keeps an estimate of it around.
    Definition SmartProjectionFactor.h:45
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
    Create JacobianFactorQ factor, takes values.
    Definition SmartProjectionFactor.h:261
    │ │ │ │ -
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
    Different (faster) way to compute a JacobianFactorSVD factor.
    Definition SmartProjectionFactor.h:267
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
    Linearize to a Hessianfactor.
    Definition SmartProjectionFactor.h:277
    │ │ │ │ -
    bool isOutlier() const
    return the outlier state
    Definition SmartProjectionFactor.h:462
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartProjectionFactor.h:103
    │ │ │ │ -
    bool triangulateAndComputeE(Matrix &E, const Values &values) const
    Triangulate and compute derivative of error with respect to point.
    Definition SmartProjectionFactor.h:349
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
    Linearize to Gaussian Factor.
    Definition SmartProjectionFactor.h:299
    │ │ │ │ -
    bool isFarPoint() const
    return the farPoint state
    Definition SmartProjectionFactor.h:465
    │ │ │ │ -
    TriangulationResult result_
    result from triangulateSafe
    Definition SmartProjectionFactor.h:63
    │ │ │ │ -
    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
    │ │ │ │ -
    TriangulationResult triangulateSafe(const Cameras &cameras) const
    Call gtsam::triangulateSafe iff we need to re-triangulate.
    Definition SmartProjectionFactor.h:174
    │ │ │ │ -
    ~SmartProjectionFactor() override
    Virtual destructor.
    Definition SmartProjectionFactor.h:95
    │ │ │ │ -
    double error(const Values &values) const override
    Calculate total reprojection error.
    Definition SmartProjectionFactor.h:433
    │ │ │ │ -
    bool isValid() const
    Is result valid?
    Definition SmartProjectionFactor.h:453
    │ │ │ │ -
    std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
    current triangulation poses
    Definition SmartProjectionFactor.h:65
    │ │ │ │ -
    bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
    Triangulate and compute derivative of error with respect to point.
    Definition SmartProjectionFactor.h:338
    │ │ │ │ -
    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
    │ │ │ │ -
    double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
    Calculate the error of the factor.
    Definition SmartProjectionFactor.h:411
    │ │ │ │ -
    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
    │ │ │ │ -
    bool isDegenerate() const
    return the degenerate state
    Definition SmartProjectionFactor.h:456
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartProjectionFactor.h:115
    │ │ │ │ -
    CAMERA Camera
    shorthand for a set of cameras
    Definition SmartProjectionFactor.h:74
    │ │ │ │ -
    boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
    Create JacobianFactorQ factor.
    Definition SmartProjectionFactor.h:251
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition SmartProjectionFactor.h:470
    │ │ │ │ -
    TriangulationResult point(const Values &values) const
    COMPUTE the landmark.
    Definition SmartProjectionFactor.h:447
    │ │ │ │ -
    SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
    Constructor.
    Definition SmartProjectionFactor.h:87
    │ │ │ │ -
    bool isPointBehindCamera() const
    return the cheirality status flag
    Definition SmartProjectionFactor.h:459
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
    linearize
    Definition SmartProjectionFactor.h:329
    │ │ │ │ -
    bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
    takes values
    Definition SmartProjectionFactor.h:385
    │ │ │ │ -
    Vector reprojectionErrorAfterTriangulation(const Values &values) const
    Calculate vector of re-projection errors, before applying noise model.
    Definition SmartProjectionFactor.h:396
    │ │ │ │ -
    TriangulationResult point() const
    return the landmark
    Definition SmartProjectionFactor.h:442
    │ │ │ │ -
    bool triangulateForLinearize(const Cameras &cameras) const
    Possibly re-triangulate before calculating Jacobians.
    Definition SmartProjectionFactor.h:193
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition SmartProjectionFactor.h:71
    │ │ │ │ -
    SmartProjectionFactor()
    Default constructor, only for serialization.
    Definition SmartProjectionFactor.h:80
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
    Linearize to Gaussian Factor.
    Definition SmartProjectionFactor.h:321
    │ │ │ │ - │ │ │ │ +
    A class for downdating an existing factor from a graph.
    Definition AntiFactor.h:31
    │ │ │ │ +
    AntiFactor(NonlinearFactor::shared_ptr factor)
    constructor - Creates the equivalent AntiFactor from an existing factor
    Definition AntiFactor.h:50
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals
    Definition AntiFactor.h:68
    │ │ │ │ +
    AntiFactor()
    default constructor - only use for serialization
    Definition AntiFactor.h:47
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
    Linearize to a GaussianFactor.
    Definition AntiFactor.h:97
    │ │ │ │ +
    bool active(const Values &c) const override
    Checks whether this factor should be used based on a set of values.
    Definition AntiFactor.h:89
    │ │ │ │ +
    size_t dim() const override
    get the dimension of the factor (same as the original factor)
    Definition AntiFactor.h:83
    │ │ │ │ +
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    implement functions needed for Testable
    Definition AntiFactor.h:62
    │ │ │ │ +
    double error(const Values &c) const override
    implement functions needed to derive from Factor
    Definition AntiFactor.h:80
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AntiFactor.h:55
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition AntiFactor.h:110
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,743 +1,173 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartProjectionFactor.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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ +33 private: │ │ │ │ │ 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: │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 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 │ │ │ │ │ -_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() {} │ │ │ │ │ +_8_0 double _e_r_r_o_r(const _V_a_l_u_e_s& c) const override { return -factor_->error(c); } │ │ │ │ │ 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; │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -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 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 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 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 │ │ │ │ │ -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 │ │ │ │ │ +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 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 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_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/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/OrientedPlane3Factor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,464 +98,113 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SmartFactorBase.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
    │ │ │ │ -
    22#pragma once
    │ │ │ │ -
    23
    │ │ │ │ -
    24#include <gtsam/slam/JacobianFactorQ.h>
    │ │ │ │ -
    25#include <gtsam/slam/JacobianFactorSVD.h>
    │ │ │ │ - │ │ │ │ -
    27
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    12
    │ │ │ │ +
    13namespace gtsam {
    │ │ │ │ +
    14
    │ │ │ │ +
    │ │ │ │ +
    18class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
    │ │ │ │ +
    19 protected:
    │ │ │ │ +
    20 OrientedPlane3 measured_p_;
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23 public:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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_;
    │ │ │ │ + │ │ │ │ +
    56
    │ │ │ │ +
    57 public:
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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};
    │ │ │ │ +
    │ │ │ │ +
    78
    │ │ │ │ +
    79} // gtsam
    │ │ │ │
    80
    │ │ │ │ -
    81 boost::optional<Pose3>
    │ │ │ │ - │ │ │ │ -
    83
    │ │ │ │ -
    84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
    │ │ │ │ -
    85 mutable FBlocks Fs;
    │ │ │ │ -
    86
    │ │ │ │ -
    87 public:
    │ │ │ │ - │ │ │ │ -
    89
    │ │ │ │ -
    91 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    92
    │ │ │ │ - │ │ │ │ -
    95
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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.
    │ │ │ │ - │ │ │ │ -
    232
    │ │ │ │ -
    233 return error;
    │ │ │ │ -
    234 }
    │ │ │ │ -
    │ │ │ │ -
    235
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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> > //
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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{
    │ │ │ │ - │ │ │ │ -
    411 return *body_P_sensor_;
    │ │ │ │ -
    412 else
    │ │ │ │ -
    413 return Pose3(); // if unspecified, the transformation is the identity
    │ │ │ │ -
    414 }
    │ │ │ │ -
    415
    │ │ │ │ -
    416private:
    │ │ │ │ -
    417
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Base class to create smart factors on poses or cameras.
    │ │ │ │ -
    HessianFactor class with constant sized blocks.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ -
    A subclass of GaussianFactor specialized to structureless SFM.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
    SVD computes economy SVD A=U*S*V'.
    Definition Matrix.cpp:560
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
    Definition OrientedPlane3.h:36
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Pose3 inverse() const
    inverse transformation with derivatives
    Definition Pose3.cpp:49
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
    Definition NoiseModel.h:516
    │ │ │ │ -
    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
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
    Check if two factors are equal.
    Definition NonlinearFactor.cpp:47
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactor.cpp:37
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    Base class for smart factors.
    Definition SmartFactorBase.h:50
    │ │ │ │ -
    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
    │ │ │ │ -
    void add(const Z &measured, const Key &key)
    Add a new measurement and pose/camera key.
    Definition SmartFactorBase.h:126
    │ │ │ │ -
    ~SmartFactorBase() override
    Virtual destructor, subclasses from NonlinearFactor.
    Definition SmartFactorBase.h:118
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    SharedIsotropic noiseModel_
    As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
    Definition SmartFactorBase.h:72
    │ │ │ │ -
    static const int Dim
    Camera dimension.
    Definition SmartFactorBase.h:60
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual Cameras cameras(const Values &values) const
    Collect all cameras: important that in key order.
    Definition SmartFactorBase.h:162
    │ │ │ │ -
    static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
    Create BIG block-diagonal matrix F from Fblocks.
    Definition SmartFactorBase.h:400
    │ │ │ │ -
    double totalReprojectionError(const Cameras &cameras, const POINT &point) const
    Calculate the error of the factor.
    Definition SmartFactorBase.h:267
    │ │ │ │ -
    void add(const ZVector &measurements, const KeyVector &cameraKeys)
    Add a bunch of measurements, together with the camera keys.
    Definition SmartFactorBase.h:136
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    ZVector measured_
    Measurements for each of the m views.
    Definition SmartFactorBase.h:79
    │ │ │ │ -
    GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor.
    Definition SmartFactorBase.h:91
    │ │ │ │ -
    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
    │ │ │ │ -
    SmartFactorBase()
    Default Constructor, for serialization.
    Definition SmartFactorBase.h:97
    │ │ │ │ -
    void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
    Whiten the Jacobians computed by computeJacobians using noiseModel_.
    Definition SmartFactorBase.h:347
    │ │ │ │ -
    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
    │ │ │ │ -
    void add(const SFM_TRACK &trackToAdd)
    Add an entire SfM_track (collection of cameras observing a single point).
    Definition SmartFactorBase.h:148
    │ │ │ │ -
    const ZVector & measured() const
    Return the 2D measurements (ZDim, in general).
    Definition SmartFactorBase.h:159
    │ │ │ │ -
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
    Return Jacobians as JacobianFactorSVD.
    Definition SmartFactorBase.h:386
    │ │ │ │ -
    size_t dim() const override
    Return the dimension (number of rows!) of the factor.
    Definition SmartFactorBase.h:156
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition SmartFactorBase.h:419
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartFactorBase.h:174
    │ │ │ │ -
    boost::optional< Pose3 > body_P_sensor_
    Pose of the camera in the body frame.
    Definition SmartFactorBase.h:82
    │ │ │ │ -
    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
    │ │ │ │ -
    static const int ZDim
    Measurement dimension.
    Definition SmartFactorBase.h:61
    │ │ │ │ -
    static Matrix PointCov(const Matrix &E)
    Computes Point Covariance P from the "point Jacobian" E.
    Definition SmartFactorBase.h:274
    │ │ │ │ -
    CameraSet< CAMERA > Cameras
    The CameraSet data structure is used to refer to a set of cameras.
    Definition SmartFactorBase.h:94
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartFactorBase.h:187
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Factor to measure a planar landmark from a given pose.
    Definition OrientedPlane3Factor.h:18
    │ │ │ │ +
    OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
    Constructor with measured plane (a,b,c,d) coefficients.
    Definition OrientedPlane3Factor.h:36
    │ │ │ │ +
    OrientedPlane3Factor()
    Constructor.
    Definition OrientedPlane3Factor.h:25
    │ │ │ │ +
    Definition OrientedPlane3Factor.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    NoiseModelFactorN< OrientedPlane3 > Base
    measured plane parameters
    Definition OrientedPlane3Factor.h:55
    │ │ │ │ +
    OrientedPlane3DirectionPrior()
    Constructor.
    Definition OrientedPlane3Factor.h:60
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,629 +1,139 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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) │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ -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__; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ 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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_B_a_s_e_._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/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/KarcherMeanFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,407 +98,111 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EssentialMatrixFactor.h
    │ │ │ │ +
    KarcherMeanFactor-inl.h
    │ │ │ │
    │ │ │ │
    │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27#include <iostream>
    │ │ │ │ -
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    │ │ │ │ -
    34class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
    │ │ │ │ -
    35 Vector3 vA_, vB_;
    │ │ │ │ -
    36
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    39
    │ │ │ │ -
    40 public:
    │ │ │ │ +
    13 * @file KarcherMeanFactor.cpp
    │ │ │ │ +
    14 * @author Frank Dellaert
    │ │ │ │ +
    15 * @date March 2019
    │ │ │ │ +
    16 */
    │ │ │ │ +
    17
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <gtsam/slam/KarcherMeanFactor.h>
    │ │ │ │ +
    23
    │ │ │ │ +
    24using namespace std;
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    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>
    │ │ │ │
    │ │ │ │ -
    49 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
    │ │ │ │ -
    50 const SharedNoiseModel& model)
    │ │ │ │ -
    51 : Base(model, key) {
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    102};
    │ │ │ │ -
    │ │ │ │ -
    103
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    109 : public NoiseModelFactorN<EssentialMatrix, double> {
    │ │ │ │ -
    110 Point3 dP1_;
    │ │ │ │ -
    111 Point2 pn_;
    │ │ │ │ -
    112 double f_;
    │ │ │ │ -
    113
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    223};
    │ │ │ │ -
    │ │ │ │ -
    224// EssentialMatrixFactor2
    │ │ │ │ -
    225
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    306};
    │ │ │ │ -
    │ │ │ │ -
    307// EssentialMatrixFactor3
    │ │ │ │ -
    308
    │ │ │ │ -
    322template <class CALIBRATION>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    324 : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
    │ │ │ │ -
    325 private:
    │ │ │ │ -
    326 Point2 pA_, pB_;
    │ │ │ │ -
    327
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    406};
    │ │ │ │ -
    │ │ │ │ -
    407// EssentialMatrixFactor4
    │ │ │ │ -
    408
    │ │ │ │ -
    409} // namespace gtsam
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    static Vector3 Homogeneous(const Point2 &p)
    Static function to convert Point2 to homogeneous coordinates.
    Definition EssentialMatrix.h:34
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    Factor that evaluates epipolar error p'Ep for given essential matrix.
    Definition EssentialMatrixFactor.h:34
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:76
    │ │ │ │ -
    EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:49
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:82
    │ │ │ │ -
    EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
    Constructor.
    Definition EssentialMatrixFactor.h:66
    │ │ │ │ -
    Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 1D vector
    Definition EssentialMatrixFactor.h:92
    │ │ │ │ -
    Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
    Definition EssentialMatrixFactor.h:109
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:154
    │ │ │ │ -
    EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
    Constructor.
    Definition EssentialMatrixFactor.h:144
    │ │ │ │ -
    EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:126
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:160
    │ │ │ │ -
    Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
    Definition EssentialMatrixFactor.h:231
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:273
    │ │ │ │ -
    EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:247
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:267
    │ │ │ │ -
    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
    │ │ │ │ -
    Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (...
    Definition EssentialMatrixFactor.h:324
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:355
    │ │ │ │ -
    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
    │ │ │ │ -
    EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:344
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:349
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
    Construct from given keys.
    Definition KarcherMeanFactor-inl.h:60
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,488 +1,117 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EssentialMatrixFactor.h │ │ │ │ │ +KarcherMeanFactor-inl.h │ │ │ │ │ 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 │ │ │ │ │ -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_; │ │ │ │ │ -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 │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_l_a_m │ │ │ │ │ - * EEsssseennttiiaallMMaattrriixxFFaaccttoorr..hh │ │ │ │ │ + * 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/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/InitializePose3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,54 +96,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros
    │ │ │ │ -
    BetweenFactor.h File Reference
    │ │ │ │ +Typedefs
    │ │ │ │ +
    InitializePose3.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ +

    Initialize Pose3 in a factor graph. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    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...
    struct  gtsam::InitializePose3
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ -Macros

    │ │ │ │ -#define BETWEENFACTOR_VISIBILITY   GTSAM_EXPORT
     

    │ │ │ │ +Typedefs

    │ │ │ │ +typedef std::map< Key, std::vector< size_t > > gtsam::KeyVectorMap
     
    │ │ │ │ +typedef std::map< Key, Rot3gtsam::KeyRotMap
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Frank Dellaert, Viorela Ila
    │ │ │ │ +

    Initialize Pose3 in a factor graph.

    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    August, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +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 │ │ │ │ │ -BetweenFactor.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 │ │ │ │ │ +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 │ │ │ │ │ - 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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  BBEETTWWEEEENNFFAACCTTOORR__VVIISSIIBBIILLIITTYY   GTSAM_EXPORT │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef std::map< _K_e_y, std::vector< size_t > >  ggttssaamm::::KKeeyyVVeeccttoorrMMaapp │ │ │ │ │ +  │ │ │ │ │ + typedef std::map< _K_e_y, _R_o_t_3 >  ggttssaamm::::KKeeyyRRoottMMaapp │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Initialize Pose3 in a factor graph. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Viorela Ila │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + August, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _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.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a01274 = [ │ │ │ │ │ - ["gtsam::traits< BetweenFactor< VALUE > >", "a04768.html", null], │ │ │ │ │ - ["gtsam::BetweenConstraint< VALUE >", "a04772.html", "a04772"], │ │ │ │ │ - ["gtsam::traits< BetweenConstraint< VALUE > >", "a04776.html", null] │ │ │ │ │ + ["gtsam::InitializePose3", "a04856.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/InitializePose3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,215 +98,97 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BetweenFactor.h
    │ │ │ │ +
    InitializePose3.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
    │ │ │ │ -
    16#pragma once
    │ │ │ │ -
    17
    │ │ │ │ -
    18#include <ostream>
    │ │ │ │ -
    19
    │ │ │ │ -
    20#include <gtsam/base/Testable.h>
    │ │ │ │ -
    21#include <gtsam/base/Lie.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
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/geometry/Rot3.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │ +
    29#include <map>
    │ │ │ │ +
    30#include <vector>
    │ │ │ │
    31
    │ │ │ │
    32namespace gtsam {
    │ │ │ │
    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:
    │ │ │ │ -
    47
    │ │ │ │ -
    48 typedef VALUE T;
    │ │ │ │ -
    49
    │ │ │ │ -
    50 private:
    │ │ │ │ -
    51
    │ │ │ │ -
    52 typedef BetweenFactor<VALUE> This;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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 {}
    │ │ │ │ -
    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))); }
    │ │ │ │ -
    │ │ │ │ +
    34typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
    │ │ │ │ +
    35typedef std::map<Key, Rot3> KeyRotMap;
    │ │ │ │ +
    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);
    │ │ │ │ +
    76
    │ │ │ │ +
    81 static Values initializeOrientations(const NonlinearFactorGraph& graph);
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    Graph algorithm using boost library.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Lie Group Concept.
    Definition Lie.h:260
    │ │ │ │ -
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    A class for a measurement predicted by "between(config[key1],config[key2])".
    Definition BetweenFactor.h:40
    │ │ │ │ -
    BetweenFactor()
    default constructor - only use for serialization
    Definition BetweenFactor.h:66
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    assert equality up to a tolerance
    Definition BetweenFactor.h:98
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition BetweenFactor.h:79
    │ │ │ │ -
    boost::shared_ptr< BetweenFactor > shared_ptr
    The measurement.
    Definition BetweenFactor.h:60
    │ │ │ │ -
    const VALUE & measured() const
    return the measurement
    Definition BetweenFactor.h:128
    │ │ │ │ -
    BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition BetweenFactor.h:69
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BetweenFactor.h:136
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print with optional string
    Definition BetweenFactor.h:87
    │ │ │ │ -
    Binary between constraint - forces between to a given value This constraint requires the underlying t...
    Definition BetweenFactor.h:161
    │ │ │ │ -
    BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
    Syntactic sugar for constrained version.
    Definition BetweenFactor.h:166
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BetweenFactor.h:174
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    Definition InitializePose3.h:37
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,278 +1,106 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BetweenFactor.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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> │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -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 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 {} │ │ │ │ │ -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))); } │ │ │ │ │ +34typedef std::map > KeyVectorMap; │ │ │ │ │ +35typedef std::map KeyRotMap; │ │ │ │ │ +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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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) │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:317 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ -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 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 │ │ │ │ │ +_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 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/a01277_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,170 +98,587 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    RotateFactor.h
    │ │ │ │ +
    RegularImplicitSchurFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -
    1/*
    │ │ │ │ -
    2 * @file RotateFactor.cpp
    │ │ │ │ -
    3 * @brief RotateFactor class
    │ │ │ │ -
    4 * @author Frank Dellaert
    │ │ │ │ -
    5 * @date December 17, 2013
    │ │ │ │ -
    6 */
    │ │ │ │ -
    7
    │ │ │ │ +Go to the documentation of this file.
    1
    │ │ │ │
    8#pragma once
    │ │ │ │
    9
    │ │ │ │ - │ │ │ │ -
    11#include <gtsam/geometry/Rot3.h>
    │ │ │ │ -
    12
    │ │ │ │ -
    13namespace gtsam {
    │ │ │ │ -
    14
    │ │ │ │ -
    │ │ │ │ -
    23class RotateFactor: public NoiseModelFactorN<Rot3> {
    │ │ │ │ -
    24
    │ │ │ │ -
    25 Point3 p_, z_;
    │ │ │ │ -
    26
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    13
    │ │ │ │ +
    14#include <iosfwd>
    │ │ │ │ +
    15#include <map>
    │ │ │ │ +
    16#include <string>
    │ │ │ │ +
    17#include <vector>
    │ │ │ │ +
    18
    │ │ │ │ +
    19namespace gtsam {
    │ │ │ │ +
    20
    │ │ │ │ +
    38template<class CAMERA>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    40
    │ │ │ │ +
    41public:
    │ │ │ │ + │ │ │ │ +
    43 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    44
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    67 }
    │ │ │ │ +
    │ │ │ │
    68
    │ │ │ │ -
    69 Unit3 i_p_, c_z_;
    │ │ │ │
    70
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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");
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ - │ │ │ │ -
    117};
    │ │ │ │ -
    │ │ │ │ -
    118} // namespace gtsam
    │ │ │ │ -
    119
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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 }
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    257 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
    │ │ │ │ + │ │ │ │ +
    259 throw std::runtime_error(
    │ │ │ │ +
    260 "RegularImplicitSchurFactor::clone non implemented");
    │ │ │ │ +
    261 }
    │ │ │ │ +
    │ │ │ │ +
    262
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    264 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    509
    │ │ │ │ +
    510template<class CAMERA>
    │ │ │ │ + │ │ │ │ +
    512
    │ │ │ │ +
    513// traits
    │ │ │ │ +
    │ │ │ │ +
    514template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
    │ │ │ │ +
    515 RegularImplicitSchurFactor<CAMERA> > {
    │ │ │ │ +
    516};
    │ │ │ │ +
    │ │ │ │ +
    517
    │ │ │ │ +
    518}
    │ │ │ │ +
    519
    │ │ │ │ +
    Base class to create smart factors on poses or cameras.
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    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
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Vector.
    Definition Unit3.cpp:151
    │ │ │ │ -
    void print(const std::string &s=std::string()) const
    The print fuction.
    Definition Unit3.cpp:164
    │ │ │ │ -
    Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
    Signed, vector-valued error between two directions.
    Definition Unit3.cpp:199
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    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
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    bool empty() const
    Whether the factor is empty (involves zero variables).
    Definition Factor.h:128
    │ │ │ │
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Whic...
    Definition RotateFactor.h:23
    │ │ │ │ -
    RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
    Constructor.
    Definition RotateFactor.h:33
    │ │ │ │ -
    Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 2D vector
    Definition RotateFactor.h:53
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RotateFactor.h:44
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition RotateFactor.h:39
    │ │ │ │ -
    Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than...
    Definition RotateFactor.h:67
    │ │ │ │ -
    Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 2D vector
    Definition RotateFactor.h:105
    │ │ │ │ -
    RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
    Constructor.
    Definition RotateFactor.h:77
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RotateFactor.h:96
    │ │ │ │ -
    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::NonlinearFactor::shared_ptr clone() const override
    Definition RotateFactor.h:91
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ +
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    VectorValues hessianDiagonal() const
    Return the diagonal of the Hessian for this factor.
    Definition GaussianFactor.cpp:35
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    iterator insert(const std::pair< Key, Vector > &key_value)
    Insert a vector value with key j.
    Definition VectorValues.cpp:91
    │ │ │ │ +
    std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
    Emplace a vector value with key j.
    Definition VectorValues.h:185
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    RegularImplicitSchurFactor.
    Definition RegularImplicitSchurFactor.h:39
    │ │ │ │ +
    const Matrix E_
    The 2m*3 E Jacobian with respect to the point.
    Definition RegularImplicitSchurFactor.h:60
    │ │ │ │ +
    void projectError(const Error2s &e1, Error2s &e2) const
    Calculate corrected error Q*e = (I - E*P*E')*e.
    Definition RegularImplicitSchurFactor.h:357
    │ │ │ │ +
    GaussianFactor::shared_ptr clone() const override
    Clone a factor (make a deep copy)
    Definition RegularImplicitSchurFactor.h:256
    │ │ │ │ +
    void hessianDiagonalAdd(VectorValues &d) const override
    Add the diagonal of the Hessian for this factor to existing VectorValues.
    Definition RegularImplicitSchurFactor.h:174
    │ │ │ │ +
    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
    │ │ │ │ +
    RegularImplicitSchurFactor()
    Constructor.
    Definition RegularImplicitSchurFactor.h:66
    │ │ │ │ +
    const Vector b_
    2m-dimensional RHS vector
    Definition RegularImplicitSchurFactor.h:61
    │ │ │ │ +
    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
    │ │ │ │ +
    Eigen::Matrix< double, D, D > MatrixDD
    camera Hessian
    Definition RegularImplicitSchurFactor.h:55
    │ │ │ │ +
    void gradientAtZero(double *d) const override
    Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
    Definition RegularImplicitSchurFactor.h:479
    │ │ │ │ +
    static const int ZDim
    Measurement dimension.
    Definition RegularImplicitSchurFactor.h:52
    │ │ │ │ +
    Matrix information() const override
    Compute full information matrix
    Definition RegularImplicitSchurFactor.h:163
    │ │ │ │ +
    ~RegularImplicitSchurFactor() override
    Destructor.
    Definition RegularImplicitSchurFactor.h:85
    │ │ │ │ +
    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
    │ │ │ │ +
    FBlocks FBlocks_
    All ZDim*D F blocks (one for each camera)
    Definition RegularImplicitSchurFactor.h:58
    │ │ │ │ +
    VectorValues gradientAtZero() const override
    Calculate gradient, which is -F'Q*b, see paper.
    Definition RegularImplicitSchurFactor.h:457
    │ │ │ │ +
    Eigen::Matrix< double, ZDim, D > MatrixZD
    type of an F block
    Definition RegularImplicitSchurFactor.h:54
    │ │ │ │ +
    const Matrix PointCovariance_
    the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
    Definition RegularImplicitSchurFactor.h:59
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    Hessian-vector multiply, i.e.
    Definition RegularImplicitSchurFactor.h:413
    │ │ │ │ +
    std::map< Key, Matrix > hessianBlockDiagonal() const override
    Return the block diagonal of the Hessian for this factor.
    Definition RegularImplicitSchurFactor.h:231
    │ │ │ │ +
    Matrix augmentedInformation() const override
    Compute full augmented information matrix
    Definition RegularImplicitSchurFactor.h:155
    │ │ │ │ +
    GaussianFactor::shared_ptr negate() const override
    Construct the corresponding anti-factor to negate information stored stored in this factor.
    Definition RegularImplicitSchurFactor.h:263
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition RegularImplicitSchurFactor.h:43
    │ │ │ │ +
    Error2s e1
    Scratch space for multiplyHessianAdd.
    Definition RegularImplicitSchurFactor.h:374
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RegularImplicitSchurFactor.h:105
    │ │ │ │ +
    DenseIndex getDim(const_iterator variable) const override
    Degrees of freedom of camera.
    Definition RegularImplicitSchurFactor.h:134
    │ │ │ │ +
    bool equals(const GaussianFactor &lf, double tol) const override
    equals
    Definition RegularImplicitSchurFactor.h:118
    │ │ │ │ +
    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
    │ │ │ │ +
    RegularImplicitSchurFactor This
    Typedef to this class.
    Definition RegularImplicitSchurFactor.h:42
    │ │ │ │ +
    static const int D
    Camera dimension.
    Definition RegularImplicitSchurFactor.h:51
    │ │ │ │ +
    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
    │ │ │ │ +
    void multiplyHessianAdd(double alpha, const double *x, double *y) const
    double* Hessian-vector multiply, i.e.
    Definition RegularImplicitSchurFactor.h:380
    │ │ │ │ +
    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
    │ │ │ │ +
    void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
    Dummy version to measure overhead of key access.
    Definition RegularImplicitSchurFactor.h:442
    │ │ │ │ +
    Vector gradient(Key key, const VectorValues &x) const override
    Gradient wrt a key at any values.
    Definition RegularImplicitSchurFactor.h:499
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,226 +1,729 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RotateFactor.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * @file RotateFactor.cpp │ │ │ │ │ -3 * @brief RotateFactor class │ │ │ │ │ -4 * @author Frank Dellaert │ │ │ │ │ -5 * @date December 17, 2013 │ │ │ │ │ -6 */ │ │ │ │ │ -7 │ │ │ │ │ +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 │ │ │ │ │ 8#pragma once │ │ │ │ │ 9 │ │ │ │ │ -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}; │ │ │ │ │ +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: │ │ │ │ │ +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 │ │ │ │ │ -_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 { │ │ │ │ │ +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 │ │ │ │ │ -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"); │ │ │ │ │ +_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 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. │ │ │ │ │ +_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 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_:_:_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_:_:_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_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_:_:_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) │ │ │ │ │ +_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 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) │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 │ │ │ │ │ - * RRoottaatteeFFaaccttoorr..hh │ │ │ │ │ + * _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/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/ProjectionFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    GeneralSFMFactor.h File Reference
    │ │ │ │ +
    ProjectionFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    a general SFM factor with an unknown calibration │ │ │ │ +

    Reprojection of a LANDMARK to a 2D point. │ │ │ │ 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...
    class  gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >
     Non-linear factor for a constraint derived from a 2D measurement. 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::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
     traits 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
    │ │ │ │ +

    Reprojection of a LANDMARK to a 2D point.

    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +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 │ │ │ │ │ -GeneralSFMFactor.h File Reference │ │ │ │ │ -a general SFM factor with an unknown calibration _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_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_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_:_:_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 ********** │ │ │ │ │ -a general SFM factor with an unknown calibration │ │ │ │ │ - Date │ │ │ │ │ - Dec 15, 2010 │ │ │ │ │ +Reprojection of a LANDMARK to a 2D point. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Chris Beall │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + 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 │ │ │ │ │ + * _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/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::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >", "a04900.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/ProjectionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GeneralSFMFactor.h
    │ │ │ │ +
    ProjectionFactor.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,330 +114,227 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    21#pragma once
    │ │ │ │
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    30#include <gtsam/base/concepts.h>
    │ │ │ │ -
    31#include <gtsam/base/Manifold.h>
    │ │ │ │ -
    32#include <gtsam/base/Matrix.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 */
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    28#include <boost/optional.hpp>
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    45 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │ +
    46 boost::optional<POSE> body_P_sensor_;
    │ │ │ │ +
    47
    │ │ │ │ +
    48 // verbosity handling for Cheirality Exceptions
    │ │ │ │ + │ │ │ │ + │ │ │ │
    51
    │ │ │ │ -
    52namespace gtsam {
    │ │ │ │ +
    52 public:
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    73
    │ │ │ │ -
    74public:
    │ │ │ │ -
    75
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    78
    │ │ │ │ -
    79 // shorthand for a smart pointer to a factor
    │ │ │ │ -
    80 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    81
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ - │ │ │ │ -
    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)));}
    │ │ │ │ + │ │ │ │ +
    56
    │ │ │ │ + │ │ │ │ +
    59
    │ │ │ │ +
    61 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    62
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    65 measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
    │ │ │ │ +
    66 }
    │ │ │ │ +
    │ │ │ │ +
    67
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    97 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
    │ │ │ │ + │ │ │ │ +
    99 boost::optional<POSE> body_P_sensor = boost::none) :
    │ │ │ │ +
    100 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    102
    │ │ │ │ + │ │ │ │
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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 = ";
    │ │ │ │ + │ │ │ │ +
    119 if(this->body_P_sensor_)
    │ │ │ │ +
    120 this->body_P_sensor_->print(" sensor pose in body frame: ");
    │ │ │ │ +
    121 Base::print("", keyFormatter);
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    212
    │ │ │ │ -
    213public:
    │ │ │ │ -
    214
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    2D Point
    │ │ │ │ - │ │ │ │ -
    A binary JacobianFactor specialization that uses fixed matrix math for speed.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
    │ │ │ │ +
    158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    Definition CalibratedCamera.h:32
    │ │ │ │
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ +
    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
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition GeneralSFMFactor.h:60
    │ │ │ │ -
    boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
    Linearize using fixed-size matrices.
    Definition GeneralSFMFactor.h:138
    │ │ │ │ -
    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::NonlinearFactor::shared_ptr clone() const override
    Definition GeneralSFMFactor.h:101
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition GeneralSFMFactor.h:118
    │ │ │ │ -
    const Point2 measured() const
    return the measured
    Definition GeneralSFMFactor.h:177
    │ │ │ │ -
    GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
    Constructor.
    Definition GeneralSFMFactor.h:89
    │ │ │ │ -
    GeneralSFMFactor()
    default constructor
    Definition GeneralSFMFactor.h:93
    │ │ │ │ -
    GeneralSFMFactor< CAMERA, LANDMARK > This
    typedef for this object
    Definition GeneralSFMFactor.h:76
    │ │ │ │ -
    ~GeneralSFMFactor() override
    destructor
    Definition GeneralSFMFactor.h:98
    │ │ │ │ -
    NoiseModelFactorN< CAMERA, LANDMARK > Base
    typedef for the base class
    Definition GeneralSFMFactor.h:77
    │ │ │ │ -
    void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition GeneralSFMFactor.h:110
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition GeneralSFMFactor.h:183
    │ │ │ │ -
    Point2 measured_
    the 2D measurement
    Definition GeneralSFMFactor.h:72
    │ │ │ │ -
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition GeneralSFMFactor.h:204
    │ │ │ │ -
    GeneralSFMFactor2()
    default constructor
    Definition GeneralSFMFactor.h:232
    │ │ │ │ -
    ~GeneralSFMFactor2() override
    destructor
    Definition GeneralSFMFactor.h:234
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition GeneralSFMFactor.h:237
    │ │ │ │ -
    NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
    typedef for the base class
    Definition GeneralSFMFactor.h:217
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition GeneralSFMFactor.h:246
    │ │ │ │ -
    Point2 measured_
    the 2D measurement
    Definition GeneralSFMFactor.h:211
    │ │ │ │ -
    const Point2 measured() const
    return the measured
    Definition GeneralSFMFactor.h:280
    │ │ │ │ -
    PinholeCamera< CALIBRATION > Camera
    typedef for camera type
    Definition GeneralSFMFactor.h:216
    │ │ │ │ -
    GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
    Constructor.
    Definition GeneralSFMFactor.h:230
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition GeneralSFMFactor.h:254
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition GeneralSFMFactor.h:286
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition ProjectionFactor.h:40
    │ │ │ │ +
    bool verboseCheirality() const
    return verbosity
    Definition ProjectionFactor.h:181
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition ProjectionFactor.h:116
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition ProjectionFactor.h:125
    │ │ │ │ +
    Point2 measured_
    2D measurement
    Definition ProjectionFactor.h:44
    │ │ │ │ +
    GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
    shorthand for this class
    Definition ProjectionFactor.h:58
    │ │ │ │ +
    const Point2 & measured() const
    return the measurement
    Definition ProjectionFactor.h:166
    │ │ │ │ +
    boost::optional< POSE > body_P_sensor_
    The pose of the sensor in the body frame.
    Definition ProjectionFactor.h:46
    │ │ │ │ +
    boost::shared_ptr< CALIBRATION > K_
    shared pointer to calibration object
    Definition ProjectionFactor.h:45
    │ │ │ │ +
    ~GenericProjectionFactor() override
    Virtual destructor.
    Definition ProjectionFactor.h:104
    │ │ │ │ +
    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
    │ │ │ │ +
    GenericProjectionFactor()
    Default constructor.
    Definition ProjectionFactor.h:64
    │ │ │ │ +
    bool throwCheirality_
    If true, rethrows Cheirality exceptions (default: false)
    Definition ProjectionFactor.h:49
    │ │ │ │ +
    NoiseModelFactorN< POSE, LANDMARK > Base
    shorthand for base class type
    Definition ProjectionFactor.h:55
    │ │ │ │ +
    const boost::optional< POSE > & body_P_sensor() const
    return the (optional) sensor pose with respect to the vehicle frame
    Definition ProjectionFactor.h:176
    │ │ │ │ +
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition ProjectionFactor.h:184
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition ProjectionFactor.h:189
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition ProjectionFactor.h:107
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition ProjectionFactor.h:61
    │ │ │ │ +
    bool verboseCheirality_
    If true, prints text for Cheirality exceptions (default: false)
    Definition ProjectionFactor.h:50
    │ │ │ │ +
    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
    │ │ │ │ +
    const boost::shared_ptr< CALIBRATION > calibration() const
    return the calibration object
    Definition ProjectionFactor.h:171
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GeneralSFMFactor.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 │ │ │ │ │ @@ -16,280 +16,175 @@ │ │ │ │ │ 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_/_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 */ │ │ │ │ │ +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 { │ │ │ │ │ +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__; │ │ │ │ │ +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 │ │ │ │ │ -52namespace _g_t_s_a_m { │ │ │ │ │ +52 public: │ │ │ │ │ 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: │ │ │ │ │ -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_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)));} │ │ │ │ │ +_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 │ │ │ │ │ -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); │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +_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_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. │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ _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 │ │ │ │ │ @@ -313,25 +208,28 @@ │ │ │ │ │ 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_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 │ │ │ │ │ @@ -339,139 +237,114 @@ │ │ │ │ │ _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_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 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 │ │ │ │ │ +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 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 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 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 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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/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/InitializePose.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,53 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ProjectionFactor.h File Reference
    │ │ │ │ +
    InitializePose.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Reprojection of a LANDMARK to a 2D point. │ │ │ │ +

    common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ 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...
     
    struct  gtsam::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Reprojection of a LANDMARK to a 2D point.

    │ │ │ │ -
    Author
    Chris Beall
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Alex Cunningham
    │ │ │ │ +

    common code between lago.* (2D) and InitializePose3.* (3D)

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    August, 2020
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -ProjectionFactor.h File Reference │ │ │ │ │ -Reprojection of a LANDMARK to a 2D point. _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_:_:_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. │ │ │ │ │ +common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Date │ │ │ │ │ + August, 2020 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/InitializePose.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,243 +98,116 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ProjectionFactor.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
    │ │ │ │ -
    21#pragma once
    │ │ │ │ -
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28#include <boost/optional.hpp>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28namespace initialize {
    │ │ │ │
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ +
    30static constexpr Key kAnchorKey = 99999999;
    │ │ │ │
    31
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    45 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │ -
    46 boost::optional<POSE> body_P_sensor_;
    │ │ │ │ -
    47
    │ │ │ │ -
    48 // verbosity handling for Cheirality Exceptions
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    51
    │ │ │ │ -
    52 public:
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    56
    │ │ │ │ - │ │ │ │ -
    59
    │ │ │ │ -
    61 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    57template <class Pose>
    │ │ │ │ +
    58static Values computePoses(const Values& initialRot,
    │ │ │ │ +
    59 NonlinearFactorGraph* posegraph,
    │ │ │ │ +
    60 bool singleIter = true) {
    │ │ │ │ +
    61 const auto origin = Pose().translation();
    │ │ │ │
    62
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    65 measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
    │ │ │ │ -
    66 }
    │ │ │ │ -
    │ │ │ │ -
    67
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    97 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
    │ │ │ │ - │ │ │ │ -
    99 boost::optional<POSE> body_P_sensor = boost::none) :
    │ │ │ │ -
    100 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    102
    │ │ │ │ - │ │ │ │ -
    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 = ";
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ - │ │ │ │ -
    157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
    │ │ │ │ -
    158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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 // 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
    │ │ │ │ + │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Definition CalibratedCamera.h:32
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ -
    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
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition ProjectionFactor.h:40
    │ │ │ │ -
    bool verboseCheirality() const
    return verbosity
    Definition ProjectionFactor.h:181
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition ProjectionFactor.h:116
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition ProjectionFactor.h:125
    │ │ │ │ -
    Point2 measured_
    2D measurement
    Definition ProjectionFactor.h:44
    │ │ │ │ -
    GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
    shorthand for this class
    Definition ProjectionFactor.h:58
    │ │ │ │ -
    const Point2 & measured() const
    return the measurement
    Definition ProjectionFactor.h:166
    │ │ │ │ -
    boost::optional< POSE > body_P_sensor_
    The pose of the sensor in the body frame.
    Definition ProjectionFactor.h:46
    │ │ │ │ -
    boost::shared_ptr< CALIBRATION > K_
    shared pointer to calibration object
    Definition ProjectionFactor.h:45
    │ │ │ │ -
    ~GenericProjectionFactor() override
    Virtual destructor.
    Definition ProjectionFactor.h:104
    │ │ │ │ -
    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
    │ │ │ │ -
    GenericProjectionFactor()
    Default constructor.
    Definition ProjectionFactor.h:64
    │ │ │ │ -
    bool throwCheirality_
    If true, rethrows Cheirality exceptions (default: false)
    Definition ProjectionFactor.h:49
    │ │ │ │ -
    NoiseModelFactorN< POSE, LANDMARK > Base
    shorthand for base class type
    Definition ProjectionFactor.h:55
    │ │ │ │ -
    const boost::optional< POSE > & body_P_sensor() const
    return the (optional) sensor pose with respect to the vehicle frame
    Definition ProjectionFactor.h:176
    │ │ │ │ -
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition ProjectionFactor.h:184
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition ProjectionFactor.h:189
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition ProjectionFactor.h:107
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition ProjectionFactor.h:61
    │ │ │ │ -
    bool verboseCheirality_
    If true, prints text for Cheirality exceptions (default: false)
    Definition ProjectionFactor.h:50
    │ │ │ │ -
    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
    │ │ │ │ -
    const boost::shared_ptr< CALIBRATION > calibration() const
    return the calibration object
    Definition ProjectionFactor.h:171
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
    add is a synonym for push_back.
    Definition FactorGraph.h:210
    │ │ │ │ +
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ + │ │ │ │ + │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,350 +1,124 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ProjectionFactor.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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ +30static constexpr _K_e_y kAnchorKey = 99999999; │ │ │ │ │ 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__; │ │ │ │ │ -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: │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -_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_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 │ │ │ │ │ -_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 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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 // 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_:_:_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_:_:_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 │ │ │ │ │ -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_:_:_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 │ │ │ │ │ - * _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._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/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/PoseRotationPrior.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,153 +98,146 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ReferenceFrameFactor.h
    │ │ │ │ +
    PoseRotationPrior.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 * -------------------------------------------------------------------------- */
    │ │ │ │ +Go to the documentation of this file.
    1
    │ │ │ │ +
    10#pragma once
    │ │ │ │
    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 */
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    14
    │ │ │ │ +
    15
    │ │ │ │ +
    16namespace gtsam {
    │ │ │ │
    17
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ +
    18template<class POSE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    20public:
    │ │ │ │
    21
    │ │ │ │ -
    22namespace gtsam {
    │ │ │ │ -
    23
    │ │ │ │ -
    29template<class T, class P>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    56template<class POINT, class TRANSFORM>
    │ │ │ │ -
    │ │ │ │ -
    57class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
    │ │ │ │ -
    58protected:
    │ │ │ │ - │ │ │ │ -
    61
    │ │ │ │ -
    62public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ +
    22 typedef PoseRotationPrior<POSE> This;
    │ │ │ │ + │ │ │ │ +
    24 typedef POSE Pose;
    │ │ │ │ +
    25 typedef typename POSE::Translation Translation;
    │ │ │ │ +
    26 typedef typename POSE::Rotation Rotation;
    │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ +
    36protected:
    │ │ │ │ +
    37
    │ │ │ │ +
    38 Rotation measured_;
    │ │ │ │ +
    39
    │ │ │ │ +
    40public:
    │ │ │ │ +
    41
    │ │ │ │ + │ │ │ │ +
    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()) {}
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    Matrix trans(const Matrix &A)
    static transpose function, just calls Eigen transpose member function
    Definition Matrix.h:242
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition NonlinearFactor.h:218
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
    Definition ReferenceFrameFactor.h:57
    │ │ │ │ -
    void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition ReferenceFrameFactor.h:107
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition ReferenceFrameFactor.h:123
    │ │ │ │ -
    ReferenceFrameFactor()
    default constructor for serialization only
    Definition ReferenceFrameFactor.h:60
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    Definition PoseRotationPrior.h:19
    │ │ │ │ +
    PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
    standard constructor
    Definition PoseRotationPrior.h:46
    │ │ │ │ +
    PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
    Constructor that pulls the translation from an incoming POSE.
    Definition PoseRotationPrior.h:50
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition PoseRotationPrior.h:56
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals specialized to this factor
    Definition PoseRotationPrior.h:66
    │ │ │ │ +
    PoseRotationPrior()
    default constructor - only use for serialization
    Definition PoseRotationPrior.h:43
    │ │ │ │ +
    Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
    h(x)-z
    Definition PoseRotationPrior.h:78
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print contents
    Definition PoseRotationPrior.h:72
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition PoseRotationPrior.h:92
    │ │ │ │ +
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,203 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +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 │ │ │ │ │ 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 */ │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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) │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ +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()) {} │ │ │ │ │ +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 │ │ │ │ │ -_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_:_:_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_:_:_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_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_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 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 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 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 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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * RReeffeerreenncceeFFrraammeeFFaaccttoorr..hh │ │ │ │ │ + * _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/a01289.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/lago.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,185 +95,170 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    lago.h File Reference
    │ │ │ │ +
    dataset.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). │ │ │ │ +

    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 LINESIZE   81920
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ Typedefs

    │ │ │ │ -typedef std::map< Key, double > gtsam::lago::key2doubleMap
     
    │ │ │ │ +template<typename T >
    using gtsam::Parser = std::function< boost::optional< T >(istream &is, const string &tag)>
     
    │ │ │ │ +using gtsam::BearingRange2D = BearingRange< Pose2, Point2 >
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ 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.
     
    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< IndexedPosegtsam::parseVertexPose (std::istream &is, const std::string &tag)
     Parse TORO/G2O vertex "id x y yaw".
     
    │ │ │ │ +template<>
    GTSAM_EXPORT std::map< size_t, Pose2gtsam::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, Point2gtsam::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< Samplergtsam::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, Pose3gtsam::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, Point3gtsam::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

    │ │ │ │ -

    Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization).

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ +

    utility functions for loading datasets

    │ │ │ │ +
    Date
    Jan 22, 2010
    │ │ │ │ +
    Author
    Kai Ni
    │ │ │ │
    │ │ │ │ -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
    │ │ │ │ +Luca Carlone │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    May 14, 2014
    │ │ │ │ -

    Function Documentation

    │ │ │ │ - │ │ │ │ -

    ◆ buildLinearOrientationGraph()

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    GTSAM_EXPORT GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph (const vector< size_t > & spanningTreeIds,
    const vector< size_t > & chordsIds,
    const NonlinearFactorGraphg,
    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 NonlinearFactorGraphg 
    )
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -

    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]

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +Frank Dellaert │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,114 +1,188 @@ │ │ │ │ │ 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 │ │ │ │ │ -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_. │ │ │ │ │ +_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_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  LLIINNEESSIIZZEE   81920 │ │ │ │ │ +  │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -typedef std::map< _K_e_y, double >  ggttssaamm::::llaaggoo::::kkeeyy22ddoouubblleeMMaapp │ │ │ │ │ +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 │ │ │ │ │ - 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. │ │ │ │ │ -  │ │ │ │ │ - _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. │ │ │ │ │ + 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 ********** │ │ │ │ │ -Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph │ │ │ │ │ -Optimization). │ │ │ │ │ - Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ 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) │ │ │ │ │ + Jan 22, 2010 │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - 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] │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _l_a_g_o_._h │ │ │ │ │ + * _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/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/SmartProjectionPoseFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,55 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    InitializePose3.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SmartProjectionPoseFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Initialize Pose3 in a factor graph. │ │ │ │ +

    Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::InitializePose3
    class  gtsam::SmartProjectionPoseFactor< CALIBRATION >
     If you are using the factor, please cite: L. More...
     
    struct  gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

    │ │ │ │ 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, Rot3gtsam::KeyRotMap
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Initialize Pose3 in a factor graph.

    │ │ │ │ +

    Smart factor on poses, assuming camera calibration is fixed.

    │ │ │ │
    Author
    Luca Carlone
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    August, 2014
    │ │ │ │ +Chris Beall │ │ │ │ +
    │ │ │ │ +Zsolt Kira
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _T_y_p_e_d_e_f_s │ │ │ │ │ -InitializePose3.h File Reference │ │ │ │ │ -Initialize Pose3 in a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ _G_o_ _t_o_ _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 │ │ │ │ │ + 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_<_ _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. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::map< _K_e_y, std::vector< size_t > >  ggttssaamm::::KKeeyyVVeeccttoorrMMaapp │ │ │ │ │ -  │ │ │ │ │ - typedef std::map< _K_e_y, _R_o_t_3 >  ggttssaamm::::KKeeyyRRoottMMaapp │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Initialize Pose3 in a factor graph. │ │ │ │ │ +Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - August, 2014 │ │ │ │ │ + Chris Beall │ │ │ │ │ + Zsolt Kira │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01292.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01292 = [ │ │ │ │ │ - ["gtsam::InitializePose3", "a04856.html", null] │ │ │ │ │ + ["gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >", "a04948.html", null] │ │ │ │ │ ]; │ │ │ ├── ./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/SmartProjectionPoseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,97 +98,176 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    InitializePose3.h
    │ │ │ │ +
    SmartProjectionPoseFactor.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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ -
    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);
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    44template <class CALIBRATION>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46 : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
    │ │ │ │ +
    47 private:
    │ │ │ │ +
    48 typedef PinholePose<CALIBRATION> Camera;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    51
    │ │ │ │ +
    52protected:
    │ │ │ │ +
    53
    │ │ │ │ +
    54 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │
    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);
    │ │ │ │ -
    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
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    Graph algorithm using boost library.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    56public:
    │ │ │ │ +
    57
    │ │ │ │ +
    59 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    60
    │ │ │ │ + │ │ │ │ +
    65
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    73 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ +
    74 const boost::shared_ptr<CALIBRATION> K,
    │ │ │ │ + │ │ │ │ +
    76 : Base(sharedNoiseModel, params), K_(K) {
    │ │ │ │ +
    77 }
    │ │ │ │ +
    │ │ │ │ +
    78
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    87 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ +
    88 const boost::shared_ptr<CALIBRATION> K,
    │ │ │ │ +
    89 const boost::optional<Pose3> body_P_sensor,
    │ │ │ │ + │ │ │ │ +
    91 : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
    │ │ │ │ +
    92 this->body_P_sensor_ = body_P_sensor;
    │ │ │ │ +
    93 }
    │ │ │ │ +
    │ │ │ │ +
    94
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Smart factor on cameras (pose + calibration)
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    Definition InitializePose3.h:37
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    boost::optional< Pose3 > body_P_sensor_
    Pose of the camera in the body frame.
    Definition SmartFactorBase.h:82
    │ │ │ │ +
    Definition SmartFactorParams.h:42
    │ │ │ │ +
    SmartProjectionFactor: triangulates point and keeps an estimate of it around.
    Definition SmartProjectionFactor.h:45
    │ │ │ │ +
    double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
    Calculate the error of the factor.
    Definition SmartProjectionFactor.h:411
    │ │ │ │ +
    If you are using the factor, please cite: L.
    Definition SmartProjectionPoseFactor.h:46
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartProjectionPoseFactor.h:111
    │ │ │ │ +
    SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
    Constructor.
    Definition SmartProjectionPoseFactor.h:72
    │ │ │ │ +
    Base::Cameras cameras(const Values &values) const override
    Collect all cameras involved in this factor.
    Definition SmartProjectionPoseFactor.h:138
    │ │ │ │ +
    double error(const Values &values) const override
    error calculates the error of the factor.
    Definition SmartProjectionPoseFactor.h:119
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition SmartProjectionPoseFactor.h:59
    │ │ │ │ +
    boost::shared_ptr< CALIBRATION > K_
    calibration object (one for all cameras)
    Definition SmartProjectionPoseFactor.h:54
    │ │ │ │ +
    ~SmartProjectionPoseFactor() override
    Virtual destructor.
    Definition SmartProjectionPoseFactor.h:96
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartProjectionPoseFactor.h:104
    │ │ │ │ +
    const boost::shared_ptr< CALIBRATION > calibration() const
    return calibration shared pointers
    Definition SmartProjectionPoseFactor.h:128
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition SmartProjectionPoseFactor.h:152
    │ │ │ │ +
    SmartProjectionPoseFactor()
    Default constructor, only for serialization.
    Definition SmartProjectionPoseFactor.h:64
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,106 +1,236 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -InitializePose3.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 │ │ │ │ │ -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; │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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. │ │ │ │ │ -_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. │ │ │ │ │ +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); │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 │ │ │ │ │ -DDeeffiinniittiioonn InitializePose3.h:37 │ │ │ │ │ +_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/SmartFactorBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,57 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    FrobeniusFactor.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    SmartFactorBase.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Various factors that minimize some Frobenius norm. │ │ │ │ +

    Base class to create smart factors on poses or cameras. │ │ │ │ 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...
    class  gtsam::SmartFactorBase< CAMERA >
     Base class for smart factors. 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
    │ │ │ │ +

    Base class to create smart factors on poses or cameras.

    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Antoni Rosinol
    │ │ │ │ +
    │ │ │ │ +Zsolt Kira
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,44 +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 | _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 │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ -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 │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Antoni Rosinol │ │ │ │ │ + Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Chris Beall │ │ │ │ │ * _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_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/a01295.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ 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::SmartFactorBase< CAMERA >", "a04928.html", "a04928"] │ │ │ │ │ ]; │ │ │ ├── ./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/SmartFactorBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,181 +98,464 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    FrobeniusFactor.h
    │ │ │ │ +
    SmartFactorBase.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>
    │ │ │ │ - │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ +
    24#include <gtsam/slam/JacobianFactorQ.h>
    │ │ │ │ +
    25#include <gtsam/slam/JacobianFactorSVD.h>
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    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 };
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 public:
    │ │ │ │ -
    │ │ │ │ -
    83 FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
    │ │ │ │ -
    84 : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
    │ │ │ │ -
    85 j2) {}
    │ │ │ │ -
    │ │ │ │ +
    81 boost::optional<Pose3>
    │ │ │ │ + │ │ │ │ +
    83
    │ │ │ │ +
    84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
    │ │ │ │ +
    85 mutable FBlocks Fs;
    │ │ │ │
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    N*N matrix representation of SO(N).
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    87 public:
    │ │ │ │ + │ │ │ │ +
    89
    │ │ │ │ +
    91 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    92
    │ │ │ │ + │ │ │ │ +
    95
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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.
    │ │ │ │ + │ │ │ │ +
    232
    │ │ │ │ +
    233 return error;
    │ │ │ │ +
    234 }
    │ │ │ │ +
    │ │ │ │ +
    235
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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> > //
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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{
    │ │ │ │ + │ │ │ │ +
    411 return *body_P_sensor_;
    │ │ │ │ +
    412 else
    │ │ │ │ +
    413 return Pose3(); // if unspecified, the transformation is the identity
    │ │ │ │ +
    414 }
    │ │ │ │ +
    415
    │ │ │ │ +
    416private:
    │ │ │ │ +
    417
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class to create smart factors on poses or cameras.
    │ │ │ │ +
    HessianFactor class with constant sized blocks.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    A subclass of GaussianFactor specialized to structureless SFM.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::string demangle(const char *name)
    Pretty print Value type name.
    Definition types.cpp:37
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
    SVD computes economy SVD A=U*S*V'.
    Definition Matrix.cpp:560
    │ │ │ │ +
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Pose3 inverse() const
    inverse transformation with derivatives
    Definition Pose3.cpp:49
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
    Definition NoiseModel.h:516
    │ │ │ │ +
    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
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
    Definition FrobeniusFactor.h:51
    │ │ │ │ -
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition FrobeniusFactor.h:60
    │ │ │ │ -
    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
    │ │ │ │ -
    FrobeniusFactor calculates the Frobenius norm between rotation matrices.
    Definition FrobeniusFactor.h:78
    │ │ │ │ -
    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
    │ │ │ │ -
    FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition FrobeniusFactor.h:83
    │ │ │ │ -
    FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
    Definition FrobeniusFactor.h:104
    │ │ │ │ -
    FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
    Construct from two keys and measured rotation.
    Definition FrobeniusFactor.h:117
    │ │ │ │ -
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print with optional string
    Definition FrobeniusFactor.h:130
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    assert equality up to a tolerance
    Definition FrobeniusFactor.h:140
    │ │ │ │ +
    virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
    Check if two factors are equal.
    Definition NonlinearFactor.cpp:47
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactor.cpp:37
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    Base class for smart factors.
    Definition SmartFactorBase.h:50
    │ │ │ │ +
    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
    │ │ │ │ +
    void add(const Z &measured, const Key &key)
    Add a new measurement and pose/camera key.
    Definition SmartFactorBase.h:126
    │ │ │ │ +
    ~SmartFactorBase() override
    Virtual destructor, subclasses from NonlinearFactor.
    Definition SmartFactorBase.h:118
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    SharedIsotropic noiseModel_
    As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
    Definition SmartFactorBase.h:72
    │ │ │ │ +
    static const int Dim
    Camera dimension.
    Definition SmartFactorBase.h:60
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual Cameras cameras(const Values &values) const
    Collect all cameras: important that in key order.
    Definition SmartFactorBase.h:162
    │ │ │ │ +
    static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
    Create BIG block-diagonal matrix F from Fblocks.
    Definition SmartFactorBase.h:400
    │ │ │ │ +
    double totalReprojectionError(const Cameras &cameras, const POINT &point) const
    Calculate the error of the factor.
    Definition SmartFactorBase.h:267
    │ │ │ │ +
    void add(const ZVector &measurements, const KeyVector &cameraKeys)
    Add a bunch of measurements, together with the camera keys.
    Definition SmartFactorBase.h:136
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    ZVector measured_
    Measurements for each of the m views.
    Definition SmartFactorBase.h:79
    │ │ │ │ +
    GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor.
    Definition SmartFactorBase.h:91
    │ │ │ │ +
    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
    │ │ │ │ +
    SmartFactorBase()
    Default Constructor, for serialization.
    Definition SmartFactorBase.h:97
    │ │ │ │ +
    void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
    Whiten the Jacobians computed by computeJacobians using noiseModel_.
    Definition SmartFactorBase.h:347
    │ │ │ │ +
    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
    │ │ │ │ +
    void add(const SFM_TRACK &trackToAdd)
    Add an entire SfM_track (collection of cameras observing a single point).
    Definition SmartFactorBase.h:148
    │ │ │ │ +
    const ZVector & measured() const
    Return the 2D measurements (ZDim, in general).
    Definition SmartFactorBase.h:159
    │ │ │ │ +
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
    Return Jacobians as JacobianFactorSVD.
    Definition SmartFactorBase.h:386
    │ │ │ │ +
    size_t dim() const override
    Return the dimension (number of rows!) of the factor.
    Definition SmartFactorBase.h:156
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition SmartFactorBase.h:419
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartFactorBase.h:174
    │ │ │ │ +
    boost::optional< Pose3 > body_P_sensor_
    Pose of the camera in the body frame.
    Definition SmartFactorBase.h:82
    │ │ │ │ +
    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
    │ │ │ │ +
    static const int ZDim
    Measurement dimension.
    Definition SmartFactorBase.h:61
    │ │ │ │ +
    static Matrix PointCov(const Matrix &E)
    Computes Point Covariance P from the "point Jacobian" E.
    Definition SmartFactorBase.h:274
    │ │ │ │ +
    CameraSet< CAMERA > Cameras
    The CameraSet data structure is used to refer to a set of cameras.
    Definition SmartFactorBase.h:94
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartFactorBase.h:187
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,149 +1,384 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FrobeniusFactor.h │ │ │ │ │ +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-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> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 }; │ │ │ │ │ +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 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) {} │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_:_:_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. │ │ │ │ │ @@ -152,75 +387,243 @@ │ │ │ │ │ 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_:_:_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_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _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_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/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/KarcherMeanFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,94 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    OrientedPlane3Factor.h
    │ │ │ │ +
    KarcherMeanFactor.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>
    │ │ │ │ - │ │ │ │ -
    12
    │ │ │ │ -
    13namespace gtsam {
    │ │ │ │ -
    14
    │ │ │ │ -
    │ │ │ │ -
    18class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
    │ │ │ │ -
    19 protected:
    │ │ │ │ -
    20 OrientedPlane3 measured_p_;
    │ │ │ │ - │ │ │ │ +
    1/* ----------------------------------------------------------------------------
    │ │ │ │ +
    2
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * All Rights Reserved
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │
    22
    │ │ │ │ -
    23 public:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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_;
    │ │ │ │ - │ │ │ │ -
    56
    │ │ │ │ -
    57 public:
    │ │ │ │ - │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    61 }
    │ │ │ │ -
    │ │ │ │ -
    62
    │ │ │ │ -
    │ │ │ │ -
    64 OrientedPlane3DirectionPrior(Key key, const Vector4& z,
    │ │ │ │ -
    65 const SharedGaussian& noiseModel)
    │ │ │ │ -
    66 : Base(noiseModel, key), measured_p_(z) {}
    │ │ │ │ -
    │ │ │ │ +
    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 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
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
    Definition OrientedPlane3.h:36
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Factor to measure a planar landmark from a given pose.
    Definition OrientedPlane3Factor.h:18
    │ │ │ │ -
    OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
    Constructor with measured plane (a,b,c,d) coefficients.
    Definition OrientedPlane3Factor.h:36
    │ │ │ │ -
    OrientedPlane3Factor()
    Constructor.
    Definition OrientedPlane3Factor.h:25
    │ │ │ │ -
    Definition OrientedPlane3Factor.h:52
    │ │ │ │ -
    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
    │ │ │ │ -
    NoiseModelFactorN< OrientedPlane3 > Base
    measured plane parameters
    Definition OrientedPlane3Factor.h:55
    │ │ │ │ -
    OrientedPlane3DirectionPrior()
    Constructor.
    Definition OrientedPlane3Factor.h:60
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher me...
    Definition KarcherMeanFactor.h:45
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor: always zero.
    Definition KarcherMeanFactor.h:69
    │ │ │ │ +
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition KarcherMeanFactor.h:72
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
    linearize to a GaussianFactor
    Definition KarcherMeanFactor.h:75
    │ │ │ │ +
    ~KarcherMeanFactor() override
    Destructor.
    Definition KarcherMeanFactor.h:66
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,139 +1,111 @@ │ │ │ │ │ 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 */ │ │ │ │ │ +KarcherMeanFactor.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; │ │ │ │ │ +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 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 │ │ │ │ │ -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) {} │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_l_a_m │ │ │ │ │ - * OOrriieenntteeddPPllaannee33FFaaccttoorr..hh │ │ │ │ │ + * 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/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/lago.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,151 +96,184 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ Typedefs | │ │ │ │ -Enumerations | │ │ │ │ Functions
    │ │ │ │ -
    dataset.h File Reference
    │ │ │ │ +
    lago.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    utility functions for loading datasets │ │ │ │ +

    Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Namespaces

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

    │ │ │ │ Typedefs

    │ │ │ │ -typedef std::pair< size_t, Pose2gtsam::IndexedPose
     Return type for auxiliary functions.
     
    │ │ │ │ -typedef std::pair< size_t, Point2gtsam::IndexedLandmark
     
    │ │ │ │ -typedef std::pair< std::pair< size_t, size_t >, Pose2gtsam::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.
     
    │ │ │ │ +typedef std::map< Key, double > gtsam::lago::key2doubleMap
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ 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< IndexedPosegtsam::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)
     
    │ │ │ │ +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

    │ │ │ │ -

    utility functions for loading datasets

    │ │ │ │ -
    Date
    Jan 22, 2010
    │ │ │ │ -
    Author
    Ni Kai
    │ │ │ │ +

    Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization).

    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │
    │ │ │ │ -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
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ -
    │ │ │ │ +Frank Dellaert │ │ │ │ +
    Date
    May 14, 2014
    │ │ │ │ +

    Function Documentation

    │ │ │ │ + │ │ │ │ +

    ◆ buildLinearOrientationGraph()

    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    GTSAM_EXPORT GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph (const vector< size_t > & spanningTreeIds,
    const vector< size_t > & chordsIds,
    const NonlinearFactorGraphg,
    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 NonlinearFactorGraphg 
    )
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +

    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,164 +1,114 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_. │ │ │ │ │ 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. │ │ │ │ │ +typedef std::map< _K_e_y, double >  ggttssaamm::::llaaggoo::::kkeeyy22ddoouubblleeMMaapp │ │ │ │ │   │ │ │ │ │ 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) │ │ │ │ │ + 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. │ │ │ │ │ +  │ │ │ │ │ + _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 ********** │ │ │ │ │ -utility functions for loading datasets │ │ │ │ │ +Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph │ │ │ │ │ +Optimization). │ │ │ │ │ + Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Jan 22, 2010 │ │ │ │ │ + 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 │ │ │ │ │ - Ni Kai │ │ │ │ │ Luca Carlone │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + 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 │ │ │ │ │ - * _d_a_t_a_s_e_t_._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/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/lago.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,208 +98,75 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    dataset.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/sfm/SfmData.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    34#include <gtsam/base/Testable.h>
    │ │ │ │ -
    35#include <gtsam/base/types.h>
    │ │ │ │ +
    35#pragma once
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    72};
    │ │ │ │ -
    │ │ │ │ -
    73
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ -
    115
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ -
    Calibration used by Bundler.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    2D Pose
    │ │ │ │ - │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ -
    Data structure for dealing with Structure from Motion data.
    │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 algorithm using boost library.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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_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.
    │ │ │ │ -
    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
    │ │ │ │ -
    std::pair< size_t, Pose2 > IndexedPose
    Return type for auxiliary functions.
    Definition dataset.h:112
    │ │ │ │ -
    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_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.
    │ │ │ │ -
    KernelFunctionType
    Robust kernel type to wrap around quadratic noise model.
    Definition dataset.h:75
    │ │ │ │ -
    NoiseFormat
    Indicates how noise parameters are stored in file.
    Definition dataset.h:66
    │ │ │ │ -
    @ NoiseFormatGRAPH
    default: toro-style order, but covariance matrix !
    Definition dataset.h:69
    │ │ │ │ -
    @ NoiseFormatAUTO
    Try to guess covariance matrix layout.
    Definition dataset.h:71
    │ │ │ │ -
    @ NoiseFormatTORO
    Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
    Definition dataset.h:68
    │ │ │ │ -
    @ NoiseFormatCOV
    Covariance matrix C11, C12, C13, C22, C23, C33.
    Definition dataset.h:70
    │ │ │ │ -
    @ NoiseFormatG2O
    Information matrix I11, I12, I13, I22, I23, I33.
    Definition dataset.h:67
    │ │ │ │ -
    boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
    Parse G2O landmark vertex "id x y".
    Definition dataset.cpp:187
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
    Parse TORO/G2O edge "id1 id2 x y yaw".
    Definition dataset.cpp:293
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
    Parse TORO/G2O vertex "id x y yaw".
    Definition dataset.cpp:167
    │ │ │ │ -
    GraphAndValues load3D(const string &filename)
    Load TORO 3D Graph.
    Definition dataset.cpp:918
    │ │ │ │ -
    void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
    save 2d graph
    Definition dataset.cpp:584
    │ │ │ │ -
    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
    │ │ │ │ -
    string findExampleDataFile(const string &name)
    Find the full path to an example dataset distributed with gtsam.
    Definition dataset.cpp:62
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,295 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -dataset.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_/_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> │ │ │ │ │ +35#pragma once │ │ │ │ │ 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_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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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. │ │ │ │ │ +_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. │ │ │ │ │ -_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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ - * _d_a_t_a_s_e_t_._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/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/SmartProjectionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,539 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    KarcherMeanFactor-inl.h
    │ │ │ │ +
    SmartProjectionFactor.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 KarcherMeanFactor.cpp
    │ │ │ │ -
    14 * @author Frank Dellaert
    │ │ │ │ -
    15 * @date March 2019
    │ │ │ │ -
    16 */
    │ │ │ │ -
    17
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22#include <gtsam/slam/KarcherMeanFactor.h>
    │ │ │ │ -
    23
    │ │ │ │ -
    24using namespace std;
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46
    │ │ │ │ +
    47public:
    │ │ │ │ +
    48
    │ │ │ │ +
    49private:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    52 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
    │ │ │ │ +
    53
    │ │ │ │ +
    54protected:
    │ │ │ │ +
    55
    │ │ │ │ + │ │ │ │ +
    60
    │ │ │ │ + │ │ │ │ +
    64 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
    │ │ │ │ + │ │ │ │ +
    67
    │ │ │ │ +
    68 public:
    │ │ │ │ +
    69
    │ │ │ │ +
    71 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    72
    │ │ │ │ +
    74 typedef CAMERA Camera;
    │ │ │ │ + │ │ │ │ +
    76
    │ │ │ │ + │ │ │ │ +
    81
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    88 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ + │ │ │ │ +
    90 : Base(sharedNoiseModel),
    │ │ │ │ +
    91 params_(params),
    │ │ │ │ +
    92 result_(TriangulationResult::Degenerate()) {}
    │ │ │ │ +
    │ │ │ │ +
    93
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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) {
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    183 params_.triangulation);
    │ │ │ │ +
    184 return result_;
    │ │ │ │ +
    185 }
    │ │ │ │ +
    │ │ │ │ +
    186
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    352 }
    │ │ │ │ +
    │ │ │ │ +
    353
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    370 }
    │ │ │ │ +
    371 }
    │ │ │ │ +
    │ │ │ │ +
    372
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    381 return nonDegenerate;
    │ │ │ │ +
    382 }
    │ │ │ │ +
    │ │ │ │ +
    383
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    401 else
    │ │ │ │ +
    402 return Vector::Zero(cameras.size() * 2);
    │ │ │ │ +
    403 }
    │ │ │ │ +
    │ │ │ │ +
    404
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    412 boost::optional<Point3> externalPoint = boost::none) const {
    │ │ │ │ +
    413
    │ │ │ │ +
    414 if (externalPoint)
    │ │ │ │ +
    415 result_ = TriangulationResult(*externalPoint);
    │ │ │ │ +
    416 else
    │ │ │ │ + │ │ │ │ +
    418
    │ │ │ │ +
    419 if (result_)
    │ │ │ │ +
    420 // All good, just use version in base class
    │ │ │ │ + │ │ │ │ +
    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)) {
    │ │ │ │ + │ │ │ │ +
    436 } else { // else of active flag
    │ │ │ │ +
    437 return 0.0;
    │ │ │ │ +
    438 }
    │ │ │ │ +
    439 }
    │ │ │ │ +
    │ │ │ │ +
    440
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Functions for triangulation.
    │ │ │ │ +
    utility functions for loading datasets
    │ │ │ │ +
    Base class to create smart factors on poses or cameras.
    │ │ │ │ +
    Collect common parameters for SmartProjection and SmartStereoProjection factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
    Construct from given keys.
    Definition KarcherMeanFactor-inl.h:60
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    Base class for smart factors.
    Definition SmartFactorBase.h:50
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static const int Dim
    Camera dimension.
    Definition SmartFactorBase.h:60
    │ │ │ │ +
    virtual Cameras cameras(const Values &values) const
    Collect all cameras: important that in key order.
    Definition SmartFactorBase.h:162
    │ │ │ │ +
    double totalReprojectionError(const Cameras &cameras, const POINT &point) const
    Calculate the error of the factor.
    Definition SmartFactorBase.h:267
    │ │ │ │ +
    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
    │ │ │ │ +
    ZVector measured_
    Measurements for each of the m views.
    Definition SmartFactorBase.h:79
    │ │ │ │ +
    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
    │ │ │ │ +
    void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
    Whiten the Jacobians computed by computeJacobians using noiseModel_.
    Definition SmartFactorBase.h:347
    │ │ │ │ +
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
    Return Jacobians as JacobianFactorSVD.
    Definition SmartFactorBase.h:386
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartFactorBase.h:174
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartFactorBase.h:187
    │ │ │ │ +
    Definition SmartFactorParams.h:42
    │ │ │ │ +
    LinearizationMode linearizationMode
    How to linearize the factor.
    Definition SmartFactorParams.h:44
    │ │ │ │ +
    double retriangulationThreshold
    threshold to decide whether to re-triangulate
    Definition SmartFactorParams.h:50
    │ │ │ │ +
    DegeneracyMode degeneracyMode
    How to linearize the factor.
    Definition SmartFactorParams.h:45
    │ │ │ │ +
    SmartProjectionFactor: triangulates point and keeps an estimate of it around.
    Definition SmartProjectionFactor.h:45
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
    Create JacobianFactorQ factor, takes values.
    Definition SmartProjectionFactor.h:261
    │ │ │ │ +
    boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
    Different (faster) way to compute a JacobianFactorSVD factor.
    Definition SmartProjectionFactor.h:267
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
    Linearize to a Hessianfactor.
    Definition SmartProjectionFactor.h:277
    │ │ │ │ +
    bool isOutlier() const
    return the outlier state
    Definition SmartProjectionFactor.h:462
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartProjectionFactor.h:103
    │ │ │ │ +
    bool triangulateAndComputeE(Matrix &E, const Values &values) const
    Triangulate and compute derivative of error with respect to point.
    Definition SmartProjectionFactor.h:349
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
    Linearize to Gaussian Factor.
    Definition SmartProjectionFactor.h:299
    │ │ │ │ +
    bool isFarPoint() const
    return the farPoint state
    Definition SmartProjectionFactor.h:465
    │ │ │ │ +
    TriangulationResult result_
    result from triangulateSafe
    Definition SmartProjectionFactor.h:63
    │ │ │ │ +
    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
    │ │ │ │ +
    TriangulationResult triangulateSafe(const Cameras &cameras) const
    Call gtsam::triangulateSafe iff we need to re-triangulate.
    Definition SmartProjectionFactor.h:174
    │ │ │ │ +
    ~SmartProjectionFactor() override
    Virtual destructor.
    Definition SmartProjectionFactor.h:95
    │ │ │ │ +
    double error(const Values &values) const override
    Calculate total reprojection error.
    Definition SmartProjectionFactor.h:433
    │ │ │ │ +
    bool isValid() const
    Is result valid?
    Definition SmartProjectionFactor.h:453
    │ │ │ │ +
    std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
    current triangulation poses
    Definition SmartProjectionFactor.h:65
    │ │ │ │ +
    bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
    Triangulate and compute derivative of error with respect to point.
    Definition SmartProjectionFactor.h:338
    │ │ │ │ +
    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
    │ │ │ │ +
    double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
    Calculate the error of the factor.
    Definition SmartProjectionFactor.h:411
    │ │ │ │ +
    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
    │ │ │ │ +
    bool isDegenerate() const
    return the degenerate state
    Definition SmartProjectionFactor.h:456
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartProjectionFactor.h:115
    │ │ │ │ +
    CAMERA Camera
    shorthand for a set of cameras
    Definition SmartProjectionFactor.h:74
    │ │ │ │ +
    boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
    Create JacobianFactorQ factor.
    Definition SmartProjectionFactor.h:251
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition SmartProjectionFactor.h:470
    │ │ │ │ +
    TriangulationResult point(const Values &values) const
    COMPUTE the landmark.
    Definition SmartProjectionFactor.h:447
    │ │ │ │ +
    SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
    Constructor.
    Definition SmartProjectionFactor.h:87
    │ │ │ │ +
    bool isPointBehindCamera() const
    return the cheirality status flag
    Definition SmartProjectionFactor.h:459
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
    linearize
    Definition SmartProjectionFactor.h:329
    │ │ │ │ +
    bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
    takes values
    Definition SmartProjectionFactor.h:385
    │ │ │ │ +
    Vector reprojectionErrorAfterTriangulation(const Values &values) const
    Calculate vector of re-projection errors, before applying noise model.
    Definition SmartProjectionFactor.h:396
    │ │ │ │ +
    TriangulationResult point() const
    return the landmark
    Definition SmartProjectionFactor.h:442
    │ │ │ │ +
    bool triangulateForLinearize(const Cameras &cameras) const
    Possibly re-triangulate before calculating Jacobians.
    Definition SmartProjectionFactor.h:193
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition SmartProjectionFactor.h:71
    │ │ │ │ +
    SmartProjectionFactor()
    Default constructor, only for serialization.
    Definition SmartProjectionFactor.h:80
    │ │ │ │ +
    boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
    Linearize to Gaussian Factor.
    Definition SmartProjectionFactor.h:321
    │ │ │ │ + │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,117 +1,743 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KarcherMeanFactor-inl.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/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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 <_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 │ │ │ │ │ -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. │ │ │ │ │ +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; │ │ │ │ │ +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: │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ +_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. │ │ │ │ │ +_d_a_t_a_s_e_t_._h │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ +_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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +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 │ │ │ │ │ +&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 │ │ │ │ │ +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 │ │ │ │ │ - * KKaarrcchheerrMMeeaannFFaaccttoorr--iinnll..hh │ │ │ │ │ + * _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/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/PoseTranslationPrior.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    StereoFactor.h File Reference
    │ │ │ │ +
    PoseTranslationPrior.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A non-linear factor for stereo measurements. │ │ │ │ +

    Implements a prior on the translation component of a pose. │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::GenericStereoFactor< POSE, LANDMARK >
     A Generic Stereo Factor. More...
     
    struct  gtsam::traits< GenericStereoFactor< T1, T2 > >
     traits More...
    class  gtsam::PoseTranslationPrior< POSE >
     A prior on the translation part of a pose. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    A non-linear factor for stereo measurements.

    │ │ │ │ -
    Author
    Alireza Fathi
    │ │ │ │ -
    │ │ │ │ -Chris Beall
    │ │ │ │ +

    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 │ │ │ │ │ _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_._._. │ │ │ │ │ +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_:_:_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_<_ _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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A non-linear factor for stereo measurements. │ │ │ │ │ +Implements a prior on the translation component of a pose. │ │ │ │ │ + Date │ │ │ │ │ + Jun 14, 2012 │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Chris Beall │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/a01310.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01310 = [ │ │ │ │ │ - ["gtsam::traits< GenericStereoFactor< T1, T2 > >", "a04964.html", null] │ │ │ │ │ + ["gtsam::PoseTranslationPrior< POSE >", "a04892.html", "a04892"] │ │ │ │ │ ]; │ │ │ ├── ./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/PoseTranslationPrior.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,218 +98,142 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    StereoFactor.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    14
    │ │ │ │ +
    15namespace gtsam {
    │ │ │ │ +
    16
    │ │ │ │ +
    20template<class POSE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    22public:
    │ │ │ │ +
    23 typedef PoseTranslationPrior<POSE> This;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    39 // verbosity handling for Cheirality Exceptions
    │ │ │ │ -
    40 bool throwCheirality_;
    │ │ │ │ -
    41 bool verboseCheirality_;
    │ │ │ │ -
    42
    │ │ │ │ -
    43public:
    │ │ │ │ -
    44
    │ │ │ │ -
    45 // shorthand for base class type
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    71
    │ │ │ │ +
    81
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    84 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
    │ │ │ │ - │ │ │ │ -
    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) {}
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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_;
    │ │ │ │ -
    160 }
    │ │ │ │ -
    │ │ │ │ -
    161
    │ │ │ │ -
    163 inline bool verboseCheirality() const { return verboseCheirality_; }
    │ │ │ │ -
    164
    │ │ │ │ -
    166 inline bool throwCheirality() const { return throwCheirality_; }
    │ │ │ │ -
    167
    │ │ │ │ -
    168private:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A Stereo Camera based on two Simple Cameras.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    106
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Definition StereoCamera.h:26
    │ │ │ │ -
    A stereo camera class, parameterize by left camera pose and stereo calibration.
    Definition StereoCamera.h:47
    │ │ │ │ -
    StereoPoint2 project(const Point3 &point) const
    Project 3D point to StereoPoint2 (uL,uR,v)
    Definition StereoCamera.cpp:32
    │ │ │ │ -
    A 2D stereo point, v will be same for rectified images.
    Definition StereoPoint2.h:32
    │ │ │ │ -
    Vector3 vector() const
    convert to vector
    Definition StereoPoint2.h:115
    │ │ │ │ -
    bool equals(const StereoPoint2 &q, double tol=1e-9) const
    equals
    Definition StereoPoint2.h:64
    │ │ │ │ -
    void print(const std::string &s="") const
    print
    Definition StereoPoint2.cpp:26
    │ │ │ │
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    A Generic Stereo Factor.
    Definition StereoFactor.h:31
    │ │ │ │ -
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition StereoFactor.h:166
    │ │ │ │ -
    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
    │ │ │ │ -
    bool equals(const NonlinearFactor &f, double tol=1e-9) const override
    equals
    Definition StereoFactor.h:113
    │ │ │ │ -
    const Cal3_S2Stereo::shared_ptr calibration() const
    return the calibration object
    Definition StereoFactor.h:158
    │ │ │ │ -
    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
    │ │ │ │ -
    GenericStereoFactor< POSE, LANDMARK > This
    typedef for this class (with templates)
    Definition StereoFactor.h:47
    │ │ │ │ -
    ~GenericStereoFactor() override
    Virtual destructor.
    Definition StereoFactor.h:91
    │ │ │ │ -
    bool verboseCheirality() const
    return verbosity
    Definition StereoFactor.h:163
    │ │ │ │ -
    boost::shared_ptr< GenericStereoFactor > shared_ptr
    typedef for shared pointer to this object
    Definition StereoFactor.h:48
    │ │ │ │ -
    const StereoPoint2 & measured() const
    return the measured
    Definition StereoFactor.h:153
    │ │ │ │ -
    GenericStereoFactor()
    Default constructor.
    Definition StereoFactor.h:54
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition StereoFactor.h:170
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition StereoFactor.h:103
    │ │ │ │ -
    POSE CamPose
    typedef for Pose Lie Value type
    Definition StereoFactor.h:49
    │ │ │ │ -
    NoiseModelFactorN< POSE, LANDMARK > Base
    typedef for base class
    Definition StereoFactor.h:46
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition StereoFactor.h:94
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    A prior on the translation part of a pose.
    Definition PoseTranslationPrior.h:21
    │ │ │ │ +
    PoseTranslationPrior(Key key, const Translation &measured, const noiseModel::Base::shared_ptr &model)
    standard constructor
    Definition PoseTranslationPrior.h:43
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
    h(x)-z
    Definition PoseTranslationPrior.h:62
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals specialized to this factor
    Definition PoseTranslationPrior.h:77
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition PoseTranslationPrior.h:57
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition PoseTranslationPrior.h:91
    │ │ │ │ +
    PoseTranslationPrior()
    default constructor - only use for serialization
    Definition PoseTranslationPrior.h:40
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print contents
    Definition PoseTranslationPrior.h:83
    │ │ │ │ +
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,226 +1,124 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -StereoFactor.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 │ │ │ │ │ -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 { │ │ │ │ │ -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_; │ │ │ │ │ +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 │ │ │ │ │ -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 = │ │ │ │ │ +_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 │ │ │ │ │ +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 } │ │ │ │ │ +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 { │ │ │ │ │ -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_; │ │ │ │ │ -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. │ │ │ │ │ +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} // \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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ @@ -229,89 +127,56 @@ │ │ │ │ │ _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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ + * _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/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/TriangulationFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,176 +98,245 @@ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SmartProjectionPoseFactor.h
    │ │ │ │ +
    TriangulationFactor.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
    │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    44template <class CALIBRATION>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46 : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
    │ │ │ │ -
    47 private:
    │ │ │ │ -
    48 typedef PinholePose<CALIBRATION> Camera;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    51
    │ │ │ │ -
    52protected:
    │ │ │ │ -
    53
    │ │ │ │ -
    54 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │ -
    55
    │ │ │ │ -
    56public:
    │ │ │ │ -
    57
    │ │ │ │ -
    59 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    60
    │ │ │ │ - │ │ │ │ -
    65
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    73 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ -
    74 const boost::shared_ptr<CALIBRATION> K,
    │ │ │ │ - │ │ │ │ -
    76 : Base(sharedNoiseModel, params), K_(K) {
    │ │ │ │ -
    77 }
    │ │ │ │ -
    │ │ │ │ -
    78
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    87 const SharedNoiseModel& sharedNoiseModel,
    │ │ │ │ -
    88 const boost::shared_ptr<CALIBRATION> K,
    │ │ │ │ -
    89 const boost::optional<Pose3> body_P_sensor,
    │ │ │ │ - │ │ │ │ -
    91 : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
    │ │ │ │ -
    92 this->body_P_sensor_ = body_P_sensor;
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <boost/make_shared.hpp>
    │ │ │ │ +
    23#include <boost/lexical_cast.hpp>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    32template<class CAMERA>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    34
    │ │ │ │ +
    35public:
    │ │ │ │ +
    36
    │ │ │ │ +
    38 using Camera = CAMERA;
    │ │ │ │ +
    39
    │ │ │ │ +
    40protected:
    │ │ │ │ +
    41
    │ │ │ │ + │ │ │ │ +
    44
    │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    49 using Measurement = typename CAMERA::Measurement;
    │ │ │ │ +
    50
    │ │ │ │ +
    51 // Keep a copy of measurement and calibration for I/O
    │ │ │ │ +
    52 const CAMERA camera_;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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_(
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    93 }
    │ │ │ │
    │ │ │ │
    94
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Smart factor on cameras (pose + calibration)
    │ │ │ │ +
    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");
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ +
    130 std::cout << e.what() << ": Landmark "
    │ │ │ │ +
    131 << DefaultKeyFormatter(this->key()) << " moved behind camera"
    │ │ │ │ +
    132 << std::endl;
    │ │ │ │ + │ │ │ │ +
    134 throw e;
    │ │ │ │ +
    135 return camera_.defaultErrorWhenTriangulatingBehindCamera();
    │ │ │ │ +
    136 }
    │ │ │ │ +
    137 }
    │ │ │ │ +
    │ │ │ │ +
    138
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Calibrated camera for which only pose is unknown.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ +
    Definition CalibratedCamera.h:32
    │ │ │ │
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    boost::optional< Pose3 > body_P_sensor_
    Pose of the camera in the body frame.
    Definition SmartFactorBase.h:82
    │ │ │ │ -
    Definition SmartFactorParams.h:42
    │ │ │ │ -
    SmartProjectionFactor: triangulates point and keeps an estimate of it around.
    Definition SmartProjectionFactor.h:45
    │ │ │ │ -
    double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
    Calculate the error of the factor.
    Definition SmartProjectionFactor.h:411
    │ │ │ │ -
    If you are using the factor, please cite: L.
    Definition SmartProjectionPoseFactor.h:46
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition SmartProjectionPoseFactor.h:111
    │ │ │ │ -
    SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
    Constructor.
    Definition SmartProjectionPoseFactor.h:72
    │ │ │ │ -
    Base::Cameras cameras(const Values &values) const override
    Collect all cameras involved in this factor.
    Definition SmartProjectionPoseFactor.h:138
    │ │ │ │ -
    double error(const Values &values) const override
    error calculates the error of the factor.
    Definition SmartProjectionPoseFactor.h:119
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition SmartProjectionPoseFactor.h:59
    │ │ │ │ -
    boost::shared_ptr< CALIBRATION > K_
    calibration object (one for all cameras)
    Definition SmartProjectionPoseFactor.h:54
    │ │ │ │ -
    ~SmartProjectionPoseFactor() override
    Virtual destructor.
    Definition SmartProjectionPoseFactor.h:96
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition SmartProjectionPoseFactor.h:104
    │ │ │ │ -
    const boost::shared_ptr< CALIBRATION > calibration() const
    return calibration shared pointers
    Definition SmartProjectionPoseFactor.h:128
    │ │ │ │ -
    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
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition SmartProjectionPoseFactor.h:152
    │ │ │ │ -
    SmartProjectionPoseFactor()
    Default constructor, only for serialization.
    Definition SmartProjectionPoseFactor.h:64
    │ │ │ │ +
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition TriangulationFactor.h:33
    │ │ │ │ +
    VerticalBlockMatrix Ab
    thread-safe (?) scratch memory for linearize
    Definition TriangulationFactor.h:140
    │ │ │ │ +
    const bool throwCheirality_
    If true, rethrows Cheirality exceptions (default: false)
    Definition TriangulationFactor.h:56
    │ │ │ │ +
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition TriangulationFactor.h:185
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition TriangulationFactor.h:106
    │ │ │ │ +
    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::NonlinearFactor::shared_ptr clone() const override
    Definition TriangulationFactor.h:96
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    typename CAMERA::Measurement Measurement
    shorthand for measurement type, e.g. Point2 or StereoPoint2
    Definition TriangulationFactor.h:49
    │ │ │ │ +
    const Measurement & measured() const
    return the measurement
    Definition TriangulationFactor.h:175
    │ │ │ │ +
    bool verboseCheirality() const
    return verbosity
    Definition TriangulationFactor.h:180
    │ │ │ │ +
    CAMERA Camera
    CAMERA type.
    Definition TriangulationFactor.h:38
    │ │ │ │ +
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition TriangulationFactor.h:115
    │ │ │ │ +
    ~TriangulationFactor() override
    Virtual destructor.
    Definition TriangulationFactor.h:92
    │ │ │ │ +
    const bool verboseCheirality_
    If true, prints text for Cheirality exceptions (default: false)
    Definition TriangulationFactor.h:57
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition TriangulationFactor.h:192
    │ │ │ │ +
    TriangulationFactor()
    Default constructor.
    Definition TriangulationFactor.h:66
    │ │ │ │ +
    TriangulationFactor< CAMERA > This
    shorthand for this class
    Definition TriangulationFactor.h:46
    │ │ │ │ +
    const Measurement measured_
    2D measurement
    Definition TriangulationFactor.h:53
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition TriangulationFactor.h:63
    │ │ │ │ +
    const CAMERA camera_
    CAMERA in which this landmark was seen.
    Definition TriangulationFactor.h:52
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,125 +1,189 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 { │ │ │ │ │ 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); │ │ │ │ │ -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) │ │ │ │ │ +_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. │ │ │ │ │ _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. │ │ │ │ │ @@ -128,109 +192,147 @@ │ │ │ │ │ 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_:_:_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_:_:_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_:_:_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_:_:_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 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 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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 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 │ │ │ │ │ - * _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 │ │ │ │ │ + * 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/a01316_source.html │ │ │ │ @@ -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/slam/ReferenceFrameFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,99 +98,153 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JacobianFactorSVD.h
    │ │ │ │ +
    ReferenceFrameFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -
    1/*
    │ │ │ │ -
    2 * @file JacobianFactorSVD.h
    │ │ │ │ -
    3 * @date Oct 27, 2013
    │ │ │ │ -
    4 * @uthor Frank Dellaert
    │ │ │ │ -
    5 */
    │ │ │ │ -
    6
    │ │ │ │ -
    7#pragma once
    │ │ │ │ - │ │ │ │ -
    9
    │ │ │ │ -
    10namespace gtsam {
    │ │ │ │ -
    28template<size_t D, size_t ZDim>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ +
    1/* ----------------------------------------------------------------------------
    │ │ │ │ +
    2
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * All Rights Reserved
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    21
    │ │ │ │ +
    22namespace gtsam {
    │ │ │ │ +
    23
    │ │ │ │ +
    29template<class T, class P>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    39 }
    │ │ │ │ -
    │ │ │ │ -
    40
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ +
    56template<class POINT, class TRANSFORM>
    │ │ │ │ +
    │ │ │ │ +
    57class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
    │ │ │ │ +
    58protected:
    │ │ │ │ + │ │ │ │ +
    61
    │ │ │ │ +
    62public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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(); }
    │ │ │ │ +
    120
    │ │ │ │ +
    121private:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    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
    │ │ │ │ +
    Matrix trans(const Matrix &A)
    static transpose function, just calls Eigen transpose member function
    Definition Matrix.h:242
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ -
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ -
    JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.
    Definition JacobianFactorSVD.h:29
    │ │ │ │ -
    JacobianFactorSVD()
    Default constructor.
    Definition JacobianFactorSVD.h:38
    │ │ │ │ -
    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
    │ │ │ │ -
    JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
    Empty constructor with keys.
    Definition JacobianFactorSVD.h:42
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition NonlinearFactor.h:218
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
    Definition ReferenceFrameFactor.h:57
    │ │ │ │ +
    void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition ReferenceFrameFactor.h:107
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition ReferenceFrameFactor.h:123
    │ │ │ │ +
    ReferenceFrameFactor()
    default constructor for serialization only
    Definition ReferenceFrameFactor.h:60
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,116 +1,203 @@ │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +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} │ │ │ │ │ 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. │ │ │ │ │ +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) │ │ │ │ │ +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(); } │ │ │ │ │ +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_:_:_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_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +&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 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 │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrSSVVDD..hh │ │ │ │ │ + * 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/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/JacobianFactorQR.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,193 +98,95 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BoundingConstraint.h
    │ │ │ │ +
    JacobianFactorQR.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>
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    12
    │ │ │ │ +
    13namespace gtsam {
    │ │ │ │ +
    14
    │ │ │ │ +
    15class GaussianBayesNet;
    │ │ │ │ +
    16
    │ │ │ │ +
    20template<size_t D, size_t ZDim>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    32template<class VALUE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    34 typedef VALUE X;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    23 typedef RegularJacobianFactor<D> Base;
    │ │ │ │ +
    24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
    │ │ │ │ +
    25
    │ │ │ │ +
    26public:
    │ │ │ │ +
    27
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ -
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
    Definition BoundingConstraint.h:33
    │ │ │ │ -
    bool active(const Values &c) const override
    active when constraint NOT met
    Definition BoundingConstraint.h:61
    │ │ │ │ -
    BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
    flag for greater/less than
    Definition BoundingConstraint.h:41
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BoundingConstraint.h:85
    │ │ │ │ -
    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...
    │ │ │ │ -
    Binary scalar inequality constraint, with a similar value() function to implement for specific system...
    Definition BoundingConstraint.h:101
    │ │ │ │ -
    bool active(const Values &c) const override
    active when constraint NOT met
    Definition BoundingConstraint.h:130
    │ │ │ │ -
    BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
    flag for greater/less than
    Definition BoundingConstraint.h:111
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition BoundingConstraint.h:159
    │ │ │ │ -
    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...
    │ │ │ │ +
    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
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ +
    void add(const GaussianFactor &factor)
    Add a factor by value - makes a copy.
    Definition GaussianFactorGraph.h:127
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ +
    JacobianFactor for Schur complement that uses Q noise model.
    Definition JacobianFactorQR.h:21
    │ │ │ │ +
    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,232 +1,129 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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) │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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_; │ │ │ │ │ -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 │ │ │ │ │ -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. │ │ │ │ │ +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: │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ + * 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/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/JacobianFactorQ.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,245 +98,119 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    TriangulationFactor.h
    │ │ │ │ +
    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 * 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22#include <boost/make_shared.hpp>
    │ │ │ │ -
    23#include <boost/lexical_cast.hpp>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    32template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ + │ │ │ │ +
    21
    │ │ │ │ +
    22namespace gtsam {
    │ │ │ │ +
    26template<size_t D, size_t ZDim>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    28
    │ │ │ │ +
    29 typedef RegularJacobianFactor<D> Base;
    │ │ │ │ +
    30 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
    │ │ │ │ +
    31 typedef std::pair<Key, Matrix> KeyMatrix;
    │ │ │ │ +
    32
    │ │ │ │ +
    33public:
    │ │ │ │
    34
    │ │ │ │ -
    35public:
    │ │ │ │ -
    36
    │ │ │ │ -
    38 using Camera = CAMERA;
    │ │ │ │ -
    39
    │ │ │ │ -
    40protected:
    │ │ │ │ -
    41
    │ │ │ │ - │ │ │ │ -
    44
    │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    49 using Measurement = typename CAMERA::Measurement;
    │ │ │ │ -
    50
    │ │ │ │ -
    51 // Keep a copy of measurement and calibration for I/O
    │ │ │ │ -
    52 const CAMERA camera_;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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_(
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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");
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ - │ │ │ │ -
    130 std::cout << e.what() << ": Landmark "
    │ │ │ │ -
    131 << DefaultKeyFormatter(this->key()) << " moved behind camera"
    │ │ │ │ -
    132 << std::endl;
    │ │ │ │ - │ │ │ │ -
    134 throw e;
    │ │ │ │ -
    135 return camera_.defaultErrorWhenTriangulatingBehindCamera();
    │ │ │ │ -
    136 }
    │ │ │ │ -
    137 }
    │ │ │ │ -
    │ │ │ │ -
    138
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Calibrated camera for which only pose is unknown.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    37 }
    │ │ │ │ +
    │ │ │ │ +
    38
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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};
    │ │ │ │ +
    │ │ │ │ +
    80
    │ │ │ │ +
    81}
    │ │ │ │ +
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ -
    Definition CalibratedCamera.h:32
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    virtual bool active(const Values &) const
    Checks whether a factor should be used based on a set of values.
    Definition NonlinearFactor.h:118
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition TriangulationFactor.h:33
    │ │ │ │ -
    VerticalBlockMatrix Ab
    thread-safe (?) scratch memory for linearize
    Definition TriangulationFactor.h:140
    │ │ │ │ -
    const bool throwCheirality_
    If true, rethrows Cheirality exceptions (default: false)
    Definition TriangulationFactor.h:56
    │ │ │ │ -
    bool throwCheirality() const
    return flag for throwing cheirality exceptions
    Definition TriangulationFactor.h:185
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition TriangulationFactor.h:106
    │ │ │ │ -
    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::NonlinearFactor::shared_ptr clone() const override
    Definition TriangulationFactor.h:96
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    typename CAMERA::Measurement Measurement
    shorthand for measurement type, e.g. Point2 or StereoPoint2
    Definition TriangulationFactor.h:49
    │ │ │ │ -
    const Measurement & measured() const
    return the measurement
    Definition TriangulationFactor.h:175
    │ │ │ │ -
    bool verboseCheirality() const
    return verbosity
    Definition TriangulationFactor.h:180
    │ │ │ │ -
    CAMERA Camera
    CAMERA type.
    Definition TriangulationFactor.h:38
    │ │ │ │ -
    bool equals(const NonlinearFactor &p, double tol=1e-9) const override
    equals
    Definition TriangulationFactor.h:115
    │ │ │ │ -
    ~TriangulationFactor() override
    Virtual destructor.
    Definition TriangulationFactor.h:92
    │ │ │ │ -
    const bool verboseCheirality_
    If true, prints text for Cheirality exceptions (default: false)
    Definition TriangulationFactor.h:57
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition TriangulationFactor.h:192
    │ │ │ │ -
    TriangulationFactor()
    Default constructor.
    Definition TriangulationFactor.h:66
    │ │ │ │ -
    TriangulationFactor< CAMERA > This
    shorthand for this class
    Definition TriangulationFactor.h:46
    │ │ │ │ -
    const Measurement measured_
    2D measurement
    Definition TriangulationFactor.h:53
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shorthand for a smart pointer to a factor
    Definition TriangulationFactor.h:63
    │ │ │ │ -
    const CAMERA camera_
    CAMERA in which this landmark was seen.
    Definition TriangulationFactor.h:52
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ +
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ +
    JacobianFactor for Schur complement that uses Q noise model.
    Definition JacobianFactorQ.h:27
    │ │ │ │ +
    JacobianFactorQ()
    Default constructor.
    Definition JacobianFactorQ.h:36
    │ │ │ │ +
    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
    │ │ │ │ +
    JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
    Empty constructor with keys.
    Definition JacobianFactorQ.h:40
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,338 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TriangulationFactor.h │ │ │ │ │ +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 * 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 <_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 { │ │ │ │ │ +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: │ │ │ │ │ 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 { │ │ │ │ │ -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. │ │ │ │ │ +_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 │ │ │ │ │ +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_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -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 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() │ │ │ │ │ +_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 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 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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * TTrriiaanngguullaattiioonnFFaaccttoorr..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/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/EssentialMatrixFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,146 +98,407 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PoseRotationPrior.h
    │ │ │ │ +
    EssentialMatrixFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -Go to the documentation of this file.
    1
    │ │ │ │ -
    10#pragma once
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    14
    │ │ │ │ -
    15
    │ │ │ │ -
    16namespace gtsam {
    │ │ │ │ -
    17
    │ │ │ │ -
    18template<class POSE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    20public:
    │ │ │ │ -
    21
    │ │ │ │ -
    22 typedef PoseRotationPrior<POSE> This;
    │ │ │ │ - │ │ │ │ -
    24 typedef POSE Pose;
    │ │ │ │ -
    25 typedef typename POSE::Translation Translation;
    │ │ │ │ -
    26 typedef typename POSE::Rotation Rotation;
    │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    35
    │ │ │ │ -
    36protected:
    │ │ │ │ -
    37
    │ │ │ │ -
    38 Rotation measured_;
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27#include <iostream>
    │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    │ │ │ │ +
    34class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
    │ │ │ │ +
    35 Vector3 vA_, vB_;
    │ │ │ │ +
    36
    │ │ │ │ + │ │ │ │ + │ │ │ │
    39
    │ │ │ │ -
    40public:
    │ │ │ │ -
    41
    │ │ │ │ - │ │ │ │ -
    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()) {}
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ +
    40 public:
    │ │ │ │ +
    │ │ │ │ +
    49 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
    │ │ │ │ +
    50 const SharedNoiseModel& model)
    │ │ │ │ +
    51 : Base(model, key) {
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    54 }
    │ │ │ │ +
    │ │ │ │ +
    55
    │ │ │ │ +
    65 template <class CALIBRATION>
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    88
    │ │ │ │ -
    89private:
    │ │ │ │
    90
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    102};
    │ │ │ │
    │ │ │ │ -
    101
    │ │ │ │ -
    102} // \namespace gtsam
    │ │ │ │
    103
    │ │ │ │ -
    104
    │ │ │ │ -
    105
    │ │ │ │ -
    106
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    109 : public NoiseModelFactorN<EssentialMatrix, double> {
    │ │ │ │ +
    110 Point3 dP1_;
    │ │ │ │ +
    111 Point2 pn_;
    │ │ │ │ +
    112 double f_;
    │ │ │ │ +
    113
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    223};
    │ │ │ │ +
    │ │ │ │ +
    224// EssentialMatrixFactor2
    │ │ │ │ +
    225
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    306};
    │ │ │ │ +
    │ │ │ │ +
    307// EssentialMatrixFactor3
    │ │ │ │ +
    308
    │ │ │ │ +
    322template <class CALIBRATION>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    324 : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
    │ │ │ │ +
    325 private:
    │ │ │ │ +
    326 Point2 pA_, pB_;
    │ │ │ │ +
    327
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    406};
    │ │ │ │ +
    │ │ │ │ +
    407// EssentialMatrixFactor4
    │ │ │ │ +
    408
    │ │ │ │ +
    409} // namespace gtsam
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static Vector3 Homogeneous(const Point2 &p)
    Static function to convert Point2 to homogeneous coordinates.
    Definition EssentialMatrix.h:34
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    Definition PoseRotationPrior.h:19
    │ │ │ │ -
    PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
    standard constructor
    Definition PoseRotationPrior.h:46
    │ │ │ │ -
    PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
    Constructor that pulls the translation from an incoming POSE.
    Definition PoseRotationPrior.h:50
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition PoseRotationPrior.h:56
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals specialized to this factor
    Definition PoseRotationPrior.h:66
    │ │ │ │ -
    PoseRotationPrior()
    default constructor - only use for serialization
    Definition PoseRotationPrior.h:43
    │ │ │ │ -
    Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
    h(x)-z
    Definition PoseRotationPrior.h:78
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print contents
    Definition PoseRotationPrior.h:72
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition PoseRotationPrior.h:92
    │ │ │ │ -
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    Factor that evaluates epipolar error p'Ep for given essential matrix.
    Definition EssentialMatrixFactor.h:34
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:76
    │ │ │ │ +
    EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:49
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:82
    │ │ │ │ +
    EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
    Constructor.
    Definition EssentialMatrixFactor.h:66
    │ │ │ │ +
    Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 1D vector
    Definition EssentialMatrixFactor.h:92
    │ │ │ │ +
    Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
    Definition EssentialMatrixFactor.h:109
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:154
    │ │ │ │ +
    EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
    Constructor.
    Definition EssentialMatrixFactor.h:144
    │ │ │ │ +
    EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:126
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:160
    │ │ │ │ +
    Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
    Definition EssentialMatrixFactor.h:231
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:273
    │ │ │ │ +
    EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:247
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:267
    │ │ │ │ +
    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
    │ │ │ │ +
    Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (...
    Definition EssentialMatrixFactor.h:324
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition EssentialMatrixFactor.h:355
    │ │ │ │ +
    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
    │ │ │ │ +
    EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixFactor.h:344
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixFactor.h:349
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,115 +1,347 @@ │ │ │ │ │ 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 │ │ │ │ │ +EssentialMatrixFactor.h │ │ │ │ │ +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 │ │ │ │ │ -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 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 │ │ │ │ │ -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_; │ │ │ │ │ +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 │ │ │ │ │ +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_; │ │ │ │ │ +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 │ │ │ │ │ -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()) {} │ │ │ │ │ -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: │ │ │ │ │ +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 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 │ │ │ │ │ +_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 │ │ │ │ │ -104 │ │ │ │ │ -105 │ │ │ │ │ -106 │ │ │ │ │ +_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. │ │ │ │ │ _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. │ │ │ │ │ @@ -117,70 +349,140 @@ │ │ │ │ │ _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_:_:_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_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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,... │ │ │ │ │ +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 │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ + * 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/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/FrobeniusFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,43 @@ │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    InitializePose.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    FrobeniusFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ +

    Various factors that minimize some Frobenius norm. │ │ │ │ More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ 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

    │ │ │ │ -

    common code between lago.* (2D) and InitializePose3.* (3D)

    │ │ │ │ +

    Various factors that minimize some Frobenius norm.

    │ │ │ │ +
    Date
    March 2019
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    August, 2020
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,29 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ -_G_o_ _t_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 | _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 ********** │ │ │ │ │ -common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ │ +Various factors that minimize some Frobenius norm. │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - August, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._h │ │ │ │ │ + * _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/a01331.html │ │ │ │ @@ -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/slam/SmartFactorParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    InitializePose3.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Enumerations
    │ │ │ │ +
    SmartFactorParams.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Collect common parameters for SmartProjection and SmartStereoProjection factors. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    struct  gtsam::SmartProjectionParams
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ 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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ +

    Collect common parameters for SmartProjection and SmartStereoProjection factors.

    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Zsolt Kira
    │ │ │ │
    │ │ │ │ Frank Dellaert
    │ │ │ │ -
    Date
    August, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,38 @@ │ │ │ │ │ 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 | _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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ +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 ********** │ │ │ │ │ +Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ +factors. │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ + Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - August, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/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/RotateFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,142 +98,170 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PoseTranslationPrior.h
    │ │ │ │ +
    RotateFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ -Go to the documentation of this file.
    1
    │ │ │ │ -
    10#pragma once
    │ │ │ │ -
    11
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    1/*
    │ │ │ │ +
    2 * @file RotateFactor.cpp
    │ │ │ │ +
    3 * @brief RotateFactor class
    │ │ │ │ +
    4 * @author Frank Dellaert
    │ │ │ │ +
    5 * @date December 17, 2013
    │ │ │ │ +
    6 */
    │ │ │ │ +
    7
    │ │ │ │ +
    8#pragma once
    │ │ │ │ +
    9
    │ │ │ │ + │ │ │ │ +
    11#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    12
    │ │ │ │ +
    13namespace gtsam {
    │ │ │ │
    14
    │ │ │ │ -
    15namespace gtsam {
    │ │ │ │ -
    16
    │ │ │ │ -
    20template<class POSE>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    22public:
    │ │ │ │ -
    23 typedef PoseTranslationPrior<POSE> This;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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()) {
    │ │ │ │ +
    │ │ │ │ +
    23class RotateFactor: public NoiseModelFactorN<Rot3> {
    │ │ │ │ +
    24
    │ │ │ │ +
    25 Point3 p_, z_;
    │ │ │ │ +
    26
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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))); }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ +
    61};
    │ │ │ │
    │ │ │ │ +
    62
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    68
    │ │ │ │ +
    69 Unit3 i_p_, c_z_;
    │ │ │ │ +
    70
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    73
    │ │ │ │ +
    74public:
    │ │ │ │
    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);
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    87
    │ │ │ │ -
    88private:
    │ │ │ │
    89
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ +
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    101
    │ │ │ │ -
    102} // \namespace gtsam
    │ │ │ │
    103
    │ │ │ │ -
    104
    │ │ │ │ -
    105
    │ │ │ │ -
    106
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    117};
    │ │ │ │ +
    │ │ │ │ +
    118} // namespace gtsam
    │ │ │ │ +
    119
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    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
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Vector.
    Definition Unit3.cpp:151
    │ │ │ │ +
    void print(const std::string &s=std::string()) const
    The print fuction.
    Definition Unit3.cpp:164
    │ │ │ │ +
    Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
    Signed, vector-valued error between two directions.
    Definition Unit3.cpp:199
    │ │ │ │
    virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print
    Definition Factor.cpp:29
    │ │ │ │ -
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ -
    A prior on the translation part of a pose.
    Definition PoseTranslationPrior.h:21
    │ │ │ │ -
    PoseTranslationPrior(Key key, const Translation &measured, const noiseModel::Base::shared_ptr &model)
    standard constructor
    Definition PoseTranslationPrior.h:43
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
    h(x)-z
    Definition PoseTranslationPrior.h:62
    │ │ │ │ -
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    equals specialized to this factor
    Definition PoseTranslationPrior.h:77
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition PoseTranslationPrior.h:57
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition PoseTranslationPrior.h:91
    │ │ │ │ -
    PoseTranslationPrior()
    default constructor - only use for serialization
    Definition PoseTranslationPrior.h:40
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print contents
    Definition PoseTranslationPrior.h:83
    │ │ │ │ -
    Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Whic...
    Definition RotateFactor.h:23
    │ │ │ │ +
    RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
    Constructor.
    Definition RotateFactor.h:33
    │ │ │ │ +
    Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 2D vector
    Definition RotateFactor.h:53
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RotateFactor.h:44
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition RotateFactor.h:39
    │ │ │ │ +
    Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than...
    Definition RotateFactor.h:67
    │ │ │ │ +
    Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
    vector of errors returns 2D vector
    Definition RotateFactor.h:105
    │ │ │ │ +
    RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
    Constructor.
    Definition RotateFactor.h:77
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition RotateFactor.h:96
    │ │ │ │ +
    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::NonlinearFactor::shared_ptr clone() const override
    Definition RotateFactor.h:91
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,182 +1,226 @@ │ │ │ │ │ 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 │ │ │ │ │ -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> │ │ │ │ │ +RotateFactor.h │ │ │ │ │ +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 <_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 │ │ │ │ │ -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()) { │ │ │ │ │ +_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 │ │ │ │ │ -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))); } │ │ │ │ │ +_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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 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); │ │ │ │ │ +_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 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_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 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 │ │ │ │ │ +_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 │ │ │ │ │ -104 │ │ │ │ │ -105 │ │ │ │ │ -106 │ │ │ │ │ +_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 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 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 │ │ │ │ │ +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 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,... │ │ │ │ │ +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 │ │ │ │ │ - * _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_._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/a01337.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,57 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    EssentialMatrixConstraint.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    FrobeniusFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ +

    Various factors that minimize some Frobenius norm. │ │ │ │ +More...

    │ │ │ │ + │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::EssentialMatrixConstraint
     Binary factor between two Pose3 variables induced by an EssentialMatrix measurement. More...
    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.
     
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ +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,27 +1,44 @@ │ │ │ │ │ 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 │ │ │ │ │ -EssentialMatrixConstraint.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 │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ +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_._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/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/FrobeniusFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,127 +98,181 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EssentialMatrixConstraint.h
    │ │ │ │ +
    FrobeniusFactor.h
    │ │ │ │
    │ │ │ │
    │ │ │ │ Go to the documentation of this file.
    1/* ----------------------------------------------------------------------------
    │ │ │ │
    2
    │ │ │ │ -
    3 * GTSAM Copyright 2010-2014, 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
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/geometry/EssentialMatrix.h>
    │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ +
    21#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    22#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    23#include <gtsam/geometry/SOn.h>
    │ │ │ │ + │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ -
    30class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
    │ │ │ │ -
    31
    │ │ │ │ -
    32private:
    │ │ │ │ -
    33
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    36
    │ │ │ │ -
    37 EssentialMatrix measuredE_;
    │ │ │ │ -
    39public:
    │ │ │ │ -
    40
    │ │ │ │ -
    41 // shorthand for a smart pointer to a factor
    │ │ │ │ -
    42 typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
    │ │ │ │ -
    43
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    46 }
    │ │ │ │ -
    │ │ │ │ -
    47
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    56 const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
    │ │ │ │ -
    57 Base(model, key1, key2), measuredE_(measuredE) {
    │ │ │ │ -
    58 }
    │ │ │ │ -
    │ │ │ │ -
    59
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    105};
    │ │ │ │ -
    │ │ │ │ -
    106// \class EssentialMatrixConstraint
    │ │ │ │ -
    107
    │ │ │ │ -
    108}
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    N*N matrix representation of SO(N).
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::string demangle(const char *name)
    Pretty print Value type name.
    Definition types.cpp:37
    │ │ │ │ +
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    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
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
    Definition EssentialMatrixConstraint.h:30
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition EssentialMatrixConstraint.h:64
    │ │ │ │ -
    const EssentialMatrix & measured() const
    return the measured
    Definition EssentialMatrixConstraint.h:86
    │ │ │ │ -
    boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
    The measurement is an essential matrix.
    Definition EssentialMatrixConstraint.h:42
    │ │ │ │ -
    EssentialMatrixConstraint()
    default constructor - only use for serialization
    Definition EssentialMatrixConstraint.h:45
    │ │ │ │ -
    EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
    Constructor.
    Definition EssentialMatrixConstraint.h:55
    │ │ │ │ +
    FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
    Definition FrobeniusFactor.h:51
    │ │ │ │ +
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition FrobeniusFactor.h:60
    │ │ │ │ +
    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
    │ │ │ │ +
    FrobeniusFactor calculates the Frobenius norm between rotation matrices.
    Definition FrobeniusFactor.h:78
    │ │ │ │ +
    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
    │ │ │ │ +
    FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
    Constructor.
    Definition FrobeniusFactor.h:83
    │ │ │ │ +
    FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
    Definition FrobeniusFactor.h:104
    │ │ │ │ +
    FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
    Construct from two keys and measured rotation.
    Definition FrobeniusFactor.h:117
    │ │ │ │ +
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print with optional string
    Definition FrobeniusFactor.h:130
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
    assert equality up to a tolerance
    Definition FrobeniusFactor.h:140
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,159 +1,226 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EssentialMatrixConstraint.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-2014, 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_/_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 { │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ -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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_:_:_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) │ │ │ │ │ +_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 EssentialMatrixConstraint.h:55 │ │ │ │ │ +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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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/a01340_source.html │ │ │ │ @@ -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/slam/BoundingConstraint.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,119 +98,193 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JacobianFactorQ.h
    │ │ │ │ +
    BoundingConstraint.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
    │ │ │ │
    19
    │ │ │ │ - │ │ │ │ -
    21
    │ │ │ │ -
    22namespace gtsam {
    │ │ │ │ -
    26template<size_t D, size_t ZDim>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    28
    │ │ │ │ -
    29 typedef RegularJacobianFactor<D> Base;
    │ │ │ │ -
    30 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
    │ │ │ │ -
    31 typedef std::pair<Key, Matrix> KeyMatrix;
    │ │ │ │ -
    32
    │ │ │ │ -
    33public:
    │ │ │ │ -
    34
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    37 }
    │ │ │ │ -
    │ │ │ │ -
    38
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    20#include <gtsam/base/Lie.h>
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    32template<class VALUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    34 typedef VALUE X;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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// traits
    │ │ │ │ -
    │ │ │ │ -
    77template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
    │ │ │ │ -
    78 JacobianFactorQ<D, ZDim> > {
    │ │ │ │ -
    79};
    │ │ │ │ -
    │ │ │ │ -
    80
    │ │ │ │ -
    81}
    │ │ │ │ -
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ +
    76 if (isGreaterThan_)
    │ │ │ │ +
    77 return (Vector(1) << error).finished();
    │ │ │ │ +
    78 else
    │ │ │ │ +
    79 return -1.0 * (Vector(1) << error).finished();
    │ │ │ │ +
    80 }
    │ │ │ │ +
    81
    │ │ │ │ +
    82private:
    │ │ │ │ +
    83
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ -
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ -
    JacobianFactor for Schur complement that uses Q noise model.
    Definition JacobianFactorQ.h:27
    │ │ │ │ -
    JacobianFactorQ()
    Default constructor.
    Definition JacobianFactorQ.h:36
    │ │ │ │ -
    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
    │ │ │ │ -
    JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
    Empty constructor with keys.
    Definition JacobianFactorQ.h:40
    │ │ │ │ +
    double error(const Values &c) const override
    Calculate the error of the factor.
    Definition NonlinearFactor.cpp:138
    │ │ │ │ +
    const SharedNoiseModel & noiseModel() const
    access to the noise model
    Definition NonlinearFactor.h:223
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
    Definition BoundingConstraint.h:33
    │ │ │ │ +
    bool active(const Values &c) const override
    active when constraint NOT met
    Definition BoundingConstraint.h:61
    │ │ │ │ +
    BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
    flag for greater/less than
    Definition BoundingConstraint.h:41
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BoundingConstraint.h:85
    │ │ │ │ +
    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...
    │ │ │ │ +
    Binary scalar inequality constraint, with a similar value() function to implement for specific system...
    Definition BoundingConstraint.h:101
    │ │ │ │ +
    bool active(const Values &c) const override
    active when constraint NOT met
    Definition BoundingConstraint.h:130
    │ │ │ │ +
    BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
    flag for greater/less than
    Definition BoundingConstraint.h:111
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition BoundingConstraint.h:159
    │ │ │ │ +
    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,141 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactorQ.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 │ │ │ │ │ -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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ +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_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 │ │ │ │ │ +_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// 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 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_; │ │ │ │ │ +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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrQQ..hh │ │ │ │ │ + * _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/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/SymbolicBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,76 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    SymbolicFactor-inst.h File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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 Orderingkeys 
    )
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -

    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
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +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 │ │ │ │ │ -SymbolicFactor-inst.h 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. │ │ │ │ │   │ │ │ │ │ -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_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ + * _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/a01343.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01343 = [ │ │ │ │ │ - ["EliminateSymbolic", "a01343.html#ae394348e1a503834f713405afa9afad8", null] │ │ │ │ │ + ["gtsam::traits< SymbolicBayesNet >", "a04976.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01343_source.html │ │ │ │ @@ -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/symbolic/SymbolicBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,99 +98,146 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymbolicFactor-inst.h
    │ │ │ │ +
    SymbolicBayesNet.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ -
    24#include <gtsam/base/timing.h>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <gtsam/base/types.h>
    │ │ │ │
    25
    │ │ │ │ -
    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);
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    │ │ │ │ +
    32 class SymbolicBayesNet : public BayesNet<SymbolicConditional> {
    │ │ │ │ +
    33 public:
    │ │ │ │ + │ │ │ │ +
    35 typedef SymbolicBayesNet This;
    │ │ │ │ + │ │ │ │ +
    37 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    38 typedef boost::shared_ptr<ConditionalType> sharedConditional;
    │ │ │ │ +
    39
    │ │ │ │
    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 }
    │ │ │ │ -
    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 utilities.
    │ │ │ │ - │ │ │ │ -
    The base class for all factors.
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    61 : Base(graph) {}
    │ │ │ │ +
    │ │ │ │ +
    62
    │ │ │ │ +
    │ │ │ │ +
    67 SymbolicBayesNet(std::initializer_list<boost::shared_ptr<SymbolicConditional>> conditionals)
    │ │ │ │ +
    68 : Base(conditionals) {}
    │ │ │ │ +
    │ │ │ │ +
    69
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    72 push_back(boost::make_shared<SymbolicConditional>(c));
    │ │ │ │ +
    73 }
    │ │ │ │ +
    │ │ │ │ +
    74
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Bayes network.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print out graph
    Definition BayesNet-inst.h:32
    │ │ │ │
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:121
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ +
    A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
    Definition SymbolicBayesNet.h:32
    │ │ │ │ +
    virtual ~SymbolicBayesNet()
    Destructor.
    Definition SymbolicBayesNet.h:87
    │ │ │ │ +
    SymbolicBayesNet(SymbolicConditional &&c)
    Construct from a single conditional.
    Definition SymbolicBayesNet.h:71
    │ │ │ │ +
    GTSAM_EXPORT void print(const std::string &s="SymbolicBayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition SymbolicBayesNet.h:98
    │ │ │ │ +
    SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    Construct from iterator over conditionals.
    Definition SymbolicBayesNet.h:48
    │ │ │ │ +
    SymbolicBayesNet(std::initializer_list< boost::shared_ptr< SymbolicConditional > > conditionals)
    Constructor that takes an initializer list of shared pointers.
    Definition SymbolicBayesNet.h:67
    │ │ │ │ +
    SymbolicBayesNet(const CONTAINER &conditionals)
    Construct from container of factors (shared_ptr or plain objects)
    Definition SymbolicBayesNet.h:53
    │ │ │ │ +
    SymbolicBayesNet()
    Construct empty factor graph.
    Definition SymbolicBayesNet.h:44
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition SymbolicBayesNet.h:108
    │ │ │ │ +
    SymbolicBayesNet & operator()(SymbolicConditional &&c)
    Add a single conditional and return a reference.
    Definition SymbolicBayesNet.h:81
    │ │ │ │ +
    SymbolicBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition SymbolicBayesNet.h:60
    │ │ │ │ +
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,120 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicFactor-inst.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 │ │ │ │ │ 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_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._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_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> │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_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 │ │ │ │ │ +_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. │ │ │ │ │ _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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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_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_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/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/SymbolicEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicISAM.h File Reference
    │ │ │ │ +
    SymbolicEliminationTree.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::SymbolicISAM
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    July 29, 2013
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -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_I_S_A_M │ │ │ │ │ -  │ │ │ │ │ +_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 │ │ │ │ │ - July 29, 2013 │ │ │ │ │ + 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_I_S_A_M_._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/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/SymbolicFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,33 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicJunctionTree.cpp File Reference
    │ │ │ │ +
    SymbolicFactorGraph.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    Mar 29, 2013
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Date
    Oct 29, 2009
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicJunctionTree.cpp File Reference │ │ │ │ │ +SymbolicFactorGraph.cpp File Reference │ │ │ │ │ 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_J_u_n_c_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_._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/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/SymbolicJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,43 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    SymbolicConditional.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    SymbolicJunctionTree.h 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...
    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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Oct 17, 2010
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -SymbolicConditional.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_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_._._. │ │ │ │ │ +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 │ │ │ │ │ + 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_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/a01352.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01352 = [ │ │ │ │ │ - ["gtsam::SymbolicConditional", "a04996.html", "a04996"], │ │ │ │ │ - ["gtsam::traits< SymbolicConditional >", "a05000.html", null] │ │ │ │ │ + ["SymbolicCluster", "a01352.html#ad78d0a5ba773ceac7ff253525032a15e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01352_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,168 +98,68 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymbolicConditional.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
    │ │ │ │
    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/base/types.h>
    │ │ │ │ -
    22#include <gtsam/inference/Conditional-inst.h>
    │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    │ │ │ │ -
    36 class GTSAM_EXPORT SymbolicConditional :
    │ │ │ │ -
    37 public SymbolicFactor,
    │ │ │ │ -
    38 public Conditional<SymbolicFactor, SymbolicConditional> {
    │ │ │ │ -
    39
    │ │ │ │ -
    40 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    44 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    50
    │ │ │ │ - │ │ │ │ -
    53
    │ │ │ │ - │ │ │ │ +
    27 // Forward declarations
    │ │ │ │ +
    28 class SymbolicEliminationTree;
    │ │ │ │ +
    29
    │ │ │ │ +
    │ │ │ │ +
    50 class GTSAM_EXPORT SymbolicJunctionTree :
    │ │ │ │ +
    51 public JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> {
    │ │ │ │ +
    52 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    55 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │
    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 {
    │ │ │ │ - │ │ │ │ -
    71 (BaseFactor&)result = BaseFactor::FromIterators(firstKey, lastKey);
    │ │ │ │ -
    72 result.nrFrontals_ = nrFrontals;
    │ │ │ │ -
    73 return result;
    │ │ │ │ -
    74 }
    │ │ │ │ +
    65 SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
    │ │ │ │ +
    66 };
    │ │ │ │
    │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ - │ │ │ │ +
    67
    │ │ │ │ + │ │ │ │ +
    70}
    │ │ │ │ +
    The junction tree.
    │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ -
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │ -
    SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3)
    Three parents.
    Definition SymbolicConditional.h:64
    │ │ │ │ -
    SymbolicConditional()
    Empty Constructor to make serialization possible.
    Definition SymbolicConditional.h:52
    │ │ │ │ -
    static SymbolicConditional FromKeys(const CONTAINER &keys, size_t nrFrontals)
    Named constructor from an arbitrary number of keys and frontals.
    Definition SymbolicConditional.h:88
    │ │ │ │ -
    BaseFactor::const_iterator const_iterator
    iterator to keys
    Definition SymbolicConditional.h:46
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Typedef to the conditional base class.
    Definition SymbolicConditional.h:44
    │ │ │ │ -
    Conditional< BaseFactor, This > BaseConditional
    Typedef to the factor base class.
    Definition SymbolicConditional.h:43
    │ │ │ │ -
    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
    │ │ │ │ -
    static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
    Named constructor from an arbitrary number of keys and frontals.
    Definition SymbolicConditional.h:68
    │ │ │ │ -
    SymbolicConditional(Key j, Key parent)
    Single parent.
    Definition SymbolicConditional.h:58
    │ │ │ │ -
    SymbolicConditional(Key j)
    No parents.
    Definition SymbolicConditional.h:55
    │ │ │ │ -
    SymbolicConditional(Key j, Key parent1, Key parent2)
    Two parents.
    Definition SymbolicConditional.h:61
    │ │ │ │ -
    SymbolicFactor::shared_ptr clone() const
    Copy this object as its actual derived type.
    Definition SymbolicConditional.h:99
    │ │ │ │ -
    BaseFactor::iterator iterator
    Boost shared_ptr to this class.
    Definition SymbolicConditional.h:45
    │ │ │ │ -
    SymbolicFactor BaseFactor
    Typedef to this class.
    Definition SymbolicConditional.h:42
    │ │ │ │ -
    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
    │ │ │ │ -
    SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
    Definition SymbolicFactor.h:39
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Overriding the shared_ptr typedef.
    Definition SymbolicFactor.h:48
    │ │ │ │ +
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │ +
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ +
    Definition SymbolicEliminationTree.h:29
    │ │ │ │ +
    A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition SymbolicJunctionTree.h:51
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition SymbolicJunctionTree.h:55
    │ │ │ │ +
    JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base
    Base class.
    Definition SymbolicJunctionTree.h:53
    │ │ │ │ +
    SymbolicJunctionTree This
    This class.
    Definition SymbolicJunctionTree.h:54
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,244 +1,79 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicConditional.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 │ │ │ │ │ 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_/_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> │ │ │ │ │ +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 │ │ │ │ │ -_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) {} │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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_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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +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_C_o_n_d_i_t_i_o_n_a_l_._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/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/SymbolicFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    SymbolicJunctionTree.h File Reference
    │ │ │ │ +Functions
    │ │ │ │ +
    SymbolicFactor.cpp 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...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ -Typedefs

    │ │ │ │ -using gtsam::SymbolicCluster = SymbolicJunctionTree::Cluster
     typedef for wrapper:
     

    │ │ │ │ +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
    Mar 29, 2013
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Oct 17, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _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_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ -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: │ │ │ │ │ +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 │ │ │ │ │ - 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_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _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/a01355.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01355 = [ │ │ │ │ │ - ["SymbolicCluster", "a01355.html#ad78d0a5ba773ceac7ff253525032a15e", null] │ │ │ │ │ + ["EliminateSymbolic", "a01355.html#a659f20993995258b1405737e375efd10", null] │ │ │ │ │ ]; │ │ │ ├── ./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/SymbolicConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicBayesNet.cpp File Reference
    │ │ │ │ +
    SymbolicConditional.h 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

    │ │ │ │ -
    Date
    Oct 29, 2009
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Oct 17, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicBayesNet.cpp File Reference │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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 │ │ │ │ │ + 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_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/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/SymbolicISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicEliminationTree.h File Reference
    │ │ │ │ +
    SymbolicISAM.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
    │ │ │ │ +
    Date
    July 29, 2013
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +SymbolicISAM.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + 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_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._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/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/SymbolicJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicBayesNet.h File Reference
    │ │ │ │ +
    SymbolicJunctionTree.cpp 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
    │ │ │ │ +
    Date
    Mar 29, 2013
    │ │ │ │
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** 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_N_e_t_._h │ │ │ │ │ + * _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/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/SymbolicEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicFactorGraph.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

    │ │ │ │ -
    Date
    Oct 29, 2009
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Mar 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 │ │ │ │ │ -SymbolicFactorGraph.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 │ │ │ │ │ - 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_F_a_c_t_o_r_G_r_a_p_h_._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/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/SymbolicFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,47 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    SymbolicFactorGraph.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    SymbolicFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::EliminationTraits< SymbolicFactorGraph >
    class  gtsam::SymbolicFactor
     SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with any numerical function. More...
     
    class  gtsam::SymbolicFactorGraph
     Symbolic Factor Graph. More...
     
    struct  gtsam::traits< SymbolicFactorGraph >
     traits 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
    Oct 29, 2009
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Oct 17, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -SymbolicFactorGraph.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 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +  _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_._._. │ │ │ │ │   │ │ │ │ │ - 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_ _> │ │ │ │ │ +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 │ │ │ │ │ - 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_G_r_a_p_h_._h │ │ │ │ │ + * _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/a01379.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,5 @@ │ │ │ │ │ var a01379 = [ │ │ │ │ │ - ["gtsam::EliminationTraits< SymbolicFactorGraph >", "a05020.html", "a05020"], │ │ │ │ │ - ["gtsam::SymbolicFactorGraph", "a05024.html", "a05024"], │ │ │ │ │ - ["gtsam::traits< SymbolicFactorGraph >", "a05028.html", null] │ │ │ │ │ + ["gtsam::SymbolicFactor", "a05012.html", "a05012"], │ │ │ │ │ + ["gtsam::traits< SymbolicFactor >", "a05016.html", null], │ │ │ │ │ + ["EliminateSymbolic", "a01379.html#a659f20993995258b1405737e375efd10", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01379_source.html │ │ │ │ @@ -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/symbolic/SymbolicFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,204 +98,216 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymbolicFactorGraph.h
    │ │ │ │ +
    SymbolicFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    37 {
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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); }
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    70 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    46
    │ │ │ │ +
    48 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    49
    │ │ │ │ +
    52
    │ │ │ │ + │ │ │ │ +
    55
    │ │ │ │ +
    │ │ │ │ +
    57 explicit SymbolicFactor(Key j) :
    │ │ │ │ +
    58 Base(KeyVector{j}) {}
    │ │ │ │ +
    │ │ │ │ +
    59
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    74
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    95 std::initializer_list<boost::shared_ptr<SymbolicFactor>> sharedFactors)
    │ │ │ │ -
    96 : Base(sharedFactors) {}
    │ │ │ │ -
    │ │ │ │ -
    97
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    100 push_back(boost::make_shared<SymbolicFactor>(c));
    │ │ │ │ -
    101 }
    │ │ │ │ -
    │ │ │ │ -
    102
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    110 push_back(boost::make_shared<SymbolicFactor>(c));
    │ │ │ │ -
    111 return *this;
    │ │ │ │ -
    112 }
    │ │ │ │ -
    │ │ │ │ -
    113
    │ │ │ │ - │ │ │ │ -
    116
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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);
    │ │ │ │ -
    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
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    Variable elimination algorithms for factor graphs.
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    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
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ -
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    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
    │ │ │ │ -
    A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
    Definition SymbolicBayesNet.h:32
    │ │ │ │ -
    A Bayes tree that represents the connectivity between variables but is not associated with any probab...
    Definition SymbolicBayesTree.h:51
    │ │ │ │
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │ -
    Definition SymbolicEliminationTree.h:29
    │ │ │ │
    SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
    Definition SymbolicFactor.h:39
    │ │ │ │ -
    SymbolicFactor FactorType
    Type of factors in factor graph.
    Definition SymbolicFactorGraph.h:38
    │ │ │ │ -
    SymbolicFactorGraph FactorGraphType
    Type of the factor graph (e.g. GaussianFactorGraph)
    Definition SymbolicFactorGraph.h:39
    │ │ │ │ -
    SymbolicEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition SymbolicFactorGraph.h:42
    │ │ │ │ -
    SymbolicBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition SymbolicFactorGraph.h:41
    │ │ │ │ -
    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
    │ │ │ │ -
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition SymbolicFactorGraph.h:50
    │ │ │ │ -
    SymbolicJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition SymbolicFactorGraph.h:44
    │ │ │ │ -
    SymbolicBayesTree BayesTreeType
    Type of Bayes tree.
    Definition SymbolicFactorGraph.h:43
    │ │ │ │ -
    SymbolicConditional ConditionalType
    Type of conditionals from elimination.
    Definition SymbolicFactorGraph.h:40
    │ │ │ │ -
    Symbolic Factor Graph.
    Definition SymbolicFactorGraph.h:64
    │ │ │ │ -
    SymbolicFactorGraph(SymbolicFactor &&c)
    Construct from a single factor.
    Definition SymbolicFactorGraph.h:99
    │ │ │ │ -
    SymbolicFactorGraph & operator()(SymbolicFactor &&c)
    Add a single factor and return a reference.
    Definition SymbolicFactorGraph.h:109
    │ │ │ │ -
    SymbolicFactorGraph()
    Construct empty factor graph.
    Definition SymbolicFactorGraph.h:76
    │ │ │ │ -
    SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition SymbolicFactorGraph.h:88
    │ │ │ │ -
    SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > > sharedFactors)
    Constructor that takes an initializer list of shared pointers.
    Definition SymbolicFactorGraph.h:94
    │ │ │ │ -
    void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition SymbolicFactorGraph.h:125
    │ │ │ │ -
    EliminateableFactorGraph< This > BaseEliminateable
    Typedef to base elimination class.
    Definition SymbolicFactorGraph.h:69
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition SymbolicFactorGraph.h:70
    │ │ │ │ -
    SymbolicFactorGraph This
    Typedef to this class.
    Definition SymbolicFactorGraph.h:67
    │ │ │ │ -
    SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition SymbolicFactorGraph.h:80
    │ │ │ │ -
    virtual ~SymbolicFactorGraph()
    Destructor.
    Definition SymbolicFactorGraph.h:115
    │ │ │ │ -
    FactorGraph< SymbolicFactor > Base
    Typedef to base factor graph type.
    Definition SymbolicFactorGraph.h:68
    │ │ │ │ -
    SymbolicFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition SymbolicFactorGraph.h:84
    │ │ │ │ -
    A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition SymbolicJunctionTree.h:51
    │ │ │ │ +
    void print(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition SymbolicFactor.h:96
    │ │ │ │ +
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
    Construct 6-way factor.
    Definition SymbolicFactor.h:77
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Overriding the shared_ptr typedef.
    Definition SymbolicFactor.h:48
    │ │ │ │ +
    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
    │ │ │ │ +
    SymbolicFactor(const Factor &factor)
    Create symbolic version of any factor.
    Definition SymbolicFactor.h:81
    │ │ │ │ +
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4)
    Construct 4-way factor.
    Definition SymbolicFactor.h:69
    │ │ │ │ +
    SymbolicFactor(Key j1, Key j2)
    Construct binary factor.
    Definition SymbolicFactor.h:61
    │ │ │ │ +
    static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:115
    │ │ │ │ +
    void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print only keys
    Definition SymbolicFactor.h:103
    │ │ │ │ +
    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
    │ │ │ │ +
    SymbolicFactor(Key j1, Key j2, Key j3)
    Construct ternary factor.
    Definition SymbolicFactor.h:65
    │ │ │ │ +
    SymbolicFactor(Key j)
    Construct unary factor.
    Definition SymbolicFactor.h:57
    │ │ │ │ +
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5)
    Construct 5-way factor.
    Definition SymbolicFactor.h:73
    │ │ │ │ +
    static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:121
    │ │ │ │ +
    SymbolicFactor()
    Default constructor for I/O.
    Definition SymbolicFactor.h:54
    │ │ │ │ +
    SymbolicFactor::shared_ptr clone() const
    Copy this object as its actual derived type.
    Definition SymbolicFactor.h:86
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,151 +1,168 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicFactorGraph.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 │ │ │ │ │ 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; │ │ │ │ │ +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> │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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) │ │ │ │ │ -{} │ │ │ │ │ +_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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ @@ -162,148 +179,97 @@ │ │ │ │ │ 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_:_:_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ * _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_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/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/SymbolicFactor-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,37 +96,74 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    SymbolicFactor.cpp File Reference
    │ │ │ │ +
    SymbolicFactor-inst.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ 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.
     
    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 Orderingkeys 
    )
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +

    Implementation of dense elimination function for symbolic factors.

    │ │ │ │ +

    This is a templated version for internally doing symbolic elimination on any factor.

    │ │ │ │ + │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +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 │ │ │ │ │ -SymbolicFactor.cpp File Reference │ │ │ │ │ +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_. │ │ │ │ │ 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 │ │ │ │ │ +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) │ │ │ │ │ -  Dense elimination function for symbolic │ │ │ │ │ - factors. │ │ │ │ │ +  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. │ │ │ │ │ * _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_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/a01382.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01382 = [ │ │ │ │ │ - ["EliminateSymbolic", "a01382.html#a659f20993995258b1405737e375efd10", null] │ │ │ │ │ + ["EliminateSymbolic", "a01382.html#ae394348e1a503834f713405afa9afad8", null] │ │ │ │ │ ]; │ │ │ ├── ./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
    │ │ │ │ - │ │ │ │ -
    21#include <gtsam/inference/Key.h>
    │ │ │ │ -
    22#include <gtsam/base/Testable.h>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │
    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:
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ - │ │ │ │ -
    46
    │ │ │ │ -
    48 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    49
    │ │ │ │ -
    52
    │ │ │ │ - │ │ │ │ -
    55
    │ │ │ │ -
    │ │ │ │ -
    57 explicit SymbolicFactor(Key j) :
    │ │ │ │ -
    58 Base(KeyVector{j}) {}
    │ │ │ │ -
    │ │ │ │ -
    59
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ - │ │ │ │ -
    The base class for all factors.
    │ │ │ │ + │ │ │ │ +
    38
    │ │ │ │ +
    40 SymbolicISAM(const SymbolicBayesTree& bayesTree);
    │ │ │ │ +
    41
    │ │ │ │ +
    43
    │ │ │ │ +
    44 };
    │ │ │ │ +
    │ │ │ │ +
    45
    │ │ │ │ +
    46}
    │ │ │ │ +
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │ -
    SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
    Definition SymbolicFactor.h:39
    │ │ │ │ -
    void print(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition SymbolicFactor.h:96
    │ │ │ │ -
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
    Construct 6-way factor.
    Definition SymbolicFactor.h:77
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Overriding the shared_ptr typedef.
    Definition SymbolicFactor.h:48
    │ │ │ │ -
    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
    │ │ │ │ -
    SymbolicFactor(const Factor &factor)
    Create symbolic version of any factor.
    Definition SymbolicFactor.h:81
    │ │ │ │ -
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4)
    Construct 4-way factor.
    Definition SymbolicFactor.h:69
    │ │ │ │ -
    SymbolicFactor(Key j1, Key j2)
    Construct binary factor.
    Definition SymbolicFactor.h:61
    │ │ │ │ -
    static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:115
    │ │ │ │ -
    void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print only keys
    Definition SymbolicFactor.h:103
    │ │ │ │ -
    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
    │ │ │ │ -
    SymbolicFactor(Key j1, Key j2, Key j3)
    Construct ternary factor.
    Definition SymbolicFactor.h:65
    │ │ │ │ -
    SymbolicFactor(Key j)
    Construct unary factor.
    Definition SymbolicFactor.h:57
    │ │ │ │ -
    SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5)
    Construct 5-way factor.
    Definition SymbolicFactor.h:73
    │ │ │ │ -
    static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:121
    │ │ │ │ -
    SymbolicFactor()
    Default constructor for I/O.
    Definition SymbolicFactor.h:54
    │ │ │ │ -
    SymbolicFactor::shared_ptr clone() const
    Copy this object as its actual derived type.
    Definition SymbolicFactor.h:86
    │ │ │ │ -
    the error.
    │ │ │ │ +
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ +
    A Bayes tree that represents the connectivity between variables but is not associated with any probab...
    Definition SymbolicBayesTree.h:51
    │ │ │ │ +
    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/SymbolicFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymbolicISAM.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
    July 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 │ │ │ │ │ -SymbolicISAM.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 │ │ │ │ │ - July 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_I_S_A_M_._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/a01391_source.html │ │ │ │ @@ -114,18 +114,18 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    25
    │ │ │ │
    26namespace gtsam {
    │ │ │ │
    27
    │ │ │ │
    28 // Forward declarations
    │ │ │ │
    29 class SymbolicConditional;
    │ │ │ │
    30
    │ │ │ │
    31 /* ************************************************************************* */
    │ │ │ │ @@ -170,18 +170,18 @@ │ │ │ │ │ │ │ │
    73
    │ │ │ │
    75template<> struct traits<SymbolicBayesTreeClique> : public Testable<SymbolicBayesTreeClique> {};
    │ │ │ │
    76template<> struct traits<SymbolicBayesTree> : public Testable<SymbolicBayesTree> {};
    │ │ │ │
    77
    │ │ │ │
    78} //\ namespace gtsam
    │ │ │ │
    79
    │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Base class for cliques of a BayesTree.
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │
    A clique in a SymbolicBayesTree.
    Definition SymbolicBayesTree.h:35
    │ │ │ ├── ./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.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,29 +93,30 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    global_includes.h File Reference
    │ │ │ │ +
    precompiled_header.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Included from all GTSAM files. │ │ │ │ +

    > │ │ │ │ More...

    │ │ │ │ │ │ │ │

    Go to the source code of this file.

    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Included from all GTSAM files.

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

    >

    │ │ │ │ +

    Include headers that will be included nearly everywhere

    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    November 2018
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,16 +1,18 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +> │ │ │ │ │ +Include headers that will be included nearly everywhere │ │ │ │ │ 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_._h │ │ │ │ │ * Generated on Tue Jan 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_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/global_includes.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
    No Matches
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    global_includes.h
    │ │ │ │ +
    precompiled_header.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,22 +114,82 @@ │ │ │ │
    7
    │ │ │ │
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    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
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    33#include <gtsam/base/FastList.h>
    │ │ │ │ +
    34#include <gtsam/base/FastMap.h>
    │ │ │ │ +
    35#include <gtsam/base/FastSet.h>
    │ │ │ │ + │ │ │ │ +
    37#include <gtsam/base/GenericValue.h>
    │ │ │ │ +
    38#include <gtsam/base/Group.h>
    │ │ │ │ +
    39#include <gtsam/base/Lie.h>
    │ │ │ │ + │ │ │ │ +
    41#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    42#include <gtsam/base/Matrix.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    48#include <gtsam/base/Testable.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>
    │ │ │ │ + │ │ │ │ +
    57
    │ │ │ │ +
    58
    │ │ │ │ +
    Group product of two Lie Groups.
    │ │ │ │ +
    Provides additional testing facilities for common data structures.
    │ │ │ │ +
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ +
    A faster implementation for DSF, which uses vector rather than btree.
    │ │ │ │ +
    Timing utilities.
    │ │ │ │ +
    A matrix with column blocks of pre-defined sizes.
    │ │ │ │ +
    Global debugging flags.
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Concept check class for variable types with Group properties.
    │ │ │ │ +
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ +
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Provides convenient mappings of common member functions for testing.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    The base class for any variable that can be optimized or used in a factor.
    │ │ │ │ +
    Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
    │ │ │ │ + │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -global_includes.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,17 +16,100 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 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 │ │ │ │ │ +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 │ │ │ │ │ +_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ +Group product of two Lie Groups. │ │ │ │ │ +_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. │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ +A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_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. │ │ │ │ │ +_d_e_b_u_g_._h │ │ │ │ │ +Global debugging flags. │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_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. │ │ │ │ │ +_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. │ │ │ │ │ +_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_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. │ │ │ │ │ +_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. │ │ │ │ │ +_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_y_p_e_s_._h │ │ │ │ │ Typedefs for easier changing of types. │ │ │ │ │ +_l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ +Provides convenient mappings of common member functions for testing. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_V_a_l_u_e_._h │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ +_c_h_o_l_e_s_k_y_._h │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ +_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 │ │ │ │ │ +_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 │ │ │ │ │ - * _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_._h │ │ │ │ │ * Generated on Tue Jan 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.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: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    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: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ 00027560: 3c21 2d2d 2063 6f6e 7465 6e74 7320 2d2d .Print (const DiscreteDistribution &m, const std::string &str="") │ │ │ │   │ │ │ │ │ │ │ │ static bool Equals (const DiscreteDistribution &m1, const DiscreteDistribution &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.h
    • │ │ │ │
    • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.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.h
    • │ │ │ │
    • /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.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.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.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
    • │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │

    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.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.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.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.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.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.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.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.h
    • │ │ │ │
    • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.cpp
    • │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    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: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    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/HessianFactor.h
    • │ │ │ │ -
    • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.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
    • │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    346
    │ │ │ │
    347} // namespace utilities
    │ │ │ │
    348
    │ │ │ │
    349}
    │ │ │ │
    350
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    2D Pose
    │ │ │ │ -
    2D Point
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    sampling from a NoiseModel
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ -
    Reprojection of a LANDMARK to a 2D point.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    2D Pose
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    2D Point
    │ │ │ │ +
    sampling from a NoiseModel
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    Reprojection of a LANDMARK to a 2D point.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    Key symbol(unsigned char c, std::uint64_t j)
    Create a symbol key from a character and index, i.e.
    Definition Symbol.h:135
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    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_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_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ _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. │ │ │ │ │ _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. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _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. │ │ │ │ │ _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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ - │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    std::uint64_t symbolIndex(Key key)
    Return the index portion of a symbol key.
    Definition Symbol.h:141
    │ │ │ │
    Key symbol(unsigned char c, std::uint64_t j)
    Create a symbol key from a character and index, i.e.
    Definition Symbol.h:135
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    unsigned char symbolChr(Key key)
    Return the character portion of a symbol key.
    Definition Symbol.h:138
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ +
    10#include <gtsam/geometry/Rot3.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
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    Vector3 Velocity3
    Velocity is currently typedef'd to Vector3.
    Definition NavState.h:28
    │ │ │ │
    Common expressions, both linear and non-linear.
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    11#include <gtsam/base/Lie.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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    T between(const T &t1, const T &t2)
    binary functions
    Definition lieProxies.h:36
    │ │ │ │ -
    Expressions for Block Automatic Differentiation.
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    T between(const T &t1, const T &t2)
    binary functions
    Definition lieProxies.h:36
    │ │ │ │ +
    Expressions for Block Automatic Differentiation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ Go to the documentation of this file.
    1
    │ │ │ │
    8#pragma once
    │ │ │ │
    9
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    15#include <gtsam/geometry/OrientedPlane3.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) {
    │ │ │ │ │ │ │ │
    195 gtsam::traits<T>::Logmap, between(x1, x2));
    │ │ │ │
    196}
    │ │ │ │
    │ │ │ │
    197
    │ │ │ │
    198} // \namespace gtsam
    │ │ │ │ -
    P rotate(const T &r, const P &pt)
    rotation functions
    Definition lieProxies.h:47
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ -
    Calibration used by Bundler.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    2D Pose
    │ │ │ │ -
    4 dimensional manifold of 3D lines
    │ │ │ │ +
    P rotate(const T &r, const P &pt)
    rotation functions
    Definition lieProxies.h:47
    │ │ │ │ +
    Calibration used by Bundler.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    2D Pose
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ +
    4 dimensional manifold of 3D lines
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
    logmap
    Definition expressions.h:192
    │ │ │ │
    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
    │ │ │ │
    Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
    cross product
    Definition Point3.cpp:64
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -199,22 +199,22 @@ │ │ │ │ │ 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. │ │ │ │ │ _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_2_._h │ │ │ │ │ 2D Pose │ │ │ │ │ +_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 │ │ │ │ │ _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 │ │ │ ├── ./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
    │ │ │ │ - │ │ │ │ -
    19#include <gtsam/base/Testable.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:
    │ │ │ │ - │ │ │ │ + │ │ │ │
    118 };
    │ │ │ │
    │ │ │ │
    119
    │ │ │ │
    121 template<class VALUE>
    │ │ │ │
    122 struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
    │ │ │ │
    123
    │ │ │ │
    124
    │ │ │ │
    125}
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
    This marks a GTSAM object to require alignment.
    Definition types.h:317
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    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>
    │ │ │ │ - │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    GenericValue< T > genericValue(const T &v)
    Functional constructor of GenericValue<T> so T can be automatically deduced.
    Definition GenericValue.h:212
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    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
     
     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
     
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/dir_7c6fd4d23c35234f8f1271a66248b63b.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,11 @@ │ │ │ │ │ var dir_7c6fd4d23c35234f8f1271a66248b63b = [ │ │ │ │ │ - ["Basis.h", "a00173.html", "a00173"], │ │ │ │ │ - ["BasisFactors.h", "a00167.html", "a00167"], │ │ │ │ │ - ["Chebyshev.cpp", "a00170.html", null], │ │ │ │ │ - ["Chebyshev.h", "a00152.html", "a00152"], │ │ │ │ │ + ["Basis.h", "a00158.html", "a00158"], │ │ │ │ │ + ["BasisFactors.h", "a00152.html", "a00152"], │ │ │ │ │ + ["Chebyshev.cpp", "a00176.html", null], │ │ │ │ │ + ["Chebyshev.h", "a00161.html", "a00161"], │ │ │ │ │ ["Chebyshev2.cpp", "a00164.html", null], │ │ │ │ │ - ["Chebyshev2.h", "a00161.html", "a00161"], │ │ │ │ │ - ["FitBasis.h", "a00176.html", "a00176"], │ │ │ │ │ - ["Fourier.h", "a00158.html", "a00158"], │ │ │ │ │ + ["Chebyshev2.h", "a00170.html", "a00170"], │ │ │ │ │ + ["FitBasis.h", "a00173.html", "a00173"], │ │ │ │ │ + ["Fourier.h", "a00167.html", "a00167"], │ │ │ │ │ ["ParameterMatrix.h", "a00155.html", "a00155"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/dir_88adea47575c2d5c40d4070390572b66.html │ │ │ │ @@ -99,176 +99,176 @@ │ │ │ │
    │ │ │ │
    nonlinear Directory Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

    │ │ │ │ Files

     CustomFactor.cpp
     Class to enable arbitrary factors with runtime swappable error function.
     CustomFactor.cpp
     Class to enable arbitrary factors with runtime swappable error function.
     
     CustomFactor.h
     Class to enable arbitrary factors with runtime swappable error function.
     CustomFactor.h
     Class to enable arbitrary factors with runtime swappable error function.
     
     DoglegOptimizer.cpp
     DoglegOptimizer.cpp
     
     DoglegOptimizer.h
     DoglegOptimizer.h
     
     DoglegOptimizerImpl.h
     Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
     DoglegOptimizerImpl.h
     Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
     
     Expression-inl.h
     Internals for Expression.h, not for general consumption.
     Expression-inl.h
     Internals for Expression.h, not for general consumption.
     
     Expression.h
     Expressions for Block Automatic Differentiation.
     Expression.h
     Expressions for Block Automatic Differentiation.
     
     ExpressionFactor.h
     ExpressionFactor.h
     
     ExpressionFactorGraph.h
     Factor graph that supports adding ExpressionFactors directly.
     ExpressionFactorGraph.h
     Factor graph that supports adding ExpressionFactors directly.
     
     expressions.h
     Common expressions, both linear and non-linear.
     
     expressionTesting.h
     Test harness methods for expressions.
     expressionTesting.h
     Test harness methods for expressions.
     
     ExtendedKalmanFilter-inl.h
     Class to perform generic Kalman Filtering using nonlinear factor graphs.
     ExtendedKalmanFilter-inl.h
     Class to perform generic Kalman Filtering using nonlinear factor graphs.
     
     ExtendedKalmanFilter.h
     Class to perform generic Kalman Filtering using nonlinear factor graphs.
     ExtendedKalmanFilter.h
     Class to perform generic Kalman Filtering using nonlinear factor graphs.
     
     factorTesting.h
     Evaluate derivatives of a nonlinear factor numerically.
     factorTesting.h
     Evaluate derivatives of a nonlinear factor numerically.
     
     FunctorizedFactor.h
     FunctorizedFactor.h
     
     GaussNewtonOptimizer.cpp
     GaussNewtonOptimizer.cpp
     
     GaussNewtonOptimizer.h
     GaussNewtonOptimizer.h
     
     GncOptimizer.h
     The GncOptimizer class.
     GncOptimizer.h
     The GncOptimizer class.
     
     GncParams.h
     GncParams.h
     
     GraphvizFormatting.cpp
     Graphviz formatter for NonlinearFactorGraph.
     GraphvizFormatting.cpp
     Graphviz formatter for NonlinearFactorGraph.
     
     GraphvizFormatting.h
     Graphviz formatter for NonlinearFactorGraph.
     GraphvizFormatting.h
     Graphviz formatter for NonlinearFactorGraph.
     
     ISAM2-impl.cpp
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     
     ISAM2-impl.h
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     ISAM2-impl.h
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     
     ISAM2.cpp
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     ISAM2.cpp
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     
     ISAM2.h
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     ISAM2.h
     Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
     
     ISAM2Clique.cpp
     Specialized iSAM2 Clique.
     ISAM2Clique.cpp
     Specialized iSAM2 Clique.
     
     ISAM2Clique.h
     Specialized iSAM2 Clique.
     ISAM2Clique.h
     Specialized iSAM2 Clique.
     
     ISAM2Params.cpp
     Parameters for iSAM 2.
     ISAM2Params.cpp
     Parameters for iSAM 2.
     
     ISAM2Params.h
     Parameters for iSAM 2.
     ISAM2Params.h
     Parameters for iSAM 2.
     
     ISAM2Result.h
     Class that stores detailed iSAM2 result.
     ISAM2Result.h
     Class that stores detailed iSAM2 result.
     
     ISAM2UpdateParams.h
     Class that stores extra params for ISAM2::update()
     ISAM2UpdateParams.h
     Class that stores extra params for ISAM2::update()
     
     LevenbergMarquardtOptimizer.cpp
     A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
     LevenbergMarquardtOptimizer.cpp
     A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
     
     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.
     
     LevenbergMarquardtParams.cpp
     Parameters for Levenberg-Marquardt trust-region scheme.
     LevenbergMarquardtParams.cpp
     Parameters for Levenberg-Marquardt trust-region scheme.
     
     LevenbergMarquardtParams.h
     Parameters for Levenberg-Marquardt trust-region scheme.
     LevenbergMarquardtParams.h
     Parameters for Levenberg-Marquardt trust-region scheme.
     
     LinearContainerFactor.cpp
     LinearContainerFactor.cpp
     
     LinearContainerFactor.h
     Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
     LinearContainerFactor.h
     Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
     
     Marginals.cpp
     Marginals.cpp
     
     Marginals.h
     A class for computing marginals in a NonlinearFactorGraph.
     Marginals.h
     A class for computing marginals in a NonlinearFactorGraph.
     
     NonlinearConjugateGradientOptimizer.cpp
     Simple non-linear optimizer that solves using non-preconditioned CG.
     NonlinearConjugateGradientOptimizer.cpp
     Simple non-linear optimizer that solves using non-preconditioned CG.
     
     NonlinearConjugateGradientOptimizer.h
     Simple non-linear optimizer that solves using non-preconditioned CG.
     NonlinearConjugateGradientOptimizer.h
     Simple non-linear optimizer that solves using non-preconditioned CG.
     
     NonlinearEquality.h
     NonlinearEquality.h
     
     nonlinearExceptions.h
     Exceptions that may be thrown by nonlinear optimization components.
     nonlinearExceptions.h
     Exceptions that may be thrown by nonlinear optimization components.
     
     NonlinearFactor.cpp
     Nonlinear Factor base classes.
     
     NonlinearFactor.h
     Non-linear factor base classes.
     NonlinearFactor.h
     Non-linear factor base classes.
     
     NonlinearFactorGraph.cpp
     Factor Graph Consisting of non-linear factors.
     NonlinearFactorGraph.cpp
     Factor Graph Consisting of non-linear factors.
     
     NonlinearFactorGraph.h
     Factor Graph consisting of non-linear factors.
     NonlinearFactorGraph.h
     Factor Graph consisting of non-linear factors.
     
     NonlinearISAM.cpp
     NonlinearISAM.cpp
     
     NonlinearISAM.h
     NonlinearISAM.h
     
     NonlinearOptimizer.cpp
     Convergence functions not dependent on graph types.
     NonlinearOptimizer.cpp
     Convergence functions not dependent on graph types.
     
     NonlinearOptimizer.h
     Base class and parameters for nonlinear optimization algorithms.
     NonlinearOptimizer.h
     Base class and parameters for nonlinear optimization algorithms.
     
     NonlinearOptimizerParams.cpp
     Parameters for nonlinear optimization.
     NonlinearOptimizerParams.cpp
     Parameters for nonlinear optimization.
     
     NonlinearOptimizerParams.h
     Parameters for nonlinear optimization.
     NonlinearOptimizerParams.h
     Parameters for nonlinear optimization.
     
     PriorFactor.h
     
     Symbol.h
     Symbol.h was moved to inference directory, this header was retained for compatibility.
     
     utilities.h
     Contains generic global functions designed particularly for the matlab interface.
     
     Values-inl.h
     Values-inl.h
     
     Values.h
     A non-templated config holding any types of Manifold-group elements.
     Values.h
     A non-templated config holding any types of Manifold-group elements.
     
     WhiteNoiseFactor.h
     Binary white noise factor.
     WhiteNoiseFactor.h
     Binary white noise factor.
     
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    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
    • │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    TestableAssertions.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>
    │ │ │ │ - │ │ │ │ +
    20#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │
    22
    │ │ │ │
    23#include <boost/optional.hpp>
    │ │ │ │
    24#include <map>
    │ │ │ │
    25#include <iostream>
    │ │ │ │
    26#include <sstream>
    │ │ │ │
    27#include <vector>
    │ │ │ │
    28
    │ │ │ │ @@ -469,28 +469,28 @@ │ │ │ │
    393 std::cout.rdbuf(old);
    │ │ │ │
    394
    │ │ │ │
    395 return assert_equal(expected, actual_);
    │ │ │ │
    396}
    │ │ │ │
    │ │ │ │
    397
    │ │ │ │
    398} // \namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
    equals with an tolerance, prints out message if unequal
    Definition Matrix.cpp:43
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00032_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00023_source.html │ │ │ │┄ Files 0% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26#include <gtsam/base/Lie.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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ - │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    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 @@ │ │ │ │
    Definition numericalDerivative.h:56
    │ │ │ │
    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/a00290.html │ │ │ │┄ Files 8% 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/geometry/CalibratedCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ThreadsafeException.h File Reference
    │ │ │ │ +
    CalibratedCamera.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Calibrated camera for which only pose is unknown. │ │ │ │ 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::CheiralityException
     
    class  gtsam::RuntimeErrorThreadsafe
     Thread-safe runtime error exception. More...
    class  gtsam::PinholeBase
     A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras. More...
     
    class  gtsam::OutOfRangeThreadsafe
     Thread-safe out of range exception. More...
    class  gtsam::CalibratedCamera
     A Calibrated camera class [R|-R't], calibration K=I. More...
     
    class  gtsam::InvalidArgumentThreadsafe
     Thread-safe invalid argument exception. More...
    struct  gtsam::traits< CalibratedCamera >
     
    class  gtsam::CholeskyFailed
     Indicate Cholesky factorization failure. More...
    struct  gtsam::traits< const CalibratedCamera >
     
    struct  gtsam::Range< CalibratedCamera, T >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

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

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

    Calibrated camera for which only pose is unknown.

    │ │ │ │ +
    Date
    Aug 17, 2009
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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_._._. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_._._. │ │ │ │ │   │ │ │ │ │ -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_._._. │ │ │ │ │ +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_ _> │ │ │ │ │   │ │ │ │ │ -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_<_ _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 ********** │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ Date │ │ │ │ │ - Aug 21, 2010 │ │ │ │ │ + Aug 17, 2009 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00041.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00845.html │ │ │ │┄ Files 4% 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/linear/GaussianISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    Value.h File Reference
    │ │ │ │ +
    GaussianISAM.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    The base class for any variable that can be optimized or used in a factor. │ │ │ │ -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::GaussianISAM
     
    struct  gtsam::traits< GaussianISAM >
     traits 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
    │ │ │ │ +
    Date
    July 29, 2013
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -Value.h File Reference │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_:_:_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 ********** │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ + Date │ │ │ │ │ + July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Jan 14, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _V_a_l_u_e_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a00041_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00179_source.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/discrete/Signature.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Value.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/config.h> // Configuration from CMake
    │ │ │ │ -
    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;
    │ │ │ │ -
    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;
    │ │ │ │ +
    20#include <string>
    │ │ │ │ +
    21#include <vector>
    │ │ │ │ +
    22#include <boost/optional.hpp>
    │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    │ │ │ │ +
    54 class GTSAM_EXPORT Signature {
    │ │ │ │ +
    55
    │ │ │ │ +
    56 public:
    │ │ │ │ +
    57
    │ │ │ │ +
    59 typedef std::vector<double> Row;
    │ │ │ │ +
    60 typedef std::vector<Row> Table;
    │ │ │ │
    61
    │ │ │ │ -
    68 virtual Value* retract_(const Vector& delta) const = 0;
    │ │ │ │ +
    62 private:
    │ │ │ │ +
    63
    │ │ │ │ +
    65 DiscreteKey key_;
    │ │ │ │ +
    66
    │ │ │ │ +
    68 DiscreteKeys parents_;
    │ │ │ │
    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() {}
    │ │ │ │ +
    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
    │ │ │ │ -
    91 private:
    │ │ │ │ -
    122 friend class boost::serialization::access;
    │ │ │ │ -
    123 template<class ARCHIVE>
    │ │ │ │ -
    124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
    │ │ │ │ -
    125 }
    │ │ │ │ -
    126
    │ │ │ │ -
    127 };
    │ │ │ │ -
    │ │ │ │ +
    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} /* namespace gtsam */
    │ │ │ │ -
    130
    │ │ │ │ -
    131BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    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}
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ +
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ -
    virtual size_t dim() const =0
    Return the dimensionality of the tangent space of this value.
    │ │ │ │ -
    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 ...
    │ │ │ │ -
    virtual void deallocate_() const =0
    Deallocate a raw pointer of this value.
    │ │ │ │ -
    virtual ~Value()
    Virutal destructor.
    Definition Value.h:89
    │ │ │ │ -
    virtual bool equals_(const Value &other, double tol=1e-9) const =0
    Compare this Value with another for equality.
    │ │ │ │ -
    virtual Vector localCoordinates_(const Value &value) const =0
    Compute the coordinates in the tangent space of this value that retract() would map to value.
    │ │ │ │ -
    virtual Value & operator=(const Value &)
    Assignment operator.
    Definition Value.h:79
    │ │ │ │ -
    virtual void print(const std::string &str="") const =0
    Print this value, for debugging and unit tests.
    │ │ │ │ -
    virtual Value * clone_() const =0
    Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
    │ │ │ │ -
    virtual boost::shared_ptr< Value > clone() const =0
    Clone this value (normal clone on the heap, delete with 'delete' operator)
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    Signature operator|(const DiscreteKey &key, const DiscreteKey &parent)
    Helper function to create Signature objects example: Signature s = D | E;.
    Definition Signature.cpp:178
    │ │ │ │ +
    Signature operator%(const DiscreteKey &key, const string &parent)
    Helper function to create Signature objects example: Signature s(D % "99/1");.
    Definition Signature.cpp:183
    │ │ │ │ +
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    Signature for a discrete conditional density, used to construct conditionals.
    Definition Signature.h:54
    │ │ │ │ +
    std::vector< double > Row
    Data type for the CPT.
    Definition Signature.h:59
    │ │ │ │ +
    const DiscreteKey & key() const
    the variable key
    Definition Signature.h:115
    │ │ │ │ +
    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 │ │ │ │ │ -Value.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,105 +15,119 @@ │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include // Configuration from CMake │ │ │ │ │ -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; │ │ │ │ │ -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; │ │ │ │ │ +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 { │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ +57 │ │ │ │ │ +_5_9 typedef std::vector _R_o_w; │ │ │ │ │ +60 typedef std::vector Table; │ │ │ │ │ 61 │ │ │ │ │ -_6_8 virtual _V_a_l_u_e* _r_e_t_r_a_c_t__(const Vector& delta) const = 0; │ │ │ │ │ +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 │ │ │ │ │ -_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() {} │ │ │ │ │ +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 │ │ │ │ │ -91 private: │ │ │ │ │ -_1_2_2 friend class boost::serialization::access; │ │ │ │ │ -123 template │ │ │ │ │ -124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127 }; │ │ │ │ │ +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} /* 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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _V_a_l_u_e_._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/a00074.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00038.html │ │ │ │┄ Files 14% similar despite different names │ │ │ │ @@ -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/debug.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ Functions
    │ │ │ │ -
    lieProxies.h File Reference
    │ │ │ │ +
    debug.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Global debugging flags. │ │ │ │ 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

    │ │ │ │ -template<class T >
    gtsam::testing::between (const T &t1, const T &t2)
     binary functions
     
    │ │ │ │ -template<class T >
    gtsam::testing::compose (const T &t1, const T &t2)
     
    │ │ │ │ -template<class T >
    gtsam::testing::inverse (const T &t)
     unary functions
     
    │ │ │ │ -template<class T , class P >
    gtsam::testing::rotate (const T &r, const P &pt)
     rotation functions
     
    │ │ │ │ -template<class T , class P >
    gtsam::testing::unrotate (const T &r, const P &pt)
     
    │ │ │ │ +bool gtsam::guardedIsDebug (const std::string &s)
     
    │ │ │ │ +void gtsam::guardedSetDebug (const std::string &s, const bool v)
     
    │ │ │ │ +bool gtsam::isDebugVersion ()
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Provides convenient mappings of common member functions for testing.

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

    Global debugging flags.

    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    Date
    Feb 1, 2011
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,40 +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 │ │ │ │ │ -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 | _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 │ │ │ │ │ -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::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │ +  │ │ │ │ │ +void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -P  ggttssaamm::::tteessttiinngg::::uunnrroottaattee (const T &r, const P &pt) │ │ │ │ │ +bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Provides convenient mappings of common member functions for testing. │ │ │ │ │ +Global debugging flags. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 1, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _l_i_e_P_r_o_x_i_e_s_._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/a00083.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00077.js │ │ │ │┄ Files 22% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,10 +1,12 @@ │ │ │ │ │ -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] │ │ │ │ │ +var a00077 = [ │ │ │ │ │ + ["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", "a00077.html#aac1ea136475ea605715c809f4b3f0156", null], │ │ │ │ │ + ["BOOST_CONCEPT_REQUIRES", "a00077.html#a2e95b5b5508cf3284107e979401bd1a4", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00083_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00776_source.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/linear/AcceleratedPowerMethod.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Testable.h
    │ │ │ │ +
    AcceleratedPowerMethod.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
    │ │ │ │ -
    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;
    │ │ │ │ -
    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());
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    Power method for fast eigenvalue and eigenvector computation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
    equals with an tolerance, prints out message if unequal
    Definition Matrix.cpp:43
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    bool equal(const T &obj1, const T &obj2, double tol)
    Call equal on the object.
    Definition Testable.h:84
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Binary predicate on shared pointers.
    Definition Testable.h:123
    │ │ │ │ -
    Requirements on type to pass it to Testable template below.
    Definition Testable.h:134
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Compute maximum Eigenpair with accelerated power method.
    Definition AcceleratedPowerMethod.h:51
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector acceleratedPowerIteration() const
    Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
    Definition AcceleratedPowerMethod.h:92
    │ │ │ │ +
    bool compute(size_t maxIterations, double tol)
    Start the accelerated iteration, after performing the accelerated iteration, calculate the ritz error...
    Definition AcceleratedPowerMethod.h:156
    │ │ │ │ +
    Compute maximum Eigenpair with power method.
    Definition PowerMethod.h:57
    │ │ │ │ +
    const Operator & A_
    Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
    Definition PowerMethod.h:63
    │ │ │ │ +
    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,175 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Testable.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 │ │ │ │ │ -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; │ │ │ │ │ -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); │ │ │ │ │ -72 } │ │ │ │ │ -73 }; // \ Testable │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00086.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01286.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/SymmetricBlockMatrix.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SymmetricBlockMatrix.cpp File Reference
    │ │ │ │ +
    PoseRotationPrior.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Access to matrices via blocks of pre-defined sizes.

    │ │ │ │ -

    Used in GaussianFactor and GaussianConditional.

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

    Implements a prior on the rotation component of a pose.

    │ │ │ │ +
    Date
    Jun 14, 2012
    │ │ │ │ +
    Author
    Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymmetricBlockMatrix.cpp File Reference │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. _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. │ │ │ │ │   │ │ │ │ │ ********** 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 │ │ │ │ │ +Implements a prior on the rotation component of a pose. │ │ │ │ │ Date │ │ │ │ │ - Sep 18, 2010 │ │ │ │ │ + Jun 14, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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_._c_p_p │ │ │ │ │ + * _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/a00110.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/inference/MetisIndex.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    FastList.h File Reference
    │ │ │ │ +
    MetisIndex.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A thin wrapper around std::list 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::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::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

    │ │ │ │ -

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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Oct 22, 2010
    │ │ │ │ +
    Author
    Andrew Melim
    │ │ │ │ +
    Date
    Oct. 10, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +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 │ │ │ │ │ -FastList.h File Reference │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_:_:_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 ********** │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Andrew Melim │ │ │ │ │ Date │ │ │ │ │ - Oct 22, 2010 │ │ │ │ │ + Oct. 10, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_L_i_s_t_._h │ │ │ │ │ + * _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/a00110_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00278_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/geometry/Cyclic.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    FastList.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#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Concept check class for variable types with Group properties.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ -
    FastList(INPUTITERATOR first, INPUTITERATOR last)
    Constructor from a range, passes through to base class.
    Definition FastList.h:51
    │ │ │ │ -
    FastList()
    Default constructor.
    Definition FastList.h:47
    │ │ │ │ -
    FastList(std::initializer_list< VALUE > l)
    Construct from c++11 initializer list:
    Definition FastList.h:60
    │ │ │ │ -
    FastList(const FastList< VALUE > &x)
    Copy constructor from another FastList.
    Definition FastList.h:54
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition FastList.h:80
    │ │ │ │ -
    FastList(const Base &x)
    Copy constructor from the base list class.
    Definition FastList.h:57
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Cyclic group of order N.
    Definition Cyclic.h:30
    │ │ │ │ +
    bool equals(const Cyclic &other, double tol=1e-9) const
    equals with an tolerance, prints out message if unequal
    Definition Cyclic.h:64
    │ │ │ │ +
    void print(const std::string &s="") const
    print with optional string
    Definition Cyclic.h:60
    │ │ │ │ +
    Cyclic()
    Default constructor yields identity.
    Definition Cyclic.h:39
    │ │ │ │ +
    Cyclic(size_t i)
    Constructor.
    Definition Cyclic.h:34
    │ │ │ │ +
    Cyclic operator-(const Cyclic &h) const
    Subtraction modulo N.
    Definition Cyclic.h:52
    │ │ │ │ +
    Cyclic operator+(const Cyclic &h) const
    Addition modulo N.
    Definition Cyclic.h:48
    │ │ │ │ +
    Cyclic operator-() const
    Inverse.
    Definition Cyclic.h:56
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,114 +1,118 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastList.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#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. │ │ │ │ │ +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 │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_L_i_s_t_._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/a00116_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00629_source.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/inference/Ordering.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    VerticalBlockMatrix.h
    │ │ │ │ +
    Ordering.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/Matrix.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    26 // Forward declarations
    │ │ │ │ -
    27 class SymmetricBlockMatrix;
    │ │ │ │ -
    28
    │ │ │ │ -
    │ │ │ │ -
    42 class GTSAM_EXPORT VerticalBlockMatrix
    │ │ │ │ -
    43 {
    │ │ │ │ -
    44 public:
    │ │ │ │ - │ │ │ │ -
    46 typedef Eigen::Block<Matrix> Block;
    │ │ │ │ -
    47 typedef Eigen::Block<const Matrix> constBlock;
    │ │ │ │ -
    48
    │ │ │ │ -
    49 protected:
    │ │ │ │ -
    50 Matrix matrix_;
    │ │ │ │ - │ │ │ │ -
    52
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    56
    │ │ │ │ -
    57 public:
    │ │ │ │ -
    58
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ +
    23#include <gtsam/inference/Key.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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    42 COLAMD, METIS, NATURAL, CUSTOM
    │ │ │ │ +
    43 };
    │ │ │ │ +
    │ │ │ │ +
    44
    │ │ │ │ +
    45 typedef Ordering This;
    │ │ │ │ +
    46 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    47
    │ │ │ │ +
    49 GTSAM_EXPORT
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    76
    │ │ │ │ +
    78 bool contains(const Key& key) const;
    │ │ │ │ +
    79
    │ │ │ │ + │ │ │ │ +
    87
    │ │ │ │
    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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    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_; }
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    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}
    │ │ │ │ -
    Serialization for matrices.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    VerticalBlockMatrix()
    Construct an empty VerticalBlockMatrix.
    Definition VerticalBlockMatrix.h:60
    │ │ │ │ -
    Block full()
    Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
    Definition VerticalBlockMatrix.h:157
    │ │ │ │ -
    Block range(DenseIndex startBlock, DenseIndex endBlock)
    access ranges of blocks at a time
    Definition VerticalBlockMatrix.h:130
    │ │ │ │ -
    const DenseIndex & firstBlock() const
    Get the apparent first block for all operations.
    Definition VerticalBlockMatrix.h:182
    │ │ │ │ -
    const Matrix & matrix() const
    Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
    Definition VerticalBlockMatrix.h:188
    │ │ │ │ -
    VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
    Construct from a container of the sizes of each vertical block.
    Definition VerticalBlockMatrix.h:69
    │ │ │ │ -
    DenseIndex rowEnd_
    Changes apparent matrix view, see main class comment.
    Definition VerticalBlockMatrix.h:54
    │ │ │ │ -
    const DenseIndex & rowEnd() const
    Get the apparent last row (exclusive, i.e.
    Definition VerticalBlockMatrix.h:176
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ -
    Block operator()(DenseIndex block)
    Access a single block in the underlying matrix with read/write access.
    Definition VerticalBlockMatrix.h:124
    │ │ │ │ -
    DenseIndex & firstBlock()
    Get or set the apparent first block for all operations.
    Definition VerticalBlockMatrix.h:185
    │ │ │ │ -
    DenseIndex cols() const
    Column size.
    Definition VerticalBlockMatrix.h:118
    │ │ │ │ -
    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
    Construct from iterator over the sizes of each vertical block.
    Definition VerticalBlockMatrix.h:93
    │ │ │ │ -
    const constBlock operator()(DenseIndex block) const
    Access a const block view.
    Definition VerticalBlockMatrix.h:127
    │ │ │ │ -
    const DenseIndex & rowStart() const
    Get the apparent first row of the underlying matrix for all operations.
    Definition VerticalBlockMatrix.h:170
    │ │ │ │ -
    Matrix matrix_
    The full matrix.
    Definition VerticalBlockMatrix.h:50
    │ │ │ │ -
    DenseIndex nBlocks() const
    Block count.
    Definition VerticalBlockMatrix.h:121
    │ │ │ │ -
    const constBlock full() const
    Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
    Definition VerticalBlockMatrix.h:160
    │ │ │ │ -
    DenseIndex & rowStart()
    Get or set the apparent first row of the underlying matrix for all operations.
    Definition VerticalBlockMatrix.h:173
    │ │ │ │ -
    Matrix & matrix()
    Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
    Definition VerticalBlockMatrix.h:191
    │ │ │ │ -
    DenseIndex & rowEnd()
    Get or set the apparent last row (exclusive, i.e.
    Definition VerticalBlockMatrix.h:179
    │ │ │ │ -
    DenseIndex rowStart_
    Changes apparent matrix view, see main class comment.
    Definition VerticalBlockMatrix.h:53
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex blockStart_
    Changes apparent matrix view, see main class comment.
    Definition VerticalBlockMatrix.h:55
    │ │ │ │ -
    FastVector< DenseIndex > variableColOffsets_
    the starting columns of each block (0-based)
    Definition VerticalBlockMatrix.h:51
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
    Definition MetisIndex.h:45
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    static Ordering Natural(const FACTOR_GRAPH &fg)
    Return a natural Ordering. Typically used by iterative solvers.
    Definition Ordering.h:190
    │ │ │ │ +
    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
    │ │ │ │ +
    Ordering(const KEYS &keys)
    Create from a container.
    Definition Ordering.h:57
    │ │ │ │ +
    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
    │ │ │ │ +
    OrderingType
    Type of ordering to use.
    Definition Ordering.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition Ordering.h:46
    │ │ │ │ +
    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
    │ │ │ │ +
    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_EXPORT Ordering()
    Create an empty ordering.
    Definition Ordering.h:50
    │ │ │ │ +
    static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
    Compute an ordering determined by METIS from a VariableIndex.
    Definition Ordering.cpp:212
    │ │ │ │ +
    FastMap< Key, size_t > invert() const
    Invert (not reverse) the ordering - returns a map from key to order position.
    Definition Ordering.cpp:36
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition Ordering.h:260
    │ │ │ │ +
    Ordering This
    Typedef to this class.
    Definition Ordering.h:45
    │ │ │ │ +
    bool contains(const Key &key) const
    Check if key exists in ordering.
    Definition Ordering.cpp:293
    │ │ │ │ +
    static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > &adj, const FACTOR_GRAPH &graph)
    METIS Formatting function.
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,346 +1,312 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VerticalBlockMatrix.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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; │ │ │ │ │ -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__; │ │ │ │ │ +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> │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -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(); │ │ │ │ │ +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 │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_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 │ │ │ │ │ +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. │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_M_e_t_i_s_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_:_:_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_:_:_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 │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _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/a00122.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00578.html │ │ │ │┄ Files 4% similar despite different names │ │ │ │ @@ -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/inference/ISAM-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    DSFVector.cpp File Reference
    │ │ │ │ +
    ISAM-inst.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

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

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

    Incremental update functionality (iSAM) for BayesTree.

    │ │ │ │ +
    Author
    Michael Kaess
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,21 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ - Date │ │ │ │ │ - Jun 25, 2010 │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Michael Kaess │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._c_p_p │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _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/a00134_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00290_source.html │ │ │ │┄ Files 18% similar despite different names │ │ │ │ @@ -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/geometry/CalibratedCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    serialization.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
    │ │ │ │ -
    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>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <gtsam/base/concepts.h>
    │ │ │ │ +
    25#include <gtsam/base/Manifold.h>
    │ │ │ │ + │ │ │ │ +
    27#include <gtsam/dllexport.h>
    │ │ │ │ +
    28#include <boost/serialization/nvp.hpp>
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    │ │ │ │ +
    32class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
    │ │ │ │ +
    33public:
    │ │ │ │ + │ │ │ │ +
    35 : CheiralityException(std::numeric_limits<Key>::max()) {}
    │ │ │ │
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    54public:
    │ │ │ │
    55
    │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    57 typedef Rot3 Rotation;
    │ │ │ │ +
    58 typedef Point3 Translation;
    │ │ │ │ +
    59
    │ │ │ │ + │ │ │ │ +
    65 typedef Point2Vector MeasurementVector;
    │ │ │ │ +
    66
    │ │ │ │ +
    67private:
    │ │ │ │
    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}
    │ │ │ │ -
    │ │ │ │ +
    69 Pose3 pose_;
    │ │ │ │ +
    70
    │ │ │ │ +
    71protected:
    │ │ │ │ +
    72
    │ │ │ │
    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}
    │ │ │ │ -
    │ │ │ │ +
    81 static Matrix26 Dpose(const Point2& pn, double d);
    │ │ │ │ +
    82
    │ │ │ │ +
    89 static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
    │ │ │ │
    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
    │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    92
    │ │ │ │ +
    93public:
    │ │ │ │ +
    94
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    140
    │ │ │ │ +
    142 bool equals(const PinholeBase &camera, double tol = 1e-9) const;
    │ │ │ │
    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}
    │ │ │ │ +
    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
    │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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}
    │ │ │ │ +
    │ │ │ │ +
    223 inline static std::pair<size_t, size_t> translationInterval() {
    │ │ │ │ +
    224 return std::make_pair(3, 5);
    │ │ │ │ +
    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>
    │ │ │ │ +
    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
    │ │ │ │
    │ │ │ │ -
    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}
    │ │ │ │ + │ │ │ │ +
    260 }
    │ │ │ │
    │ │ │ │ -
    271
    │ │ │ │ -
    272} // namespace gtsam
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void serializeToStream(const T &input, std::ostream &out_archive_stream)
    Definition serialization.h:64
    │ │ │ │ -
    bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
    deserializes from an XML file
    Definition serialization.h:174
    │ │ │ │ -
    std::string serializeToXMLString(const T &input, const std::string &name="data")
    serializes to a string in XML
    Definition serialization.h:146
    │ │ │ │ -
    void deserializeFromString(const std::string &serialized, T &output)
    deserializes from a string
    Definition serialization.h:86
    │ │ │ │ -
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ -
    void deserializeFromBinaryString(const std::string &serialized, T &output, const std::string &name="data")
    deserializes from a string in binary
    Definition serialization.h:229
    │ │ │ │ -
    void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
    deserializes from a string in binary
    Definition serialization.h:266
    │ │ │ │ -
    void deserializeFromXMLString(const std::string &serialized, T &output, const std::string &name="data")
    deserializes from a string in XML
    Definition serialization.h:155
    │ │ │ │ -
    std::string serializeToBinaryString(const T &input, const std::string &name="data")
    serializes to a string in binary
    Definition serialization.h:220
    │ │ │ │ -
    void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
    deserializes from a string in XML
    Definition serialization.h:192
    │ │ │ │ -
    bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
    serializes to an XML file
    Definition serialization.h:163
    │ │ │ │ -
    void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
    deserializes from a stream in XML
    Definition serialization.h:138
    │ │ │ │ -
    void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
    deserializes from a stream in binary
    Definition serialization.h:212
    │ │ │ │ -
    std::string serializeXML(const T &input, const std::string &name="data")
    serializes to a string in XML
    Definition serialization.h:185
    │ │ │ │ -
    std::string serializeToString(const T &input)
    serializes to a string
    Definition serialization.h:78
    │ │ │ │ -
    bool serializeToFile(const T &input, const std::string &filename)
    serializes to a file
    Definition serialization.h:93
    │ │ │ │ -
    void deserialize(const std::string &serialized, T &output)
    deserializes from a string
    Definition serialization.h:119
    │ │ │ │ -
    void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
    Definition serialization.h:204
    │ │ │ │ -
    void deserializeFromStream(std::istream &in_archive_stream, T &output)
    deserializes from a stream
    Definition serialization.h:71
    │ │ │ │ -
    std::string serializeBinary(const T &input, const std::string &name="data")
    serializes to a string in binary
    Definition serialization.h:259
    │ │ │ │ -
    bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
    serializes to a binary file
    Definition serialization.h:237
    │ │ │ │ -
    bool deserializeFromFile(const std::string &filename, T &output)
    deserializes from a file
    Definition serialization.h:103
    │ │ │ │ -
    void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
    Definition serialization.h:130
    │ │ │ │ -
    bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
    deserializes from a binary file
    Definition serialization.h:248
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    Definition ThreadsafeException.h:42
    │ │ │ │ +
    Definition BearingRange.h:40
    │ │ │ │ +
    Definition BearingRange.h:194
    │ │ │ │ +
    Definition CalibratedCamera.h:32
    │ │ │ │ +
    A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
    Definition CalibratedCamera.h:52
    │ │ │ │ +
    PinholeBase()
    Default constructor.
    Definition CalibratedCamera.h:123
    │ │ │ │ +
    PinholeBase(const Pose3 &pose)
    Constructor with pose.
    Definition CalibratedCamera.h:126
    │ │ │ │ +
    const Point3 & translation() const
    get translation
    Definition CalibratedCamera.h:162
    │ │ │ │ +
    const Rot3 & rotation() const
    get rotation
    Definition CalibratedCamera.h:157
    │ │ │ │ +
    Point2 Measurement
    Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
    Definition CalibratedCamera.h:64
    │ │ │ │ +
    const Pose3 & pose() const
    return pose, constant version
    Definition CalibratedCamera.h:152
    │ │ │ │ +
    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
    │ │ │ │ +
    Rot3 Rotation
    Pose Concept requirements.
    Definition CalibratedCamera.h:57
    │ │ │ │ +
    virtual ~PinholeBase()=default
    Default destructor.
    │ │ │ │ +
    A Calibrated camera class [R|-R't], calibration K=I.
    Definition CalibratedCamera.h:247
    │ │ │ │ +
    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
    │ │ │ │ +
    CalibratedCamera()
    default constructor
    Definition CalibratedCamera.h:259
    │ │ │ │ +
    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
    │ │ │ │ +
    size_t dim() const
    Definition CalibratedCamera.h:331
    │ │ │ │ +
    CalibratedCamera(const Vector &v)
    construct from vector
    Definition CalibratedCamera.h:303
    │ │ │ │ +
    virtual ~CalibratedCamera()
    destructor
    Definition CalibratedCamera.h:312
    │ │ │ │ +
    static size_t Dim()
    Definition CalibratedCamera.h:336
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="CalibratedCamera") const override
    print
    Definition CalibratedCamera.h:326
    │ │ │ │ +
    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
    │ │ │ │ +
    CalibratedCamera(const Pose3 &pose)
    construct with pose
    Definition CalibratedCamera.h:263
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │ +
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,349 +1,460 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -serialization.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 │ │ │ │ │ -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 │ │ │ │ │ +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_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#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 │ │ │ │ │ +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 │ │ │ │ │ +54public: │ │ │ │ │ 55 │ │ │ │ │ -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} │ │ │ │ │ +_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 │ │ │ │ │ -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} │ │ │ │ │ +69 _P_o_s_e_3 pose_; │ │ │ │ │ +70 │ │ │ │ │ +71protected: │ │ │ │ │ +72 │ │ │ │ │ 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} │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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} │ │ │ │ │ +92 │ │ │ │ │ +93public: │ │ │ │ │ +94 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +_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 │ │ │ │ │ -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} │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ +_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00869_source.html │ │ │ │┄ Files 18% similar despite different names │ │ │ │ @@ -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/linear/VectorValues.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymmetricBlockMatrix.h
    │ │ │ │ +
    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)
    │ │ │ │ +
    3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
    │ │ │ │ +
    4 * Atlanta, Georgia 30332-0415
    │ │ │ │ +
    5 * 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
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <gtsam/base/Vector.h>
    │ │ │ │ +
    23#include <gtsam/base/ConcurrentMap.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27#include <boost/shared_ptr.hpp>
    │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ -
    55 typedef Eigen::Block<Matrix> Block;
    │ │ │ │ -
    56 typedef Eigen::Block<const Matrix> constBlock;
    │ │ │ │ -
    57
    │ │ │ │ -
    58 protected:
    │ │ │ │ -
    59 Matrix matrix_;
    │ │ │ │ - │ │ │ │ -
    61
    │ │ │ │ - │ │ │ │ -
    63
    │ │ │ │ -
    64 public:
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    29
    │ │ │ │ +
    30#include <map>
    │ │ │ │ +
    31#include <string>
    │ │ │ │ +
    32#include <iosfwd>
    │ │ │ │ +
    33
    │ │ │ │ +
    34namespace gtsam {
    │ │ │ │ +
    35
    │ │ │ │ +
    │ │ │ │ +
    74 class GTSAM_EXPORT VectorValues {
    │ │ │ │ +
    75 protected:
    │ │ │ │ +
    76 typedef VectorValues This;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    79
    │ │ │ │ +
    80 public:
    │ │ │ │ +
    81 typedef Values::iterator iterator;
    │ │ │ │ +
    82 typedef Values::const_iterator const_iterator;
    │ │ │ │ +
    83 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    86 typedef std::map<Key, size_t> Dims;
    │ │ │ │
    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);
    │ │ │ │ +
    90
    │ │ │ │ + │ │ │ │ +
    93
    │ │ │ │ +
    │ │ │ │ +
    95 VectorValues(std::initializer_list<std::pair<Key, Vector>> init)
    │ │ │ │ +
    96 : values_(init.begin(), init.end()) {}
    │ │ │ │ +
    │ │ │ │ +
    97
    │ │ │ │ +
    100 VectorValues(const VectorValues& first, const VectorValues& second);
    │ │ │ │ +
    101
    │ │ │ │ +
    103 template<class CONTAINER>
    │ │ │ │ +
    104 explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
    │ │ │ │ +
    105
    │ │ │ │ +
    107 VectorValues(const VectorValues& c) : values_(c.values_) {}
    │ │ │ │ +
    108
    │ │ │ │ +
    110 template<typename ITERATOR>
    │ │ │ │ +
    111 VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
    │ │ │ │
    112
    │ │ │ │ -
    114 DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
    │ │ │ │ +
    114 VectorValues(const Vector& c, const Dims& dims);
    │ │ │ │
    115
    │ │ │ │ -
    117 DenseIndex cols() const { return rows(); }
    │ │ │ │ +
    117 VectorValues(const Vector& c, const Scatter& scatter);
    │ │ │ │
    118
    │ │ │ │ -
    120 DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }
    │ │ │ │ +
    120 static VectorValues Zero(const VectorValues& other);
    │ │ │ │
    121
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    125
    │ │ │ │ +
    127 size_t size() const { return values_.size(); }
    │ │ │ │ +
    128
    │ │ │ │ +
    130 size_t dim(Key j) const { return at(j).rows(); }
    │ │ │ │ +
    131
    │ │ │ │ +
    133 bool exists(Key j) const { return find(j) != end(); }
    │ │ │ │ +
    134
    │ │ │ │ +
    │ │ │ │ +
    139 Vector& at(Key j) {
    │ │ │ │ +
    140 iterator item = find(j);
    │ │ │ │ +
    141 if (item == end())
    │ │ │ │ +
    142 throw std::out_of_range(
    │ │ │ │ +
    143 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
    │ │ │ │ +
    144 else
    │ │ │ │ +
    145 return item->second;
    │ │ │ │ +
    146 }
    │ │ │ │ +
    │ │ │ │ +
    147
    │ │ │ │ +
    │ │ │ │ +
    152 const Vector& at(Key j) const {
    │ │ │ │ +
    153 const_iterator item = find(j);
    │ │ │ │ +
    154 if (item == end())
    │ │ │ │ +
    155 throw std::out_of_range(
    │ │ │ │ +
    156 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
    │ │ │ │ +
    157 else
    │ │ │ │ +
    158 return item->second;
    │ │ │ │ +
    159 }
    │ │ │ │ +
    │ │ │ │ +
    160
    │ │ │ │ +
    163 Vector& operator[](Key j) { return at(j); }
    │ │ │ │ +
    164
    │ │ │ │ +
    167 const Vector& operator[](Key j) const { return at(j); }
    │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    172 VectorValues& update(const VectorValues& values);
    │ │ │ │ +
    173
    │ │ │ │ +
    178 iterator insert(const std::pair<Key, Vector>& key_value);
    │ │ │ │
    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 }
    │ │ │ │ +
    184 template<class... Args>
    │ │ │ │ +
    │ │ │ │ +
    185 inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
    │ │ │ │ +
    186#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
    │ │ │ │ +
    187 return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
    │ │ │ │ +
    188#else
    │ │ │ │ +
    189 return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
    │ │ │ │ +
    190#endif
    │ │ │ │ +
    191 }
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ +
    │ │ │ │ +
    197 iterator insert(Key j, const Vector& value) {
    │ │ │ │ +
    198 return insert(std::make_pair(j, value));
    │ │ │ │ +
    199 }
    │ │ │ │ +
    │ │ │ │ +
    200
    │ │ │ │ +
    203 VectorValues& insert(const VectorValues& values);
    │ │ │ │ +
    204
    │ │ │ │ +
    │ │ │ │ +
    209 inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
    │ │ │ │ +
    210#ifdef TBB_GREATER_EQUAL_2020
    │ │ │ │ +
    211 return values_.emplace(j, value);
    │ │ │ │ +
    212#else
    │ │ │ │ +
    213 return values_.insert(std::make_pair(j, value));
    │ │ │ │ +
    214#endif
    │ │ │ │ +
    215 }
    │ │ │ │ +
    │ │ │ │ +
    216
    │ │ │ │ +
    │ │ │ │ +
    219 void insert_or_assign(Key j, const Vector& value) {
    │ │ │ │ +
    220 if (!tryInsert(j, value).second) {
    │ │ │ │ +
    221 (*this)[j] = value;
    │ │ │ │
    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
    │ │ │ │ +
    │ │ │ │ +
    226 void erase(Key var) {
    │ │ │ │ +
    227 if (values_.unsafe_erase(var) == 0)
    │ │ │ │ +
    228 throw std::invalid_argument("Requested variable '" +
    │ │ │ │ +
    229 DefaultKeyFormatter(var) +
    │ │ │ │ +
    230 "', is not in this VectorValues.");
    │ │ │ │ +
    231 }
    │ │ │ │ +
    │ │ │ │ +
    232
    │ │ │ │ +
    234 void setZero();
    │ │ │ │ +
    235
    │ │ │ │ +
    236 iterator begin() { return values_.begin(); }
    │ │ │ │ +
    │ │ │ │ +
    237 const_iterator begin() const { return values_.begin(); }
    │ │ │ │ +
    │ │ │ │ +
    238 iterator end() { return values_.end(); }
    │ │ │ │ +
    │ │ │ │ +
    239 const_iterator end() const { return values_.end(); }
    │ │ │ │
    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
    │ │ │ │ +
    245 iterator find(Key j) { return values_.find(j); }
    │ │ │ │ +
    246
    │ │ │ │ +
    251 const_iterator find(Key j) const { return values_.find(j); }
    │ │ │ │ +
    252
    │ │ │ │ +
    254 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
    │ │ │ │ +
    255
    │ │ │ │ +
    257 void print(const std::string& str = "VectorValues",
    │ │ │ │ +
    258 const KeyFormatter& formatter = DefaultKeyFormatter) const;
    │ │ │ │ +
    259
    │ │ │ │ +
    261 bool equals(const VectorValues& x, double tol = 1e-9) const;
    │ │ │ │ +
    262
    │ │ │ │ +
    266
    │ │ │ │ +
    268 Vector vector() const;
    │ │ │ │
    269
    │ │ │ │ -
    273 DenseIndex& blockStart() { return blockStart_; }
    │ │ │ │ -
    274
    │ │ │ │ -
    277 DenseIndex blockStart() const { return blockStart_; }
    │ │ │ │ -
    278
    │ │ │ │ -
    289 void choleskyPartial(DenseIndex nFrontals);
    │ │ │ │ -
    290
    │ │ │ │ - │ │ │ │ +
    271 template <typename CONTAINER>
    │ │ │ │ +
    │ │ │ │ +
    272 Vector vector(const CONTAINER& keys) const {
    │ │ │ │ +
    273 DenseIndex totalDim = 0;
    │ │ │ │ + │ │ │ │ +
    275 items.reserve(keys.end() - keys.begin());
    │ │ │ │ +
    276 for (Key key : keys) {
    │ │ │ │ +
    277 const Vector* v = &at(key);
    │ │ │ │ +
    278 totalDim += v->size();
    │ │ │ │ +
    279 items.push_back(v);
    │ │ │ │ +
    280 }
    │ │ │ │ +
    281
    │ │ │ │ +
    282 Vector result(totalDim);
    │ │ │ │ +
    283 DenseIndex pos = 0;
    │ │ │ │ +
    284 for (const Vector* v : items) {
    │ │ │ │ +
    285 result.segment(pos, v->size()) = *v;
    │ │ │ │ +
    286 pos += v->size();
    │ │ │ │ +
    287 }
    │ │ │ │ +
    288
    │ │ │ │ +
    289 return result;
    │ │ │ │ +
    290 }
    │ │ │ │ +
    │ │ │ │ +
    291
    │ │ │ │ +
    293 Vector vector(const Dims& dims) const;
    │ │ │ │ +
    294
    │ │ │ │ +
    296 void swap(VectorValues& other);
    │ │ │ │
    297
    │ │ │ │ -
    298 protected:
    │ │ │ │ -
    299
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    302 return variableColOffsets_.size();
    │ │ │ │ -
    303 }
    │ │ │ │ -
    │ │ │ │ +
    299 bool hasSameStructure(const VectorValues other) const;
    │ │ │ │ +
    300
    │ │ │ │
    304
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    307 return nOffsets() - 1;
    │ │ │ │ -
    308 }
    │ │ │ │ -
    │ │ │ │ +
    308 double dot(const VectorValues& v) const;
    │ │ │ │
    309
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    311 double norm() const;
    │ │ │ │ +
    312
    │ │ │ │ +
    314 double squaredNorm() const;
    │ │ │ │ +
    315
    │ │ │ │ +
    318 VectorValues operator+(const VectorValues& c) const;
    │ │ │ │ +
    319
    │ │ │ │ +
    322 VectorValues add(const VectorValues& c) const;
    │ │ │ │ +
    323
    │ │ │ │ +
    326 VectorValues& operator+=(const VectorValues& c);
    │ │ │ │ +
    327
    │ │ │ │ +
    330 VectorValues& addInPlace(const VectorValues& c);
    │ │ │ │ +
    331
    │ │ │ │ +
    333 VectorValues& addInPlace_(const VectorValues& c);
    │ │ │ │ +
    334
    │ │ │ │ +
    337 VectorValues operator-(const VectorValues& c) const;
    │ │ │ │
    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}
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    341 VectorValues subtract(const VectorValues& c) const;
    │ │ │ │ +
    342
    │ │ │ │ +
    344 friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);
    │ │ │ │ +
    345
    │ │ │ │ +
    347 VectorValues scale(const double a) const;
    │ │ │ │ +
    348
    │ │ │ │ +
    350 VectorValues& operator*=(double alpha);
    │ │ │ │ +
    351
    │ │ │ │ +
    353 VectorValues& scaleInPlace(double alpha);
    │ │ │ │ +
    354
    │ │ │ │ +
    356
    │ │ │ │ +
    359
    │ │ │ │ +
    365 std::string html(
    │ │ │ │ +
    366 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │ +
    367
    │ │ │ │ +
    369
    │ │ │ │ +
    370 private:
    │ │ │ │ +
    372 friend class boost::serialization::access;
    │ │ │ │ +
    373 template<class ARCHIVE>
    │ │ │ │ +
    374 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    375 ar & BOOST_SERIALIZATION_NVP(values_);
    │ │ │ │ +
    376 }
    │ │ │ │ +
    377 }; // VectorValues definition
    │ │ │ │ +
    378
    │ │ │ │ +
    380 template<>
    │ │ │ │ +
    │ │ │ │ +
    381 struct traits<VectorValues> : public Testable<VectorValues> {
    │ │ │ │ +
    382 };
    │ │ │ │ +
    │ │ │ │ +
    383
    │ │ │ │ +
    384} // \namespace gtsam
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    Maps global variable indices to slot indices.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    Block full()
    Get the full matrix as a block.
    Definition SymmetricBlockMatrix.h:340
    │ │ │ │ -
    DenseIndex blockStart_
    Changes apparent matrix view, see main class comment.
    Definition SymmetricBlockMatrix.h:62
    │ │ │ │ -
    void setDiagonalBlock(DenseIndex I, const XprType &xpr)
    Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
    Definition SymmetricBlockMatrix.h:195
    │ │ │ │ -
    DenseIndex nActualBlocks() const
    Number of actual blocks in the full matrix.
    Definition SymmetricBlockMatrix.h:306
    │ │ │ │ -
    Vector diagonal(DenseIndex J) const
    Get the diagonal of the J'th diagonal block.
    Definition SymmetricBlockMatrix.h:145
    │ │ │ │ -
    Matrix matrix_
    The full matrix.
    Definition SymmetricBlockMatrix.h:59
    │ │ │ │ -
    DenseIndex cols() const
    Column size.
    Definition SymmetricBlockMatrix.h:117
    │ │ │ │ -
    DenseIndex getDim(DenseIndex block) const
    Number of dimensions for variable on this diagonal block.
    Definition SymmetricBlockMatrix.h:123
    │ │ │ │ -
    void setZero()
    Set the entire active matrix zero.
    Definition SymmetricBlockMatrix.h:258
    │ │ │ │ -
    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
    │ │ │ │ -
    void setFullMatrix(const XprType &xpr)
    Set the entire active matrix. Only reads the upper triangular part of xpr.
    Definition SymmetricBlockMatrix.h:253
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
    Get self adjoint view.
    Definition SymmetricBlockMatrix.h:242
    │ │ │ │ -
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ -
    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
    │ │ │ │ -
    constBlock full() const
    Get the full matrix as a block.
    Definition SymmetricBlockMatrix.h:335
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition SymmetricBlockMatrix.h:114
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:140
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex blockStart() const
    Retrieve the first logical block, i.e.
    Definition SymmetricBlockMatrix.h:277
    │ │ │ │ -
    Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
    Get self adjoint view.
    Definition SymmetricBlockMatrix.h:247
    │ │ │ │ -
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
    Update an off diagonal block.
    Definition SymmetricBlockMatrix.h:228
    │ │ │ │ -
    SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
    Construct from a container of the sizes of each block.
    Definition SymmetricBlockMatrix.h:70
    │ │ │ │ -
    SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
    Construct from iterator over the sizes of each vertical block.
    Definition SymmetricBlockMatrix.h:80
    │ │ │ │ -
    DenseIndex & blockStart()
    Retrieve or modify the first logical block, i.e.
    Definition SymmetricBlockMatrix.h:273
    │ │ │ │ -
    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
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex nOffsets() const
    Number of offsets in the full matrix.
    Definition SymmetricBlockMatrix.h:301
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex nBlocks() const
    Block count.
    Definition SymmetricBlockMatrix.h:120
    │ │ │ │ -
    FastVector< DenseIndex > variableColOffsets_
    the starting columns of each block (0-based)
    Definition SymmetricBlockMatrix.h:60
    │ │ │ │ -
    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
    │ │ │ │ -
    DenseIndex offset(DenseIndex block) const
    Get an offset for a block index (in the active view).
    Definition SymmetricBlockMatrix.h:311
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Errors operator+(const Errors &a, const Errors &b)
    Addition.
    Definition Errors.cpp:60
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    Errors operator-(const Errors &a, const Errors &b)
    Subtraction.
    Definition Errors.cpp:75
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition ConcurrentMap.h:68
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    value_type KeyValuePair
    Typedef to pair<Key, Vector>
    Definition VectorValues.h:85
    │ │ │ │ +
    const_iterator end() const
    Iterator over variables.
    Definition VectorValues.h:239
    │ │ │ │ +
    Values::value_type value_type
    Typedef to pair<Key, Vector>
    Definition VectorValues.h:84
    │ │ │ │ +
    Values::const_iterator const_iterator
    Const iterator over vector values.
    Definition VectorValues.h:82
    │ │ │ │ +
    iterator end()
    Iterator over variables.
    Definition VectorValues.h:238
    │ │ │ │ +
    Values::iterator iterator
    Iterator over vector values.
    Definition VectorValues.h:81
    │ │ │ │ +
    iterator insert(Key j, const Vector &value)
    Insert a vector value with key j.
    Definition VectorValues.h:197
    │ │ │ │ +
    VectorValues(const CONTAINER &c)
    Create from another container holding pair<Key,Vector>.
    Definition VectorValues.h:104
    │ │ │ │ +
    Values values_
    Vectors making up this VectorValues.
    Definition VectorValues.h:78
    │ │ │ │ +
    std::map< Key, size_t > Dims
    Keyed vector dimensions.
    Definition VectorValues.h:86
    │ │ │ │ +
    ConcurrentMap< Key, Vector > Values
    Collection of Vectors making up a VectorValues.
    Definition VectorValues.h:77
    │ │ │ │ +
    Vector & operator[](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:163
    │ │ │ │ +
    std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
    Emplace a vector value with key j.
    Definition VectorValues.h:185
    │ │ │ │ +
    iterator find(Key j)
    Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
    Definition VectorValues.h:245
    │ │ │ │ +
    size_t dim(Key j) const
    Return the dimension of variable j.
    Definition VectorValues.h:130
    │ │ │ │ +
    size_t size() const
    Number of variables stored.
    Definition VectorValues.h:127
    │ │ │ │ +
    const Vector & operator[](Key j) const
    Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
    Definition VectorValues.h:167
    │ │ │ │ +
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │ +
    VectorValues()
    Default constructor creates an empty VectorValues.
    Definition VectorValues.h:92
    │ │ │ │ +
    const_iterator find(Key j) const
    Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
    Definition VectorValues.h:251
    │ │ │ │ +
    VectorValues(ITERATOR first, ITERATOR last)
    Create from a pair of iterators over pair<Key,Vector>.
    Definition VectorValues.h:111
    │ │ │ │ +
    const Vector & at(Key j) const
    Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
    Definition VectorValues.h:152
    │ │ │ │ +
    Vector vector(const CONTAINER &keys) const
    Access a vector that is a subset of relevant keys.
    Definition VectorValues.h:272
    │ │ │ │ +
    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
    │ │ │ │ +
    const_iterator begin() const
    Iterator over variables.
    Definition VectorValues.h:237
    │ │ │ │ +
    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
    │ │ │ │ +
    void erase(Key var)
    Erase the vector with the given key, or throw std::out_of_range if it does not exist.
    Definition VectorValues.h:226
    │ │ │ │ +
    VectorValues(std::initializer_list< std::pair< Key, Vector > > init)
    Construct from initializer list.
    Definition VectorValues.h:95
    │ │ │ │ +
    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
    │ │ │ │ +
    VectorValues(const VectorValues &c)
    Implicit copy constructor to specialize the explicit constructor from any container.
    Definition VectorValues.h:107
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition VectorValues.h:83
    │ │ │ │ +
    iterator begin()
    Iterator over variables.
    Definition VectorValues.h:236
    │ │ │ │ +
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,546 +1,468 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymmetricBlockMatrix.h │ │ │ │ │ +VectorValues.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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 │ │ │ │ │ +20#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_c_a_t_t_e_r_._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_/_V_e_c_t_o_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +26 │ │ │ │ │ +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 } │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33 │ │ │ │ │ +34namespace _g_t_s_a_m { │ │ │ │ │ +35 │ │ │ │ │ +_7_4 class GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s { │ │ │ │ │ +75 protected: │ │ │ │ │ +76 typedef _V_e_c_t_o_r_V_a_l_u_e_s _T_h_i_s; │ │ │ │ │ +_7_7 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_e_y_,_ _V_e_c_t_o_r_> _V_a_l_u_e_s; │ │ │ │ │ +_7_8 _V_a_l_u_e_s _v_a_l_u_e_s__; │ │ │ │ │ +79 │ │ │ │ │ +80 public: │ │ │ │ │ +_8_1 typedef Values::iterator _i_t_e_r_a_t_o_r; │ │ │ │ │ +_8_2 typedef Values::const_iterator _c_o_n_s_t___i_t_e_r_a_t_o_r; │ │ │ │ │ +_8_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_8_4 typedef _V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e _v_a_l_u_e___t_y_p_e; │ │ │ │ │ +_8_5 typedef _v_a_l_u_e___t_y_p_e _K_e_y_V_a_l_u_e_P_a_i_r; │ │ │ │ │ +_8_6 typedef std::map _D_i_m_s; │ │ │ │ │ 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); │ │ │ │ │ +90 │ │ │ │ │ +_9_2 _V_e_c_t_o_r_V_a_l_u_e_s() {} │ │ │ │ │ +93 │ │ │ │ │ +_9_5 _V_e_c_t_o_r_V_a_l_u_e_s(std::initializer_list> init) │ │ │ │ │ +96 : values_(init.begin(), init.end()) {} │ │ │ │ │ +97 │ │ │ │ │ +100 _V_e_c_t_o_r_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& first, const _V_e_c_t_o_r_V_a_l_u_e_s& second); │ │ │ │ │ +101 │ │ │ │ │ +103 template │ │ │ │ │ +_1_0_4 explicit _V_e_c_t_o_r_V_a_l_u_e_s(const CONTAINER& c) : values_(c.begin(), c.end()) {} │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 _V_e_c_t_o_r_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& c) : values_(c.values_) {} │ │ │ │ │ +108 │ │ │ │ │ +110 template │ │ │ │ │ +_1_1_1 _V_e_c_t_o_r_V_a_l_u_e_s(ITERATOR first, ITERATOR last) : values_(first, last) {} │ │ │ │ │ 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_]; } │ │ │ │ │ +114 _V_e_c_t_o_r_V_a_l_u_e_s(const Vector& c, const Dims& dims); │ │ │ │ │ 115 │ │ │ │ │ -_1_1_7 _D_e_n_s_e_I_n_d_e_x _c_o_l_s() const { return rows(); } │ │ │ │ │ +117 _V_e_c_t_o_r_V_a_l_u_e_s(const Vector& c, const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ 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_; } │ │ │ │ │ +120 static _V_e_c_t_o_r_V_a_l_u_e_s Zero(const _V_e_c_t_o_r_V_a_l_u_e_s& other); │ │ │ │ │ 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 } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 size_t _s_i_z_e() const { return values_.size(); } │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 size_t _d_i_m(_K_e_y j) const { return at(j).rows(); } │ │ │ │ │ +131 │ │ │ │ │ +_1_3_3 bool _e_x_i_s_t_s(_K_e_y j) const { return find(j) != end(); } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_9 Vector& _a_t(_K_e_y j) { │ │ │ │ │ +140 _i_t_e_r_a_t_o_r item = find(j); │ │ │ │ │ +141 if (item == end()) │ │ │ │ │ +142 throw std::out_of_range( │ │ │ │ │ +143 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this │ │ │ │ │ +VectorValues."); │ │ │ │ │ +144 else │ │ │ │ │ +145 return item->second; │ │ │ │ │ +146 } │ │ │ │ │ +147 │ │ │ │ │ +_1_5_2 const Vector& _a_t(_K_e_y j) const { │ │ │ │ │ +153 _c_o_n_s_t___i_t_e_r_a_t_o_r item = find(j); │ │ │ │ │ +154 if (item == end()) │ │ │ │ │ +155 throw std::out_of_range( │ │ │ │ │ +156 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this │ │ │ │ │ +VectorValues."); │ │ │ │ │ +157 else │ │ │ │ │ +158 return item->second; │ │ │ │ │ +159 } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_3 Vector& _o_p_e_r_a_t_o_r_[_](_K_e_y j) { return at(j); } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_7 const Vector& _o_p_e_r_a_t_o_r_[_](_K_e_y j) const { return at(j); } │ │ │ │ │ 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 } │ │ │ │ │ +172 _V_e_c_t_o_r_V_a_l_u_e_s& update(const _V_e_c_t_o_r_V_a_l_u_e_s& values); │ │ │ │ │ +173 │ │ │ │ │ +178 iterator insert(const std::pair& key_value); │ │ │ │ │ 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 │ │ │ │ │ +184 template │ │ │ │ │ +_1_8_5 inline std::pair _e_m_p_l_a_c_e(_K_e_y j, Args&&... │ │ │ │ │ +args) { │ │ │ │ │ +186#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) │ │ │ │ │ +187 return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), │ │ │ │ │ +std::forward_as_tuple(args...)); │ │ │ │ │ +188#else │ │ │ │ │ +189 return values_.insert(std::make_pair(j, Vector(std::forward │ │ │ │ │ +(args)...))); │ │ │ │ │ +190#endif │ │ │ │ │ +191 } │ │ │ │ │ 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 } │ │ │ │ │ +_1_9_7 _i_t_e_r_a_t_o_r _i_n_s_e_r_t(_K_e_y j, const Vector& value) { │ │ │ │ │ +198 return insert(std::make_pair(j, value)); │ │ │ │ │ +199 } │ │ │ │ │ +200 │ │ │ │ │ +203 _V_e_c_t_o_r_V_a_l_u_e_s& insert(const _V_e_c_t_o_r_V_a_l_u_e_s& values); │ │ │ │ │ +204 │ │ │ │ │ +_2_0_9 inline std::pair _t_r_y_I_n_s_e_r_t(_K_e_y j, const Vector& value) { │ │ │ │ │ +210#ifdef TBB_GREATER_EQUAL_2020 │ │ │ │ │ +211 return values_.emplace(j, value); │ │ │ │ │ +212#else │ │ │ │ │ +213 return values_.insert(std::make_pair(j, value)); │ │ │ │ │ +214#endif │ │ │ │ │ +215 } │ │ │ │ │ +216 │ │ │ │ │ +_2_1_9 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const Vector& value) { │ │ │ │ │ +220 if (!tryInsert(j, value).second) { │ │ │ │ │ +221 (*this)[j] = value; │ │ │ │ │ 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 │ │ │ │ │ +_2_2_6 void _e_r_a_s_e(_K_e_y var) { │ │ │ │ │ +227 if (values_.unsafe_erase(var) == 0) │ │ │ │ │ +228 throw std::invalid_argument("Requested variable '" + │ │ │ │ │ +229 DefaultKeyFormatter(var) + │ │ │ │ │ +230 "', is not in this VectorValues."); │ │ │ │ │ +231 } │ │ │ │ │ +232 │ │ │ │ │ +234 void setZero(); │ │ │ │ │ +235 │ │ │ │ │ +_2_3_6 _i_t_e_r_a_t_o_r _b_e_g_i_n() { return values_.begin(); } │ │ │ │ │ +_2_3_7 _c_o_n_s_t___i_t_e_r_a_t_o_r _b_e_g_i_n() const { return values_.begin(); } │ │ │ │ │ +_2_3_8 _i_t_e_r_a_t_o_r _e_n_d() { return values_.end(); } │ │ │ │ │ +_2_3_9 _c_o_n_s_t___i_t_e_r_a_t_o_r _e_n_d() const { return values_.end(); } │ │ │ │ │ 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 │ │ │ │ │ +_2_4_5 _i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) { return values_.find(j); } │ │ │ │ │ +246 │ │ │ │ │ +_2_5_1 _c_o_n_s_t___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) const { return values_.find(j); } │ │ │ │ │ +252 │ │ │ │ │ +254 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s&); │ │ │ │ │ +255 │ │ │ │ │ +257 void _p_r_i_n_t(const std::string& str = "VectorValues", │ │ │ │ │ +258 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ +259 │ │ │ │ │ +261 bool _e_q_u_a_l_s(const _V_e_c_t_o_r_V_a_l_u_e_s& x, double tol = 1e-9) const; │ │ │ │ │ +262 │ │ │ │ │ +266 │ │ │ │ │ +268 Vector vector() const; │ │ │ │ │ 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); │ │ │ │ │ +271 template │ │ │ │ │ +_2_7_2 Vector _v_e_c_t_o_r(const CONTAINER& keys) const { │ │ │ │ │ +273 _D_e_n_s_e_I_n_d_e_x totalDim = 0; │ │ │ │ │ +274 _F_a_s_t_V_e_c_t_o_r_<_c_o_n_s_t_ _V_e_c_t_o_r_*_> items; │ │ │ │ │ +275 items.reserve(keys.end() - keys.begin()); │ │ │ │ │ +276 for (_K_e_y key : keys) { │ │ │ │ │ +277 const Vector* v = &at(key); │ │ │ │ │ +278 totalDim += v->size(); │ │ │ │ │ +279 items.push_back(v); │ │ │ │ │ +280 } │ │ │ │ │ +281 │ │ │ │ │ +282 Vector result(totalDim); │ │ │ │ │ +283 _D_e_n_s_e_I_n_d_e_x pos = 0; │ │ │ │ │ +284 for (const Vector* v : items) { │ │ │ │ │ +285 result.segment(pos, v->size()) = *v; │ │ │ │ │ +286 pos += v->size(); │ │ │ │ │ +287 } │ │ │ │ │ +288 │ │ │ │ │ +289 return result; │ │ │ │ │ +290 } │ │ │ │ │ +291 │ │ │ │ │ +293 Vector vector(const Dims& dims) const; │ │ │ │ │ +294 │ │ │ │ │ +296 void swap(_V_e_c_t_o_r_V_a_l_u_e_s& other); │ │ │ │ │ 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 } │ │ │ │ │ +299 bool hasSameStructure(const _V_e_c_t_o_r_V_a_l_u_e_s other) const; │ │ │ │ │ +300 │ │ │ │ │ 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 } │ │ │ │ │ +308 double _d_o_t(const _V_e_c_t_o_r_V_a_l_u_e_s& v) const; │ │ │ │ │ 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 } │ │ │ │ │ +311 double norm() const; │ │ │ │ │ +312 │ │ │ │ │ +314 double squaredNorm() const; │ │ │ │ │ +315 │ │ │ │ │ +318 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_+(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +319 │ │ │ │ │ +322 _V_e_c_t_o_r_V_a_l_u_e_s add(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +323 │ │ │ │ │ +326 _V_e_c_t_o_r_V_a_l_u_e_s& operator+=(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ +327 │ │ │ │ │ +330 _V_e_c_t_o_r_V_a_l_u_e_s& addInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ +331 │ │ │ │ │ +333 _V_e_c_t_o_r_V_a_l_u_e_s& addInPlace_(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ +334 │ │ │ │ │ +337 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_-(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ 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} │ │ │ │ │ +341 _V_e_c_t_o_r_V_a_l_u_e_s subtract(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +342 │ │ │ │ │ +344 friend GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_*(const double a, const │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s &v); │ │ │ │ │ +345 │ │ │ │ │ +347 _V_e_c_t_o_r_V_a_l_u_e_s scale(const double a) const; │ │ │ │ │ +348 │ │ │ │ │ +350 _V_e_c_t_o_r_V_a_l_u_e_s& operator*=(double alpha); │ │ │ │ │ +351 │ │ │ │ │ +353 _V_e_c_t_o_r_V_a_l_u_e_s& scaleInPlace(double alpha); │ │ │ │ │ +354 │ │ │ │ │ +356 │ │ │ │ │ +359 │ │ │ │ │ +365 std::string _h_t_m_l( │ │ │ │ │ +366 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +367 │ │ │ │ │ +369 │ │ │ │ │ +370 private: │ │ │ │ │ +_3_7_2 friend class boost::serialization::access; │ │ │ │ │ +373 template │ │ │ │ │ +374 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +375 ar & BOOST_SERIALIZATION_NVP(values_); │ │ │ │ │ +376 } │ │ │ │ │ +377 }; // VectorValues definition │ │ │ │ │ +378 │ │ │ │ │ +380 template<> │ │ │ │ │ +_3_8_1 struct _t_r_a_i_t_s<_V_e_c_t_o_r_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +382 }; │ │ │ │ │ +383 │ │ │ │ │ +384} // \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. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_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. │ │ │ │ │ +_S_c_a_t_t_e_r_._h │ │ │ │ │ +Maps global variable indices to slot indices. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +value_type KeyValuePair │ │ │ │ │ +Typedef to pair │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_n_d │ │ │ │ │ +const_iterator end() const │ │ │ │ │ +Iterator over variables. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:239 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e │ │ │ │ │ +Values::value_type value_type │ │ │ │ │ +Typedef to pair │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +Values::const_iterator const_iterator │ │ │ │ │ +Const iterator over vector values. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_n_d │ │ │ │ │ +iterator end() │ │ │ │ │ +Iterator over variables. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:238 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ +Values::iterator iterator │ │ │ │ │ +Iterator over vector values. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:81 │ │ │ │ │ +_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(Key j, const Vector &value) │ │ │ │ │ +Insert a vector value with key j. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:197 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues(const CONTAINER &c) │ │ │ │ │ +Create from another container holding pair. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e_s__ │ │ │ │ │ +Values values_ │ │ │ │ │ +Vectors making up this VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_D_i_m_s │ │ │ │ │ +std::map< Key, size_t > Dims │ │ │ │ │ +Keyed vector dimensions. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ +ConcurrentMap< Key, Vector > Values │ │ │ │ │ +Collection of Vectors making up a VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +Vector & operator[](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:163 │ │ │ │ │ +_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_:_:_f_i_n_d │ │ │ │ │ +iterator find(Key j) │ │ │ │ │ +Return the iterator corresponding to the requested key, or end() if no variable │ │ │ │ │ +is present with this ... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:245 │ │ │ │ │ +_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +Number of variables stored. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +const Vector & operator[](Key j) const │ │ │ │ │ +Access the vector value with key j (const version), throws std::out_of_range if │ │ │ │ │ +j does not exist,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:167 │ │ │ │ │ +_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues() │ │ │ │ │ +Default constructor creates an empty VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_f_i_n_d │ │ │ │ │ +const_iterator find(Key j) const │ │ │ │ │ +Return the iterator corresponding to the requested key, or end() if no variable │ │ │ │ │ +is present with this ... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues(ITERATOR first, ITERATOR last) │ │ │ │ │ +Create from a pair of iterators over pair. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const Vector & at(Key j) const │ │ │ │ │ +Access the vector value with key j (const version), throws std::out_of_range if │ │ │ │ │ +j does not exist,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector vector(const CONTAINER &keys) const │ │ │ │ │ +Access a vector that is a subset of relevant keys. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:272 │ │ │ │ │ +_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_:_:_b_e_g_i_n │ │ │ │ │ +const_iterator begin() const │ │ │ │ │ +Iterator over variables. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:237 │ │ │ │ │ +_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_:_:_e_r_a_s_e │ │ │ │ │ +void erase(Key var) │ │ │ │ │ +Erase the vector with the given key, or throw std::out_of_range if it does not │ │ │ │ │ +exist. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:226 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues(std::initializer_list< std::pair< Key, Vector > > init) │ │ │ │ │ +Construct from initializer list. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:95 │ │ │ │ │ +_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues(const VectorValues &c) │ │ │ │ │ +Implicit copy constructor to specialize the explicit constructor from any │ │ │ │ │ +container. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_b_e_g_i_n │ │ │ │ │ +iterator begin() │ │ │ │ │ +Iterator over variables. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:236 │ │ │ │ │ +_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _V_e_c_t_o_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/a00176_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │┄ Files 6% 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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    62 const SharedNoiseModel& model,
    │ │ │ │ -
    63 size_t N) {
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace internal { struct NonlinearOptimizerState; }
    │ │ │ │ +
    27
    │ │ │ │ +
    │ │ │ │ +
    75class GTSAM_EXPORT NonlinearOptimizer {
    │ │ │ │ +
    76
    │ │ │ │ +
    77protected:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    Factor definitions for various Basis functors.
    │ │ │ │ -
    Compute an interpolating basis.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ +
    137
    │ │ │ │ +
    139
    │ │ │ │ +
    140protected:
    │ │ │ │ +
    144 void defaultOptimize();
    │ │ │ │ +
    145
    │ │ │ │ +
    146 virtual const NonlinearOptimizerParams& _params() const = 0;
    │ │ │ │ +
    147
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Parameters for nonlinear optimization.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::map< double, double > Sequence
    Our sequence representation is a map of {x: y} values where y = f(x)
    Definition FitBasis.h:36
    │ │ │ │ -
    std::pair< double, double > Sample
    A sample is a key-value pair from a sequence.
    Definition FitBasis.h:38
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
    Definition BasisFactors.h:39
    │ │ │ │ -
    Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebysh...
    Definition FitBasis.h:52
    │ │ │ │ -
    static NonlinearFactorGraph NonlinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
    Create nonlinear FG from Sequence.
    Definition FitBasis.h:61
    │ │ │ │ -
    Parameters parameters() const
    Return Fourier coefficients.
    Definition FitBasis.h:96
    │ │ │ │ -
    FitBasis(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
    Construct a new FitBasis object.
    Definition FitBasis.h:89
    │ │ │ │ -
    static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
    Create linear FG from Sequence.
    Definition FitBasis.h:73
    │ │ │ │ +
    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
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    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
    │ │ │ │
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ +
    const NonlinearFactorGraph & graph() const
    return the graph with nonlinear factors
    Definition NonlinearOptimizer.h:118
    │ │ │ │ +
    NonlinearFactorGraph graph_
    The graph with nonlinear factors.
    Definition NonlinearOptimizer.h:78
    │ │ │ │ +
    virtual const Values & optimize()
    Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
    Definition NonlinearOptimizer.h:98
    │ │ │ │ +
    std::unique_ptr< internal::NonlinearOptimizerState > state_
    PIMPL'd state.
    Definition NonlinearOptimizer.h:80
    │ │ │ │ +
    boost::shared_ptr< const NonlinearOptimizer > shared_ptr
    A shared pointer to this class.
    Definition NonlinearOptimizer.h:84
    │ │ │ │ +
    virtual GaussianFactorGraph::shared_ptr iterate()=0
    Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor grap...
    │ │ │ │ +
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    Verbosity
    See NonlinearOptimizerParams::verbosity.
    Definition NonlinearOptimizerParams.h:37
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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/a00203_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00254_source.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    DiscreteJunctionTree.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │
    27 // Forward declarations
    │ │ │ │
    28 class DiscreteEliminationTree;
    │ │ │ │
    29
    │ │ │ │
    │ │ │ │ @@ -137,29 +137,29 @@ │ │ │ │
    57
    │ │ │ │
    66 DiscreteJunctionTree(const DiscreteEliminationTree& eliminationTree);
    │ │ │ │
    67 };
    │ │ │ │
    │ │ │ │
    68
    │ │ │ │ │ │ │ │
    71}
    │ │ │ │ -
    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
    │ │ │ │ - │ │ │ │ -
    The junction tree.
    │ │ │ │ + │ │ │ │ +
    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
    │ │ │ │ +
    The junction tree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    Elimination tree for discrete factors.
    Definition DiscreteEliminationTree.h:33
    │ │ │ │
    An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition DiscreteJunctionTree.h:52
    │ │ │ │
    JunctionTree< DiscreteBayesTree, DiscreteFactorGraph > Base
    Base class.
    Definition DiscreteJunctionTree.h:54
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition DiscreteJunctionTree.h:56
    │ │ │ │
    DiscreteJunctionTree This
    This class.
    Definition DiscreteJunctionTree.h:55
    │ │ │ │
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -37,17 +37,17 @@ │ │ │ │ │ _5_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ 57 │ │ │ │ │ 66 _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e(const _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ 67 }; │ │ │ │ │ 68 │ │ │ │ │ _7_0 using _D_i_s_c_r_e_t_e_C_l_u_s_t_e_r = _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r; │ │ │ │ │ 71} │ │ │ │ │ +_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_T_r_e_e_._h │ │ │ │ │ Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ _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_:_:_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. │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00224.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01376.html │ │ │ │┄ Files 2% similar despite different names │ │ │ │ @@ -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/symbolic/SymbolicBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    DiscreteJunctionTree.cpp File Reference
    │ │ │ │ +
    SymbolicBayesNet.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteJunctionTree.cpp File Reference │ │ │ │ │ +SymbolicBayesNet.cpp File Reference │ │ │ │ │ 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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 0% 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>
    │ │ │ │
    │ │ │ │ │ │ │ │
    230 : public Testable<AlgebraicDecisionTree<T>> {};
    │ │ │ │
    │ │ │ │
    231} // namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
    free versions of apply
    Definition DecisionTree.h:413
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │
    AlgebraicDecisionTree operator/(const AlgebraicDecisionTree &g) const
    division
    Definition AlgebraicDecisionTree.h:194
    │ │ │ │
    AlgebraicDecisionTree sum(const L &label, size_t cardinality) const
    sum out variable
    Definition AlgebraicDecisionTree.h:199
    │ │ │ │ @@ -302,13 +302,13 @@ │ │ │ │
    NodePtr root_
    A DecisionTree just contains the root. TODO(dellaert): make protected.
    Definition DecisionTree.h:146
    │ │ │ │
    std::pair< L, size_t > LabelC
    A label annotated with cardinality.
    Definition DecisionTree.h:79
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00233_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00182_source.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    DiscreteBayesNet.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    25
    │ │ │ │
    26#include <boost/shared_ptr.hpp>
    │ │ │ │
    27#include <map>
    │ │ │ │
    28#include <string>
    │ │ │ │
    29#include <utility>
    │ │ │ │
    30#include <vector>
    │ │ │ │
    31
    │ │ │ │ @@ -229,18 +229,18 @@ │ │ │ │
    │ │ │ │
    169
    │ │ │ │
    170// traits
    │ │ │ │
    171template<> struct traits<DiscreteBayesNet> : public Testable<DiscreteBayesNet> {};
    │ │ │ │
    172
    │ │ │ │
    173} // \ namespace gtsam
    │ │ │ │
    174
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Bayes network.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Bayes network.
    │ │ │ │
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ @@ -258,13 +258,13 @@ │ │ │ │
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00239_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00266_source.html │ │ │ │┄ Files 7% 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
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/inference/Key.h>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ + │ │ │ │ +
    80 using This = DiscreteLookupDAG;
    │ │ │ │ +
    81 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    82
    │ │ │ │ +
    85
    │ │ │ │ + │ │ │ │ +
    88
    │ │ │ │ +
    90 static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet);
    │ │ │ │ +
    91
    │ │ │ │ +
    93 virtual ~DiscreteLookupDAG() {}
    │ │ │ │ +
    94
    │ │ │ │ +
    96
    │ │ │ │
    99
    │ │ │ │ -
    100 } // namespace gtsam
    │ │ │ │ - │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ -
    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
    │ │ │ │ + │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Bayes network.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2)
    Create a list from two keys.
    Definition DiscreteKey.cpp:46
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ + │ │ │ │ +
    A Bayes net made from discrete conditional distributions.
    Definition DiscreteBayesNet.h:38
    │ │ │ │ +
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    DiscreteKeys & operator&(const DiscreteKey &key)
    Add a key (non-const!)
    Definition DiscreteKey.h:70
    │ │ │ │ -
    DiscreteKeys()
    Constructor for serialization.
    Definition DiscreteKey.h:45
    │ │ │ │ -
    DiscreteKeys(const DiscreteKey &key)
    Construct from a key.
    Definition DiscreteKey.h:48
    │ │ │ │ -
    DiscreteKeys(const std::vector< DiscreteKey > &keys)
    Construct from a vector of keys.
    Definition DiscreteKey.h:56
    │ │ │ │ -
    DiscreteKeys(std::map< Key, size_t > cardinalities)
    Construct from cardinalities.
    Definition DiscreteKey.h:51
    │ │ │ │ +
    DiscreteLookupTable table for max-product.
    Definition DiscreteLookupDAG.h:40
    │ │ │ │ +
    DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT &potentials)
    Construct a new Discrete Lookup Table object.
    Definition DiscreteLookupDAG.h:53
    │ │ │ │ +
    A DAG made from lookup tables, as defined above.
    Definition DiscreteLookupDAG.h:77
    │ │ │ │ +
    bool equals(const This &bn, double tol=1e-9) const
    Check equality.
    │ │ │ │ +
    DiscreteLookupDAG()
    Construct empty DAG.
    Definition DiscreteLookupDAG.h:87
    │ │ │ │ +
    void add(Args &&... args)
    Add a DiscreteLookupTable.
    Definition DiscreteLookupDAG.h:110
    │ │ │ │ +
    virtual ~DiscreteLookupDAG()
    Destructor.
    Definition DiscreteLookupDAG.h:93
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    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/a00248_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01034_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/nonlinear/LinearContainerFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DiscreteLookupDAG.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ + │ │ │ │ +
    13
    │ │ │ │ +
    14namespace gtsam {
    │ │ │ │ +
    15
    │ │ │ │ +
    16 // Forward declarations
    │ │ │ │ +
    17 class JacobianFactor;
    │ │ │ │ +
    18 class HessianFactor;
    │ │ │ │
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    25#include <string>
    │ │ │ │ -
    26#include <utility>
    │ │ │ │ -
    27#include <vector>
    │ │ │ │ +
    │ │ │ │ +
    26class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor {
    │ │ │ │ +
    27protected:
    │ │ │ │
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    31class DiscreteBayesNet;
    │ │ │ │ -
    32
    │ │ │ │ -
    │ │ │ │ -
    40class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
    │ │ │ │ -
    41 public:
    │ │ │ │ - │ │ │ │ -
    43 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    44 using BaseConditional = Conditional<DecisionTreeFactor, This>;
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    38
    │ │ │ │ +
    39public:
    │ │ │ │ +
    40
    │ │ │ │ +
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    42
    │ │ │ │ + │ │ │ │
    45
    │ │ │ │ -
    │ │ │ │ -
    53 DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
    │ │ │ │ -
    54 const ADT& potentials)
    │ │ │ │ -
    55 : DiscreteConditional(nFrontals, keys, potentials) {}
    │ │ │ │ -
    │ │ │ │ +
    47 LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
    │ │ │ │ +
    48
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    73 void argmaxInPlace(DiscreteValues* parentsValues) const;
    │ │ │ │ -
    74};
    │ │ │ │ -
    │ │ │ │ -
    75
    │ │ │ │ -
    │ │ │ │ -
    77class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
    │ │ │ │ -
    78 public:
    │ │ │ │ - │ │ │ │ -
    80 using This = DiscreteLookupDAG;
    │ │ │ │ -
    81 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    82
    │ │ │ │ -
    85
    │ │ │ │ - │ │ │ │ -
    88
    │ │ │ │ -
    90 static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet);
    │ │ │ │ -
    91
    │ │ │ │ -
    93 virtual ~DiscreteLookupDAG() {}
    │ │ │ │ -
    94
    │ │ │ │ -
    96
    │ │ │ │ -
    99
    │ │ │ │ -
    101 bool equals(const This& bn, double tol = 1e-9) const;
    │ │ │ │ +
    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
    │ │ │ │ +
    106 GaussianFactor::shared_ptr negateToGaussian() const;
    │ │ │ │
    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};
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    137// traits
    │ │ │ │ -
    138template <>
    │ │ │ │ -
    139struct traits<DiscreteLookupDAG> : public Testable<DiscreteLookupDAG> {};
    │ │ │ │ -
    140
    │ │ │ │ -
    141} // namespace gtsam
    │ │ │ │ - │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Bayes network.
    │ │ │ │ +
    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
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ - │ │ │ │ -
    A Bayes net made from discrete conditional distributions.
    Definition DiscreteBayesNet.h:38
    │ │ │ │ -
    Discrete Conditional Density Derives from DecisionTreeFactor.
    Definition DiscreteConditional.h:40
    │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    DiscreteLookupTable table for max-product.
    Definition DiscreteLookupDAG.h:40
    │ │ │ │ -
    DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT &potentials)
    Construct a new Discrete Lookup Table object.
    Definition DiscreteLookupDAG.h:53
    │ │ │ │ -
    A DAG made from lookup tables, as defined above.
    Definition DiscreteLookupDAG.h:77
    │ │ │ │ -
    bool equals(const This &bn, double tol=1e-9) const
    Check equality.
    │ │ │ │ -
    DiscreteLookupDAG()
    Construct empty DAG.
    Definition DiscreteLookupDAG.h:87
    │ │ │ │ -
    void add(Args &&... args)
    Add a DiscreteLookupTable.
    Definition DiscreteLookupDAG.h:110
    │ │ │ │ -
    virtual ~DiscreteLookupDAG()
    Destructor.
    Definition DiscreteLookupDAG.h:93
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    Dummy version of a generic linear factor to be injected into a nonlinear factor graph.
    Definition LinearContainerFactor.h:26
    │ │ │ │ +
    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
    │ │ │ │ +
    LinearContainerFactor()
    Default constructor - necessary for serialization.
    Definition LinearContainerFactor.h:44
    │ │ │ │ +
    const boost::optional< Values > & linearizationPoint() const
    Extract the linearization point used in recalculating error.
    Definition LinearContainerFactor.h:83
    │ │ │ │ +
    bool hasLinearizationPoint() const
    Casting syntactic sugar.
    Definition LinearContainerFactor.h:138
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,164 +1,185 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteLookupDAG.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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_>; │ │ │ │ │ +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 │ │ │ │ │ -_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) {} │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -_1_0_1 bool _e_q_u_a_l_s(const _T_h_i_s& bn, double tol = 1e-9) const; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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}; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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_:_:_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_:_:_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_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_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_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_:_:_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_:_:_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 │ │ │ │ │ +_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/inference/Symbol.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    DiscreteEliminationTree.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Symbol.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    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

    │ │ │ │ +GTSAM_EXPORT std::ostream & gtsam::operator<< (std::ostream &os, const Symbol &symbol)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Mar 29, 2013
    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    June 9, 2012
    │ │ │ │ +
    Author
    : Frank Dellaert
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ +: 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 │ │ │ │ │ -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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + June 9, 2012 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + : 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_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _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/a00263_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00347_source.html │ │ │ │┄ Files 8% 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/geometry/Similarity2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DiscreteFactor.h
    │ │ │ │ +
    Similarity2.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <gtsam/base/Testable.h>
    │ │ │ │ -
    24
    │ │ │ │ -
    25#include <string>
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28class DecisionTreeFactor;
    │ │ │ │ -
    29class DiscreteConditional;
    │ │ │ │ -
    30class HybridValues;
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/base/Lie.h>
    │ │ │ │ +
    21#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    22#include <gtsam/dllexport.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    29// Forward declarations
    │ │ │ │ +
    30class Pose2;
    │ │ │ │
    31
    │ │ │ │ -
    │ │ │ │ -
    38class GTSAM_EXPORT DiscreteFactor: public Factor {
    │ │ │ │ -
    39
    │ │ │ │ -
    40public:
    │ │ │ │ +
    │ │ │ │ +
    35class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
    │ │ │ │ +
    38 typedef Rot2 Rotation;
    │ │ │ │ +
    39 typedef Point2 Translation;
    │ │ │ │
    41
    │ │ │ │ -
    42 // typedefs needed to play nice with gtsam
    │ │ │ │ - │ │ │ │ -
    44 typedef boost::shared_ptr<DiscreteFactor> shared_ptr;
    │ │ │ │ -
    45 typedef Factor Base;
    │ │ │ │ +
    42 private:
    │ │ │ │ +
    43 Rot2 R_;
    │ │ │ │ +
    44 Point2 t_;
    │ │ │ │ +
    45 double s_;
    │ │ │ │
    46
    │ │ │ │ - │ │ │ │ -
    48
    │ │ │ │ -
    49public:
    │ │ │ │ +
    47 public:
    │ │ │ │
    50
    │ │ │ │ + │ │ │ │
    53
    │ │ │ │ - │ │ │ │ +
    55 Similarity2(double s);
    │ │ │ │
    56
    │ │ │ │ -
    59 template<typename CONTAINER>
    │ │ │ │ -
    60 DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
    │ │ │ │ -
    61
    │ │ │ │ -
    │ │ │ │ -
    63 virtual ~DiscreteFactor() {
    │ │ │ │ -
    64 }
    │ │ │ │ -
    │ │ │ │ +
    58 Similarity2(const Rot2& R, const Point2& t, double s);
    │ │ │ │ +
    59
    │ │ │ │ +
    61 Similarity2(const Matrix2& R, const Vector2& t, double s);
    │ │ │ │ +
    62
    │ │ │ │ +
    64 Similarity2(const Matrix3& T);
    │ │ │ │
    65
    │ │ │ │
    69
    │ │ │ │ -
    71 virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
    │ │ │ │ +
    71 bool equals(const Similarity2& sim, double tol) const;
    │ │ │ │
    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;
    │ │ │ │ -
    89
    │ │ │ │ -
    94 double error(const HybridValues& c) const override;
    │ │ │ │ -
    95
    │ │ │ │ - │ │ │ │ -
    98
    │ │ │ │ -
    99 virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
    │ │ │ │ +
    74 bool operator==(const Similarity2& other) const;
    │ │ │ │ +
    75
    │ │ │ │ +
    77 void print(const std::string& s) const;
    │ │ │ │ +
    78
    │ │ │ │ +
    79 friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
    │ │ │ │ +
    80
    │ │ │ │ +
    84
    │ │ │ │ +
    86 static Similarity2 Identity();
    │ │ │ │ +
    87
    │ │ │ │ +
    89 Similarity2 operator*(const Similarity2& S) const;
    │ │ │ │ +
    90
    │ │ │ │ +
    92 Similarity2 inverse() const;
    │ │ │ │ +
    93
    │ │ │ │ +
    97
    │ │ │ │ +
    99 Point2 transformFrom(const Point2& p) const;
    │ │ │ │
    100
    │ │ │ │ -
    104
    │ │ │ │ - │ │ │ │ -
    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> {};
    │ │ │ │ +
    112 Pose2 transformFrom(const Pose2& T) const;
    │ │ │ │ +
    113
    │ │ │ │ +
    114 /* syntactic sugar for transformFrom */
    │ │ │ │ +
    115 Point2 operator*(const Point2& p) const;
    │ │ │ │ +
    116
    │ │ │ │ +
    120 static Similarity2 Align(const Point2Pairs& abPointPairs);
    │ │ │ │ +
    121
    │ │ │ │ +
    135 static Similarity2 Align(const Pose2Pairs& abPosePairs);
    │ │ │ │
    136
    │ │ │ │ -
    137
    │ │ │ │ -
    154std::vector<double> expNormalize(const std::vector<double> &logProbs);
    │ │ │ │ -
    155
    │ │ │ │ -
    156
    │ │ │ │ -
    157}// namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ - │ │ │ │ -
    The base class for all factors.
    │ │ │ │ +
    140
    │ │ │ │ +
    145 static Vector4 Logmap(const Similarity2& S, //
    │ │ │ │ +
    146 OptionalJacobian<4, 4> Hm = boost::none);
    │ │ │ │ +
    147
    │ │ │ │ +
    149 static Similarity2 Expmap(const Vector4& v, //
    │ │ │ │ +
    150 OptionalJacobian<4, 4> Hm = boost::none);
    │ │ │ │ +
    151
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    154 static Similarity2 Retract(const Vector4& v,
    │ │ │ │ +
    155 ChartJacobian H = boost::none) {
    │ │ │ │ +
    156 return Similarity2::Expmap(v, H);
    │ │ │ │ +
    157 }
    │ │ │ │ +
    158 static Vector4 Local(const Similarity2& other,
    │ │ │ │ +
    159 ChartJacobian H = boost::none) {
    │ │ │ │ +
    160 return Similarity2::Logmap(other, H);
    │ │ │ │ +
    161 }
    │ │ │ │ +
    162 };
    │ │ │ │ +
    │ │ │ │ +
    163
    │ │ │ │ +
    165 Matrix4 AdjointMap() const;
    │ │ │ │ +
    166
    │ │ │ │ +
    167 using LieGroup<Similarity2, 4>::inverse;
    │ │ │ │ +
    168
    │ │ │ │ +
    172
    │ │ │ │ +
    174 Matrix3 matrix() const;
    │ │ │ │ +
    175
    │ │ │ │ +
    177 Rot2 rotation() const { return R_; }
    │ │ │ │ +
    178
    │ │ │ │ +
    180 Point2 translation() const { return t_; }
    │ │ │ │ +
    181
    │ │ │ │ +
    183 double scale() const { return s_; }
    │ │ │ │ +
    184
    │ │ │ │ +
    186 inline static size_t Dim() { return 4; }
    │ │ │ │ +
    187
    │ │ │ │ +
    189 inline size_t dim() const { return 4; }
    │ │ │ │ +
    190
    │ │ │ │ +
    192};
    │ │ │ │ +
    │ │ │ │ +
    193
    │ │ │ │ +
    194template <>
    │ │ │ │ +
    195struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
    │ │ │ │ +
    196
    │ │ │ │ +
    197template <>
    │ │ │ │ +
    198struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
    │ │ │ │ +
    199
    │ │ │ │ +
    200} // namespace gtsam
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    2D rotation
    │ │ │ │ +
    2D Pose
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::vector< double > expNormalize(const std::vector< double > &logProbs)
    Normalize a set of log probabilities.
    Definition DiscreteFactor.cpp:42
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │ +
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ -
    Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
    Definition DiscreteFactor.h:38
    │ │ │ │ -
    virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
    Render as html table.
    │ │ │ │ -
    Factor Base
    Our base class.
    Definition DiscreteFactor.h:45
    │ │ │ │ -
    void print(const std::string &s="DiscreteFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition DiscreteFactor.h:74
    │ │ │ │ -
    boost::shared_ptr< DiscreteFactor > shared_ptr
    shared_ptr to this class
    Definition DiscreteFactor.h:44
    │ │ │ │ -
    virtual ~DiscreteFactor()
    Virtual destructor.
    Definition DiscreteFactor.h:63
    │ │ │ │ -
    DiscreteValues::Names Names
    Translation table from values to strings.
    Definition DiscreteFactor.h:106
    │ │ │ │ -
    virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0
    equals
    │ │ │ │ -
    DiscreteFactor This
    This class.
    Definition DiscreteFactor.h:43
    │ │ │ │ -
    virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0
    Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor.
    │ │ │ │ -
    virtual double operator()(const DiscreteValues &) const =0
    Find value for given assignment of values to variables.
    │ │ │ │ -
    virtual std::string markdown(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
    Render as markdown table.
    │ │ │ │ -
    DiscreteFactor(const CONTAINER &keys)
    Construct from container of keys.
    Definition DiscreteFactor.h:60
    │ │ │ │ -
    DiscreteFactor()
    Default constructor creates empty factor.
    Definition DiscreteFactor.h:55
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    std::map< Key, std::vector< std::string > > Names
    Translation table from values to strings.
    Definition DiscreteValues.h:95
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    the error.
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ +
    2D similarity transform
    Definition Similarity2.h:35
    │ │ │ │ +
    Point2 translation() const
    Return a GTSAM translation.
    Definition Similarity2.h:180
    │ │ │ │ +
    double scale() const
    Return the scale.
    Definition Similarity2.h:183
    │ │ │ │ +
    static size_t Dim()
    Dimensionality of tangent space = 4 DOF - used to autodetect sizes.
    Definition Similarity2.h:186
    │ │ │ │ +
    Rot2 rotation() const
    Return a GTSAM rotation.
    Definition Similarity2.h:177
    │ │ │ │ +
    size_t dim() const
    Dimensionality of tangent space = 4 DOF.
    Definition Similarity2.h:189
    │ │ │ │ +
    Chart at the origin.
    Definition Similarity2.h:153
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,196 +1,212 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteFactor.h │ │ │ │ │ +Similarity2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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; │ │ │ │ │ +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_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +29// Forward declarations │ │ │ │ │ +30class Pose2; │ │ │ │ │ 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: │ │ │ │ │ +_3_5class GTSAM_EXPORT _S_i_m_i_l_a_r_i_t_y_2 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ +38 typedef _R_o_t_2 _R_o_t_a_t_i_o_n; │ │ │ │ │ +39 typedef _P_o_i_n_t_2 Translation; │ │ │ │ │ 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; │ │ │ │ │ +42 private: │ │ │ │ │ +43 _R_o_t_2 R_; │ │ │ │ │ +44 _P_o_i_n_t_2 t_; │ │ │ │ │ +45 double s_; │ │ │ │ │ 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: │ │ │ │ │ +47 public: │ │ │ │ │ 50 │ │ │ │ │ +52 _S_i_m_i_l_a_r_i_t_y_2(); │ │ │ │ │ 53 │ │ │ │ │ -_5_5 _D_i_s_c_r_e_t_e_F_a_c_t_o_r() {} │ │ │ │ │ +55 _S_i_m_i_l_a_r_i_t_y_2(double s); │ │ │ │ │ 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 } │ │ │ │ │ +58 _S_i_m_i_l_a_r_i_t_y_2(const _R_o_t_2& R, const _P_o_i_n_t_2& t, double s); │ │ │ │ │ +59 │ │ │ │ │ +61 _S_i_m_i_l_a_r_i_t_y_2(const Matrix2& R, const Vector2& t, double s); │ │ │ │ │ +62 │ │ │ │ │ +64 _S_i_m_i_l_a_r_i_t_y_2(const Matrix3& T); │ │ │ │ │ 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; │ │ │ │ │ +71 bool _e_q_u_a_l_s(const _S_i_m_i_l_a_r_i_t_y_2& sim, double tol) const; │ │ │ │ │ 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; │ │ │ │ │ -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; │ │ │ │ │ +74 bool _o_p_e_r_a_t_o_r_=_=(const _S_i_m_i_l_a_r_i_t_y_2& other) const; │ │ │ │ │ +75 │ │ │ │ │ +77 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ +78 │ │ │ │ │ +79 friend std::ostream& operator<<(std::ostream& os, const _S_i_m_i_l_a_r_i_t_y_2& p); │ │ │ │ │ +80 │ │ │ │ │ +84 │ │ │ │ │ +86 static _S_i_m_i_l_a_r_i_t_y_2 Identity(); │ │ │ │ │ +87 │ │ │ │ │ +89 _S_i_m_i_l_a_r_i_t_y_2 _o_p_e_r_a_t_o_r_*(const _S_i_m_i_l_a_r_i_t_y_2& S) const; │ │ │ │ │ +90 │ │ │ │ │ +92 _S_i_m_i_l_a_r_i_t_y_2 inverse() const; │ │ │ │ │ +93 │ │ │ │ │ +97 │ │ │ │ │ +99 _P_o_i_n_t_2 transformFrom(const _P_o_i_n_t_2& p) const; │ │ │ │ │ 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; │ │ │ │ │ -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 │ │ │ │ │ -{}; │ │ │ │ │ +112 _P_o_s_e_2 transformFrom(const _P_o_s_e_2& T) const; │ │ │ │ │ +113 │ │ │ │ │ +114 /* syntactic sugar for transformFrom */ │ │ │ │ │ +115 _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& p) const; │ │ │ │ │ +116 │ │ │ │ │ +120 static _S_i_m_i_l_a_r_i_t_y_2 Align(const Point2Pairs& abPointPairs); │ │ │ │ │ +121 │ │ │ │ │ +135 static _S_i_m_i_l_a_r_i_t_y_2 Align(const Pose2Pairs& abPosePairs); │ │ │ │ │ 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. │ │ │ │ │ +140 │ │ │ │ │ +145 static Vector4 Logmap(const _S_i_m_i_l_a_r_i_t_y_2& S, // │ │ │ │ │ +146 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Hm = boost::none); │ │ │ │ │ +147 │ │ │ │ │ +149 static _S_i_m_i_l_a_r_i_t_y_2 Expmap(const Vector4& v, // │ │ │ │ │ +150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> 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_2 Retract(const Vector4& v, │ │ │ │ │ +155 ChartJacobian H = boost::none) { │ │ │ │ │ +156 return Similarity2::Expmap(v, H); │ │ │ │ │ +157 } │ │ │ │ │ +158 static Vector4 Local(const _S_i_m_i_l_a_r_i_t_y_2& other, │ │ │ │ │ +159 ChartJacobian H = boost::none) { │ │ │ │ │ +160 return Similarity2::Logmap(other, H); │ │ │ │ │ +161 } │ │ │ │ │ +162 }; │ │ │ │ │ +163 │ │ │ │ │ +165 Matrix4 AdjointMap() const; │ │ │ │ │ +166 │ │ │ │ │ +167 using _L_i_e_G_r_o_u_p<_S_i_m_i_l_a_r_i_t_y_2, 4>::inverse; │ │ │ │ │ +168 │ │ │ │ │ +172 │ │ │ │ │ +174 Matrix3 matrix() const; │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 _R_o_t_2 _r_o_t_a_t_i_o_n() const { return R_; } │ │ │ │ │ +178 │ │ │ │ │ +_1_8_0 _P_o_i_n_t_2 _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ +181 │ │ │ │ │ +_1_8_3 double _s_c_a_l_e() const { return s_; } │ │ │ │ │ +184 │ │ │ │ │ +_1_8_6 inline static size_t _D_i_m() { return 4; } │ │ │ │ │ +187 │ │ │ │ │ +_1_8_9 inline size_t _d_i_m() const { return 4; } │ │ │ │ │ +190 │ │ │ │ │ +192}; │ │ │ │ │ +193 │ │ │ │ │ +194template <> │ │ │ │ │ +_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. │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_P_o_s_e_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ +_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_:_:_S_i_m_i_l_a_r_i_t_y_2 │ │ │ │ │ +2D similarity transform │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +Point2 translation() const │ │ │ │ │ +Return a GTSAM translation. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_s_c_a_l_e │ │ │ │ │ +double scale() const │ │ │ │ │ +Return the scale. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:183 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +Dimensionality of tangent space = 4 DOF - used to autodetect sizes. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +Rot2 rotation() const │ │ │ │ │ +Return a GTSAM rotation. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality of tangent space = 4 DOF. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +Chart at the origin. │ │ │ │ │ +DDeeffiinniittiioonn Similarity2.h:153 │ │ │ │ │ * _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 │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_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/a00269_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00218_source.html │ │ │ │┄ Files 0% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/base/Vector.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/base/Vector.h>
    │ │ │ │
    26
    │ │ │ │
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │
    │ │ │ │ │ │ │ │
    34
    │ │ │ │
    35 protected:
    │ │ │ │ @@ -167,17 +167,17 @@ │ │ │ │
    75 }
    │ │ │ │
    │ │ │ │
    76
    │ │ │ │
    77 };
    │ │ │ │
    │ │ │ │
    78
    │ │ │ │
    79} /* namespace gtsam */
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
    │ │ │ │ - │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ + │ │ │ │ +
    Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
    │ │ │ │
    std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
    Main elimination function for DiscreteFactorGraph.
    Definition DiscreteFactorGraph.cpp:200
    │ │ │ │
    std::pair< Key, size_t > DiscreteKey
    Key type for discrete variables.
    Definition DiscreteKey.h:36
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    boost::shared_ptr< DiscreteFactor > shared_ptr
    shared_ptr to this class
    Definition DiscreteFactor.h:44
    │ │ │ │
    A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
    Definition DiscreteFactorGraph.h:101
    │ │ │ │
    A class for computing marginals of variables in a DiscreteFactorGraph.
    Definition DiscreteMarginals.h:33
    │ │ │ │ @@ -186,13 +186,13 @@ │ │ │ │
    DiscreteFactor::shared_ptr operator()(Key variable) const
    Compute the marginal of a single variable.
    Definition DiscreteMarginals.h:51
    │ │ │ │
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -63,17 +63,17 @@ │ │ │ │ │ 75 } │ │ │ │ │ 76 │ │ │ │ │ 77 }; │ │ │ │ │ 78 │ │ │ │ │ 79} /* namespace gtsam */ │ │ │ │ │ _V_e_c_t_o_r_._h │ │ │ │ │ typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_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_T_r_e_e_._h │ │ │ │ │ Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ _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_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00275_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00362_source.html │ │ │ │┄ Files 1% 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
    │ │ │ │ - │ │ │ │ + │ │ │ │
    11#include <gtsam/geometry/Unit3.h>
    │ │ │ │ - │ │ │ │ -
    13#include <gtsam/base/Manifold.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:
    │ │ │ │ - │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │
    Class retract(const TangentVector &v) const
    retract as required by manifold concept: applies v at *this
    Definition Lie.h:131
    │ │ │ │
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -161,20 +161,20 @@ │ │ │ │ │ 209 │ │ │ │ │ 210template<> │ │ │ │ │ _2_1_1struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ _M_a_n_i_f_o_l_d {}; │ │ │ │ │ 212 │ │ │ │ │ 213} // namespace gtsam │ │ │ │ │ 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 │ │ │ │ │ +_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_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00281_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00452_source.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/Similarity3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SOn-inl.h
    │ │ │ │ +
    Similarity3.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
    │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/base/Lie.h>
    │ │ │ │ +
    22#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    23#include <gtsam/dllexport.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    30// Forward declarations
    │ │ │ │ +
    31class Pose3;
    │ │ │ │
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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}
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    36class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
    │ │ │ │ +
    39 typedef Rot3 Rotation;
    │ │ │ │ +
    40 typedef Point3 Translation;
    │ │ │ │ +
    42
    │ │ │ │ +
    43 private:
    │ │ │ │ +
    44 Rot3 R_;
    │ │ │ │ +
    45 Point3 t_;
    │ │ │ │ +
    46 double s_;
    │ │ │ │ +
    47
    │ │ │ │ +
    48 public:
    │ │ │ │ +
    51
    │ │ │ │ + │ │ │ │ +
    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);
    │ │ │ │ +
    63
    │ │ │ │ +
    65 Similarity3(const Matrix4& T);
    │ │ │ │ +
    66
    │ │ │ │
    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}
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    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;
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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_; }
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    3D Point
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │ +
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │ +
    Matrix wedge(const Vector &x)
    Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ -
    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
    │ │ │ │ -
    static Matrix VectorizedGenerators()
    Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
    Definition SOn.h:300
    │ │ │ │ -
    SO inverse() const
    inverse of a rotation = transpose
    Definition SOn.h:193
    │ │ │ │ -
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ -
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ -
    MatrixNN matrix_
    Rotation matrix.
    Definition SOn.h:62
    │ │ │ │ -
    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
    │ │ │ │ -
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ -
    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
    │ │ │ │ -
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ -
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ -
    SO()
    Construct SO<N> identity for N >= 2.
    Definition SOn.h:79
    │ │ │ │ -
    static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
    Inverse of Retract.
    Definition SOn-inl.h:50
    │ │ │ │ -
    static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
    Retract uses Cayley map.
    Definition SOn-inl.h:40
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    3D similarity transform
    Definition Similarity3.h:36
    │ │ │ │ +
    static size_t Dim()
    Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
    Definition Similarity3.h:194
    │ │ │ │ +
    Point3 translation() const
    Return a GTSAM translation.
    Definition Similarity3.h:188
    │ │ │ │ +
    Rot3 rotation() const
    Return a GTSAM rotation.
    Definition Similarity3.h:185
    │ │ │ │ +
    size_t dim() const
    Dimensionality of tangent space = 7 DOF.
    Definition Similarity3.h:197
    │ │ │ │ +
    static Matrix4 wedge(const Vector7 &xi)
    wedge for Similarity3:
    Definition Similarity3.cpp:198
    │ │ │ │ +
    double scale() const
    Return the scale.
    Definition Similarity3.h:191
    │ │ │ │ +
    Chart at the origin.
    Definition Similarity3.h:153
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,194 +1,233 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SOn-inl.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 │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -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} │ │ │ │ │ +_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 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); │ │ │ │ │ +63 │ │ │ │ │ +65 _S_i_m_i_l_a_r_i_t_y_3(const Matrix4& T); │ │ │ │ │ +66 │ │ │ │ │ 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} │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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; │ │ │ │ │ +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_; } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ +_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. │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D 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_:_:_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_:_:_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 │ │ │ │ │ -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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_-_i_n_l_._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/a00287.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00332.html │ │ │ │┄ Files 8% 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/geometry/Rot3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    SO4.h File Reference
    │ │ │ │ +
    Rot3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    struct  gtsam::traits< SO4 >
    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::traits< const SO4 >
    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::SO4 = SO< 4 >
     
    │ │ │ │ +using gtsam::Rot3Vector = std::vector< Rot3, Eigen::aligned_allocator< Rot3 > >
     std::vector of Rot3s, mainly for wrapper
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

    │ │ │ │ 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.
     
    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

    │ │ │ │ -

    4*4 matrix representation of SO(4)

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

    3D rotation represented as a rotation matrix or quaternion

    │ │ │ │ +
    Author
    Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    Date
    March 2019
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,56 @@ │ │ │ │ │ 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 | _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 │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_4_ _> │ │ │ │ │ + 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_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_4_ _> │ │ │ │ │ +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::::SSOO44 = _S_O< 4 > │ │ │ │ │ +using  ggttssaamm::::RRoott33VVeeccttoorr = std::vector< _R_o_t_3, Eigen::aligned_allocator< _R_o_t_3 > │ │ │ │ │ + > │ │ │ │ │ +  std::vector of Rot3s, mainly for wrapper │ │ │ │ │   │ │ │ │ │ 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. │ │ │ │ │ +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 ********** │ │ │ │ │ -4*4 matrix representation of SO(4) │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ Author │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Luca Carlone │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._h │ │ │ │ │ + * _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/a00287_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00770_source.html │ │ │ │┄ Files 6% 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/linear/iterative.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SO4.h
    │ │ │ │ +
    iterative.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>
    │ │ │ │ -
    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);
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/base/Matrix.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
    │ │ │ │ -
    46template <>
    │ │ │ │ -
    47GTSAM_EXPORT
    │ │ │ │ -
    48Vector6 SO4::Vee(const Matrix4 &X);
    │ │ │ │ +
    46 private:
    │ │ │ │ +
    47 const Matrix& A_;
    │ │ │ │ +
    48 const Vector& b_;
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    79GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
    │ │ │ │ -
    80
    │ │ │ │ -
    82template <class Archive>
    │ │ │ │ +
    │ │ │ │ +
    63 Vector operator^(const Vector& e) const {
    │ │ │ │ +
    64 return A_ ^ e;
    │ │ │ │ +
    65 }
    │ │ │ │ +
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    76
    │ │ │ │ +
    │ │ │ │ +
    78 Vector operator*(const Vector& x) const {
    │ │ │ │ +
    79 return A() * x;
    │ │ │ │ +
    80 }
    │ │ │ │ +
    │ │ │ │ +
    81
    │ │ │ │
    │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Concept check class for variable types with Group properties.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    N*N matrix representation of SO(N).
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    141 const GaussianFactorGraph& fg,
    │ │ │ │ +
    142 const VectorValues& x,
    │ │ │ │ +
    143 const ConjugateGradientParameters & parameters);
    │ │ │ │ +
    144
    │ │ │ │ +
    145
    │ │ │ │ +
    146} // namespace gtsam
    │ │ │ │ +
    147
    │ │ │ │ + │ │ │ │ +
    149
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    Iterative methods, template implementation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │ -
    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_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
    Project to top-left 3*3 matrix.
    Definition SO4.cpp:206
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ -
    Manifold of special orthogonal rotation matrices SO<N>.
    Definition SOn.h:52
    │ │ │ │ -
    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
    │ │ │ │ -
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ -
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ -
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
    Method of conjugate gradients (CG), System version.
    Definition iterative.cpp:45
    │ │ │ │ +
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrice...
    Definition iterative.h:44
    │ │ │ │ +
    Vector operator*(const Vector &x) const
    Apply operator A.
    Definition iterative.h:78
    │ │ │ │ +
    const Matrix & A() const
    Access A matrix.
    Definition iterative.h:57
    │ │ │ │ +
    void multiplyInPlace(const Vector &x, Vector &e) const
    Apply operator A in place.
    Definition iterative.h:83
    │ │ │ │ +
    const Vector & b() const
    Access b vector.
    Definition iterative.h:60
    │ │ │ │ +
    Vector gradient(const Vector &x) const
    gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_)
    Definition iterative.h:73
    │ │ │ │ +
    void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const
    x += alpha* A'*e
    Definition iterative.h:88
    │ │ │ │ +
    Vector operator^(const Vector &e) const
    Apply operator A'*e.
    Definition iterative.h:63
    │ │ │ │ +
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,178 +1,183 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SO4.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-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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -46template <> │ │ │ │ │ -47GTSAM_EXPORT │ │ │ │ │ -48Vector6 _S_O_4_:_:_V_e_e(const Matrix4 &X); │ │ │ │ │ +46 private: │ │ │ │ │ +47 const Matrix& A_; │ │ │ │ │ +48 const Vector& b_; │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +_6_3 Vector _o_p_e_r_a_t_o_r_^(const Vector& e) const { │ │ │ │ │ +64 return A_ ^ e; │ │ │ │ │ +65 } │ │ │ │ │ +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()); │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ +_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. │ │ │ │ │ +_i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ +Iterative methods, template implementation. │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a00305_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00170_source.html │ │ │ │┄ Files 6% 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/basis/Chebyshev2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3Unified.h
    │ │ │ │ +
    Chebyshev2.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/geometry/Cal3DS2_Base.h>
    │ │ │ │ -
    27
    │ │ │ │ -
    28namespace gtsam {
    │ │ │ │ -
    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;
    │ │ │ │ -
    90
    │ │ │ │ -
    94
    │ │ │ │ -
    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
    │ │ │ │ -
    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}
    │ │ │ │ +
    33#pragma once
    │ │ │ │ +
    34
    │ │ │ │ +
    35#include <gtsam/base/Manifold.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
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │ +
    Compute an interpolating basis.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │ -
    Calibration of a omni-directional camera with mirror + lens radial distortion.
    Definition Cal3Unified.h:45
    │ │ │ │ -
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3Unified.h:133
    │ │ │ │ -
    Cal3Unified()=default
    Default Constructor with only unit focal length.
    │ │ │ │ -
    double xi() const
    mirror parameter
    Definition Cal3Unified.h:96
    │ │ │ │ -
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3Unified.h:136
    │ │ │ │ +
    CRTP Base class for function bases.
    Definition Basis.h:100
    │ │ │ │ +
    Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points...
    Definition Chebyshev2.h:46
    │ │ │ │ +
    static double Point(size_t N, int j)
    Specific Chebyshev point.
    Definition Chebyshev2.h:55
    │ │ │ │ +
    static double Point(size_t N, int j, double a, double b)
    Specific Chebyshev point, within [a,b] interval.
    Definition Chebyshev2.h:64
    │ │ │ │ +
    static Vector Points(size_t N)
    All Chebyshev points.
    Definition Chebyshev2.h:72
    │ │ │ │ +
    static Vector Points(size_t N, double a, double b)
    All Chebyshev points, within [a,b] interval.
    Definition Chebyshev2.h:79
    │ │ │ │ +
    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,151 +1,128 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Unified.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 │ │ │ │ │ -24#pragma once │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -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; │ │ │ │ │ -90 │ │ │ │ │ -94 │ │ │ │ │ -_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 │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ +145} // namespace gtsam │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ + * _b_a_s_i_s │ │ │ │ │ + * _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/a00308.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01394.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/global_includes.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Rot3Q.cpp File Reference
    │ │ │ │ +
    global_includes.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    Included from all GTSAM files. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Rotation (internal: quaternion representation*)

    │ │ │ │ +

    Included from all GTSAM files.

    │ │ │ │
    Author
    Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,15 +1,16 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Rot3Q.cpp File Reference │ │ │ │ │ -Rotation (internal: quaternion representation*) _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 ********** │ │ │ │ │ -Rotation (internal: quaternion representation*) │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_Q_._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/a00311.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00464.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    SOn.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

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

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -158,13 +158,13 @@ │ │ │ │

    N can be Eigen::Dynamic

    Author
    Frank Dellaert
    │ │ │ │
    Date
    March 2019
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00314_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01151_source.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/nonlinear/ISAM2Clique.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3Bundler.h
    │ │ │ │ +
    ISAM2Clique.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/geometry/Cal3.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
    │ │ │ │ + │ │ │ │ +
    23#include <gtsam/inference/Key.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    27#include <string>
    │ │ │ │ +
    28
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    │ │ │ │ +
    36class GTSAM_EXPORT ISAM2Clique
    │ │ │ │ +
    37 : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
    │ │ │ │ +
    38 public:
    │ │ │ │ +
    39 typedef ISAM2Clique This;
    │ │ │ │ + │ │ │ │ +
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    42 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    59 : Base(other),
    │ │ │ │ +
    60 cachedFactor_(other.cachedFactor_),
    │ │ │ │ +
    61 gradientContribution_(other.gradientContribution_) {}
    │ │ │ │ +
    │ │ │ │ +
    62
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    66 Base::operator=(other);
    │ │ │ │ +
    67 cachedFactor_ = other.cachedFactor_;
    │ │ │ │ +
    68 gradientContribution_ = other.gradientContribution_;
    │ │ │ │ +
    69 return *this;
    │ │ │ │ +
    70 }
    │ │ │ │ +
    │ │ │ │
    71
    │ │ │ │ -
    73 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
    │ │ │ │ -
    74 const Cal3Bundler& cal);
    │ │ │ │ +
    73 void setEliminationResult(
    │ │ │ │ +
    74 const FactorGraphType::EliminationResult& eliminationResult);
    │ │ │ │
    75
    │ │ │ │ -
    77 void print(const std::string& s = "") const override;
    │ │ │ │ +
    77 Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
    │ │ │ │
    78
    │ │ │ │ -
    80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
    │ │ │ │ +
    80 const Vector& gradientContribution() const { return gradientContribution_; }
    │ │ │ │
    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_; }
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    137 Matrix23 D2d_calibration(const Point2& p) const;
    │ │ │ │ -
    138
    │ │ │ │ -
    140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
    │ │ │ │ -
    141
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    179};
    │ │ │ │ -
    │ │ │ │ -
    180
    │ │ │ │ -
    181template <>
    │ │ │ │ -
    182struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
    │ │ │ │ -
    183
    │ │ │ │ -
    184template <>
    │ │ │ │ -
    185struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
    │ │ │ │ -
    186
    │ │ │ │ -
    187} // namespace gtsam
    │ │ │ │ -
    Common code for all Calibration models.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    178} // namespace gtsam
    │ │ │ │ + │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ -
    Calibration used by Bundler.
    Definition Cal3Bundler.h:32
    │ │ │ │ -
    double k1() const
    distorsion parameter k1
    Definition Cal3Bundler.h:87
    │ │ │ │ -
    double k2() const
    distorsion parameter k2
    Definition Cal3Bundler.h:90
    │ │ │ │ -
    Cal3Bundler()=default
    Default constructor.
    │ │ │ │ -
    Cal3Bundler retract(const Vector &d) const
    Update calibration with tangent space delta.
    Definition Cal3Bundler.h:153
    │ │ │ │ -
    double py() const
    image center in y
    Definition Cal3Bundler.h:96
    │ │ │ │ -
    Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
    Constructor.
    Definition Cal3Bundler.h:62
    │ │ │ │ -
    Vector3 localCoordinates(const Cal3Bundler &T2) const
    Calculate local coordinates to another calibration.
    Definition Cal3Bundler.h:158
    │ │ │ │ -
    double px() const
    image center in x
    Definition Cal3Bundler.h:93
    │ │ │ │ -
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3Bundler.h:147
    │ │ │ │ -
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3Bundler.h:150
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ + │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
    Definition ISAM2Clique.h:37
    │ │ │ │ +
    const Vector & gradientContribution() const
    Access the gradient contribution.
    Definition ISAM2Clique.h:80
    │ │ │ │ +
    ISAM2Clique(const ISAM2Clique &other)
    Copy constructor, does not copy solution pointers as these are invalid in different trees.
    Definition ISAM2Clique.h:58
    │ │ │ │ +
    ISAM2Clique & operator=(const ISAM2Clique &other)
    Assignment operator, does not copy solution pointers as these are invalid in different trees.
    Definition ISAM2Clique.h:65
    │ │ │ │ +
    Base::FactorType::shared_ptr & cachedFactor()
    Access the cached factor.
    Definition ISAM2Clique.h:77
    │ │ │ │ +
    ISAM2Clique()
    Default constructor.
    Definition ISAM2Clique.h:53
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,196 +1,205 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Bundler.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 │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ 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 │ │ │ │ │ +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 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -74 const Cal3Bundler& cal); │ │ │ │ │ +73 void setEliminationResult( │ │ │ │ │ +74 const FactorGraphType::EliminationResult& eliminationResult); │ │ │ │ │ 75 │ │ │ │ │ -77 void print(const std::string& s = "") const override; │ │ │ │ │ +_7_7 Base::FactorType::shared_ptr& _c_a_c_h_e_d_F_a_c_t_o_r() { return cachedFactor_; } │ │ │ │ │ 78 │ │ │ │ │ -80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const; │ │ │ │ │ +_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 │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -137 Matrix23 D2d_calibration(const Point2& p) const; │ │ │ │ │ -138 │ │ │ │ │ -140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const; │ │ │ │ │ -141 │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -_C_a_l_3_._h │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +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_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_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_:_:_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_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 │ │ │ │ │ +_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. │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _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/a00329_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00413_source.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    SphericalCamera.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>
    │ │ │ │ - │ │ │ │ +
    21#include <gtsam/base/Manifold.h>
    │ │ │ │ + │ │ │ │
    23#include <gtsam/base/concepts.h>
    │ │ │ │
    24#include <gtsam/dllexport.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │
    27#include <gtsam/geometry/Unit3.h>
    │ │ │ │
    28
    │ │ │ │
    29#include <boost/serialization/nvp.hpp>
    │ │ │ │
    30
    │ │ │ │
    31namespace gtsam {
    │ │ │ │
    32
    │ │ │ │
    │ │ │ │ @@ -273,31 +273,31 @@ │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ +
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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::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
    │ │ │ │
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ @@ -330,13 +330,13 @@ │ │ │ │
    SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
    Constructor with empty intrinsics (needed for smart factors)
    Definition SphericalCamera.h:97
    │ │ │ │
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -170,24 +170,24 @@ │ │ │ │ │ 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 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ Bearing-Range product. │ │ │ │ │ +_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 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00335_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00350_source.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    PinholeCamera.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │
    32template<typename Calibration>
    │ │ │ │
    │ │ │ │
    33class PinholeCamera: public PinholeBaseK<Calibration> {
    │ │ │ │
    34
    │ │ │ │ @@ -398,15 +398,15 @@ │ │ │ │
    333 ar
    │ │ │ │
    334 & boost::serialization::make_nvp("PinholeBaseK",
    │ │ │ │
    335 boost::serialization::base_object<Base>(*this));
    │ │ │ │
    336 ar & BOOST_SERIALIZATION_NVP(K_);
    │ │ │ │
    337 }
    │ │ │ │
    338
    │ │ │ │
    339public:
    │ │ │ │ - │ │ │ │ + │ │ │ │
    341};
    │ │ │ │
    │ │ │ │
    342
    │ │ │ │
    343// manifold traits
    │ │ │ │
    344
    │ │ │ │
    345template <typename Calibration>
    │ │ │ │
    │ │ │ │ @@ -421,17 +421,17 @@ │ │ │ │
    │ │ │ │
    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
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ -
    Pinhole camera with known calibration.
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Bearing-Range product.
    │ │ │ │ +
    Pinhole camera with known calibration.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ @@ -485,13 +485,13 @@ │ │ │ │
    Pose3 inverse() const
    inverse transformation with derivatives
    Definition Pose3.cpp:49
    │ │ │ │
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00353.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/slam/StereoFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3.h File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    StereoFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

    A non-linear factor for stereo measurements. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    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
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

    │ │ │ │ Classes

    class  gtsam::Cal3
     Common base class for all calibration models. More...
    class  gtsam::GenericStereoFactor< POSE, LANDMARK >
     A Generic Stereo Factor. More...
     
    struct  gtsam::traits< GenericStereoFactor< T1, T2 > >
     traits 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

    │ │ │ │ -

    Common code for all Calibration models.

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

    A non-linear factor for stereo measurements.

    │ │ │ │ +
    Author
    Alireza Fathi
    │ │ │ │ +
    │ │ │ │ +Chris Beall
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3.h 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 │ │ │ │ │ +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_:_:_C_a_l_3 │ │ │ │ │ -  Common base class for all calibration models. _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_<_ _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. │ │ │ │ │   │ │ │ │ │ -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 ********** │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ +A non-linear factor for stereo measurements. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _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/a00362.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoPoint2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    StereoPoint2.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -Functions

    │ │ │ │ -ostream & gtsam::operator<< (ostream &os, const StereoPoint2 &p)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Jan 26, 2010
    │ │ │ │ -
    Author
    dellaert
    │ │ │ │ +

    Factor Graph consisting of non-linear factors.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Carlos Nieto
    │ │ │ │ +
    │ │ │ │ +Christian Potthast
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -StereoPoint2.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. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_t_e_r_e_o_P_o_i_n_t_2 &p) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 26, 2010 │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ Author │ │ │ │ │ - dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _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_._c_p_p │ │ │ │ │ + * _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/a00377_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00365_source.html │ │ │ │┄ Files 6% 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/Rot2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    StereoCamera.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ -
    19
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ +
    23#include <gtsam/base/Lie.h>
    │ │ │ │ +
    24#include <boost/optional.hpp>
    │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ -
    26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
    │ │ │ │ -
    27public:
    │ │ │ │ - │ │ │ │ -
    29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
    │ │ │ │ -
    30
    │ │ │ │ - │ │ │ │ -
    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 {
    │ │ │ │ +
    26#include <random>
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    49public:
    │ │ │ │ -
    50
    │ │ │ │ - │ │ │ │ -
    56 typedef StereoPoint2Vector MeasurementVector;
    │ │ │ │ +
    51
    │ │ │ │ +
    53 Rot2() : c_(1.0), s_(0.0) {}
    │ │ │ │ +
    54
    │ │ │ │ +
    56 Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
    │ │ │ │
    57
    │ │ │ │ -
    58private:
    │ │ │ │ -
    59 Pose3 leftCamPose_;
    │ │ │ │ -
    60 Cal3_S2Stereo::shared_ptr K_;
    │ │ │ │ -
    61
    │ │ │ │ -
    62public:
    │ │ │ │ -
    63
    │ │ │ │ -
    64 enum {
    │ │ │ │ -
    65 dimension = 6
    │ │ │ │ -
    66 };
    │ │ │ │ -
    67
    │ │ │ │ -
    70
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    94 static Rot2 Random(std::mt19937 & rng);
    │ │ │ │ +
    95
    │ │ │ │
    99
    │ │ │ │ -
    103
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ +
    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 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    171 boost::none) const;
    │ │ │ │ -
    172
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    191template<>
    │ │ │ │ -
    │ │ │ │ -
    192struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
    │ │ │ │ -
    193};
    │ │ │ │ -
    │ │ │ │ -
    194
    │ │ │ │ -
    195template<>
    │ │ │ │ -
    │ │ │ │ -
    196struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
    │ │ │ │ -
    197};
    │ │ │ │ -
    │ │ │ │ -
    198}
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration + Stereo baseline.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    A 2D stereo point (uL,uR,v)
    │ │ │ │ +
    │ │ │ │ +
    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> {};
    │ │ │ │ +
    232
    │ │ │ │ +
    233} // gtsam
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration, stereo version.
    Definition Cal3_S2Stereo.h:30
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    bool equals(const Pose3 &pose, double tol=1e-9) const
    assert equality up to a tolerance
    Definition Pose3.cpp:157
    │ │ │ │ -
    void print(const std::string &s="") const
    print with optional string
    Definition Pose3.cpp:152
    │ │ │ │ -
    Definition StereoCamera.h:26
    │ │ │ │ -
    A stereo camera class, parameterize by left camera pose and stereo calibration.
    Definition StereoCamera.h:47
    │ │ │ │ -
    StereoCamera()
    Default constructor allocates a calibration!
    Definition StereoCamera.h:72
    │ │ │ │ -
    static size_t Dim()
    Dimensionality of the tangent space.
    Definition StereoCamera.h:110
    │ │ │ │ -
    void print(const std::string &s="") const
    print
    Definition StereoCamera.h:89
    │ │ │ │ -
    Vector6 localCoordinates(const StereoCamera &t2) const
    Local coordinates of manifold neighborhood around current value.
    Definition StereoCamera.h:120
    │ │ │ │ -
    bool equals(const StereoCamera &camera, double tol=1e-9) const
    equals
    Definition StereoCamera.h:95
    │ │ │ │ -
    size_t dim() const
    Dimensionality of the tangent space.
    Definition StereoCamera.h:105
    │ │ │ │ -
    StereoCamera retract(const Vector &v) const
    Updates a with tangent space delta.
    Definition StereoCamera.h:115
    │ │ │ │ -
    StereoPoint2 Measurement
    Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
    Definition StereoCamera.h:55
    │ │ │ │ -
    const Cal3_S2Stereo & calibration() const
    Return shared pointer to calibration.
    Definition StereoCamera.h:80
    │ │ │ │ -
    double baseline() const
    baseline
    Definition StereoCamera.h:134
    │ │ │ │ -
    const Pose3 & pose() const
    pose
    Definition StereoCamera.h:129
    │ │ │ │ -
    Vector defaultErrorWhenTriangulatingBehindCamera() const
    for Nonlinear Triangulation
    Definition StereoCamera.h:174
    │ │ │ │ -
    A 2D stereo point, v will be same for rectified images.
    Definition StereoPoint2.h:32
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ +
    Rot2 operator*(const Rot2 &R) const
    Compose - make a new rotation by adding angles.
    Definition Rot2.h:117
    │ │ │ │ +
    double c() const
    return cos
    Definition Rot2.h:198
    │ │ │ │ +
    static Matrix ExpmapDerivative(const Vector &)
    Left-trivialized derivative of the exponential map.
    Definition Rot2.h:135
    │ │ │ │ +
    Point2 unit() const
    Creates a unit vector as a Point2.
    Definition Rot2.h:182
    │ │ │ │ +
    double theta() const
    return angle (RADIANS)
    Definition Rot2.h:187
    │ │ │ │ +
    Rot2 inverse() const
    The inverse rotation - negative angle.
    Definition Rot2.h:114
    │ │ │ │ +
    Point2 operator*(const Point2 &p) const
    syntactic sugar for rotate
    Definition Rot2.h:167
    │ │ │ │ +
    double s() const
    return sin
    Definition Rot2.h:203
    │ │ │ │ +
    static Rot2 Identity()
    Identity.
    Definition Rot2.h:111
    │ │ │ │ +
    double degrees() const
    return angle (DEGREES)
    Definition Rot2.h:192
    │ │ │ │ +
    static Matrix LogmapDerivative(const Vector &)
    Left-trivialized derivative inverse of the exponential map.
    Definition Rot2.h:140
    │ │ │ │ +
    Matrix1 AdjointMap() const
    Calculate Adjoint map.
    Definition Rot2.h:132
    │ │ │ │ +
    Rot2(double theta)
    Constructor from angle in radians == exponential map at identity.
    Definition Rot2.h:59
    │ │ │ │ +
    static Rot2 fromDegrees(double theta)
    Named constructor from angle in degrees.
    Definition Rot2.h:67
    │ │ │ │ +
    Rot2()
    default constructor, zero rotation
    Definition Rot2.h:53
    │ │ │ │ +
    Rot2(const Rot2 &r)
    copy constructor
    Definition Rot2.h:56
    │ │ │ │ +
    static Rot2 fromAngle(double theta)
    Named constructor from angle in radians.
    Definition Rot2.h:62
    │ │ │ │ +
    Definition Rot2.h:145
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,265 +1,277 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -StereoCamera.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 │ │ │ │ │ -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 { │ │ │ │ │ +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 │ │ │ │ │ -_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) {} │ │ │ │ │ -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 { │ │ │ │ │ +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 { │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -_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 } │ │ │ │ │ +94 static _R_o_t_2 Random(std::mt19937 & rng); │ │ │ │ │ +95 │ │ │ │ │ 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 } │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -_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}; │ │ │ │ │ +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 │ │ │ │ │ -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_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 {}; │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ + * _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/a00380_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00332_source.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    Rot3.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,16 +117,16 @@ │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    22// \callgraph
    │ │ │ │
    23
    │ │ │ │
    24#pragma once
    │ │ │ │
    25
    │ │ │ │
    26#include <gtsam/geometry/Unit3.h>
    │ │ │ │ - │ │ │ │ -
    28#include <gtsam/geometry/SO3.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
    │ │ │ │ @@ -458,15 +458,15 @@ │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ + │ │ │ │
    569#endif
    │ │ │ │
    570 };
    │ │ │ │
    │ │ │ │
    571
    │ │ │ │
    573 using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
    │ │ │ │
    574
    │ │ │ │
    585 GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
    │ │ │ │ @@ -476,17 +476,17 @@ │ │ │ │
    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
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    Lie Group wrapper for Eigen Quaternions.
    │ │ │ │ -
    3*3 matrix representation of SO(3)
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    3*3 matrix representation of SO(3)
    │ │ │ │ +
    Lie Group wrapper for Eigen Quaternions.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
    std::vector of Rot3s, mainly for wrapper
    Definition Rot3.h:573
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │
    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
    │ │ │ │ @@ -528,13 +528,13 @@ │ │ │ │
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Vector.
    Definition Unit3.cpp:151
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -354,18 +354,18 @@ │ │ │ │ │ 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) │ │ │ │ │ +_Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ +Lie Group wrapper for Eigen Quaternions. │ │ │ │ │ _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... │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00383_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00782_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    PinholeSet.h
    │ │ │ │ +
    GaussianBayesTree.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22#include <boost/optional.hpp>
    │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    29template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ -
    30class PinholeSet: public CameraSet<CAMERA> {
    │ │ │ │ -
    31
    │ │ │ │ -
    32private:
    │ │ │ │ -
    33 typedef CameraSet<CAMERA> Base;
    │ │ │ │ -
    34 typedef PinholeSet<CAMERA> This;
    │ │ │ │ -
    35
    │ │ │ │ -
    36protected:
    │ │ │ │ -
    37
    │ │ │ │ -
    38public:
    │ │ │ │ -
    39
    │ │ │ │ -
    │ │ │ │ -
    41 virtual ~PinholeSet() {
    │ │ │ │ -
    42 }
    │ │ │ │ -
    │ │ │ │ -
    43
    │ │ │ │ -
    46
    │ │ │ │ -
    │ │ │ │ -
    48 void print(const std::string& s = "") const override {
    │ │ │ │ -
    49 Base::print(s);
    │ │ │ │ -
    50 }
    │ │ │ │ -
    │ │ │ │ -
    51
    │ │ │ │ -
    │ │ │ │ -
    53 bool equals(const PinholeSet& p, double tol = 1e-9) const {
    │ │ │ │ -
    54 return Base::equals(p, tol); // TODO all flags
    │ │ │ │ -
    55 }
    │ │ │ │ -
    │ │ │ │ -
    56
    │ │ │ │ -
    58
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    61 const typename CAMERA::MeasurementVector& measured,
    │ │ │ │ -
    62 const TriangulationParameters& params) const {
    │ │ │ │ -
    63 return gtsam::triangulateSafe(*this, measured, params);
    │ │ │ │ -
    64 }
    │ │ │ │ -
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    29 // Forward declarations
    │ │ │ │ + │ │ │ │ +
    31 class VectorValues;
    │ │ │ │ +
    32
    │ │ │ │ +
    33 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    35 class GTSAM_EXPORT GaussianBayesTreeClique :
    │ │ │ │ +
    36 public BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>
    │ │ │ │ +
    37 {
    │ │ │ │ +
    38 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    42 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    55
    │ │ │ │ +
    56 public:
    │ │ │ │ +
    57 typedef GaussianBayesTree This;
    │ │ │ │ +
    58 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    59
    │ │ │ │ + │ │ │ │ +
    62
    │ │ │ │ +
    64 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │
    65
    │ │ │ │ -
    66private:
    │ │ │ │ -
    67
    │ │ │ │ - │ │ │ │ -
    70 template<class ARCHIVE>
    │ │ │ │ -
    71 void serialize(ARCHIVE & ar, const unsigned int version) {
    │ │ │ │ -
    72 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    │ │ │ │ -
    73 }
    │ │ │ │ -
    74};
    │ │ │ │ -
    │ │ │ │ -
    75
    │ │ │ │ -
    76template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ -
    77struct traits<PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
    │ │ │ │ -
    78};
    │ │ │ │ -
    │ │ │ │ -
    79
    │ │ │ │ -
    80template<class CAMERA>
    │ │ │ │ -
    │ │ │ │ -
    81struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
    │ │ │ │ -
    82};
    │ │ │ │ -
    │ │ │ │ -
    83
    │ │ │ │ -
    84} // \ namespace gtsam
    │ │ │ │ -
    Base class to create smart factors on poses or cameras.
    │ │ │ │ -
    Functions for triangulation.
    │ │ │ │ +
    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
    │ │ │ │ +
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ +
    Base class for cliques of a BayesTree.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    PinholeSet: triangulates point and keeps an estimate of it around.
    Definition PinholeSet.h:30
    │ │ │ │ -
    TriangulationResult triangulateSafe(const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params) const
    triangulateSafe
    Definition PinholeSet.h:60
    │ │ │ │ -
    bool equals(const PinholeSet &p, double tol=1e-9) const
    equals
    Definition PinholeSet.h:53
    │ │ │ │ -
    virtual ~PinholeSet()
    Virtual destructor.
    Definition PinholeSet.h:41
    │ │ │ │ -
    void print(const std::string &s="") const override
    print
    Definition PinholeSet.h:48
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition PinholeSet.h:69
    │ │ │ │ -
    Definition triangulation.h:556
    │ │ │ │ -
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │ +
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ +
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ +
    A clique in a GaussianBayesTree.
    Definition GaussianBayesTree.h:37
    │ │ │ │ +
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ +
    GaussianBayesTree()
    Default constructor, creates an empty Bayes tree.
    Definition GaussianBayesTree.h:61
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    is the normalization constant.
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,134 +1,146 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PinholeSet.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 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ -21#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> │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -29template │ │ │ │ │ -_3_0class _P_i_n_h_o_l_e_S_e_t: public _C_a_m_e_r_a_S_e_t { │ │ │ │ │ -31 │ │ │ │ │ -32private: │ │ │ │ │ -33 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> Base; │ │ │ │ │ -34 typedef _P_i_n_h_o_l_e_S_e_t_<_C_A_M_E_R_A_> This; │ │ │ │ │ -35 │ │ │ │ │ -36protected: │ │ │ │ │ -37 │ │ │ │ │ -38public: │ │ │ │ │ -39 │ │ │ │ │ -_4_1 virtual _~_P_i_n_h_o_l_e_S_e_t() { │ │ │ │ │ -42 } │ │ │ │ │ -43 │ │ │ │ │ -46 │ │ │ │ │ -_4_8 void _p_r_i_n_t(const std::string& s = "") const override { │ │ │ │ │ -49 Base::print(s); │ │ │ │ │ -50 } │ │ │ │ │ -51 │ │ │ │ │ -_5_3 bool _e_q_u_a_l_s(const _P_i_n_h_o_l_e_S_e_t& p, double tol = 1e-9) const { │ │ │ │ │ -54 return Base::equals(p, tol); // TODO all flags │ │ │ │ │ -55 } │ │ │ │ │ -56 │ │ │ │ │ -58 │ │ │ │ │ -_6_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( │ │ │ │ │ -61 const typename CAMERA::MeasurementVector& measured, │ │ │ │ │ -62 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) const { │ │ │ │ │ -63 return _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(*this, measured, params); │ │ │ │ │ -64 } │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -66private: │ │ │ │ │ -67 │ │ │ │ │ -_6_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; │ │ │ │ │ -70 template │ │ │ │ │ -71 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -72 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -73 } │ │ │ │ │ -74}; │ │ │ │ │ -75 │ │ │ │ │ -76template │ │ │ │ │ -_7_7struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_S_e_t > : public _T_e_s_t_a_b_l_e > { │ │ │ │ │ -78}; │ │ │ │ │ -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. │ │ │ │ │ +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_._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_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_:_:_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_:_:_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_:_:_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_:_:_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_S_e_t │ │ │ │ │ -PinholeSet: triangulates point and keeps an estimate of it around. │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ -TriangulationResult triangulateSafe(const typename CAMERA::MeasurementVector │ │ │ │ │ -&measured, const TriangulationParameters ¶ms) const │ │ │ │ │ -triangulateSafe │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const PinholeSet &p, double tol=1e-9) const │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_~_P_i_n_h_o_l_e_S_e_t │ │ │ │ │ -virtual ~PinholeSet() │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PinholeSet.h:69 │ │ │ │ │ -_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_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_:_:_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_S_e_t_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a00386.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00428.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/Quaternion.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    SO4.cpp File Reference
    │ │ │ │ +Macros | │ │ │ │ +Typedefs
    │ │ │ │ +
    Quaternion.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ +Classes

    struct  gtsam::traits< QUATERNION_TYPE >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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) \).
     

    │ │ │ │ +Macros

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

    │ │ │ │ +Typedefs

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

    Detailed Description

    │ │ │ │ -

    4*4 matrix representation of SO(4)

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ +

    Lie Group wrapper for Eigen Quaternions.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -SO4.cpp 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 | _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_:_:_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. │ │ │ │ │   │ │ │ │ │ -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. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  QQUUAATTEERRNNIIOONN__TTYYPPEE   Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │   │ │ │ │ │ -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) \). │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef Eigen::Quaternion< double, Eigen::DontAlign >  ggttssaamm::::QQuuaatteerrnniioonn │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -4*4 matrix representation of SO(4) │ │ │ │ │ +Lie Group wrapper for Eigen Quaternions. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._c_p_p │ │ │ │ │ + * _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/a00392.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00605.html │ │ │ │┄ Files 6% 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/inference/EliminationTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    Cal3Unified.cpp File Reference
    │ │ │ │ +Namespaces
    │ │ │ │ +
    EliminationTree-inst.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ 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
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Varun Agrawal
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    Date
    Oct 13, 2010
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +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 │ │ │ │ │ -Cal3Unified.cpp File Reference │ │ │ │ │ +_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_. │ │ │ │ │ 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 │ │ │ │ │ Author │ │ │ │ │ - Jing Dong │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 13, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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/a00440.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │

    Detailed Description

    │ │ │ │

    3D Point

    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00404_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00473_source.html │ │ │ │┄ Files 12% similar despite different names │ │ │ │ @@ -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/Point2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3DS2.h
    │ │ │ │ +
    Point2.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
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/base/VectorSpace.h>
    │ │ │ │ +
    21#include <boost/serialization/nvp.hpp>
    │ │ │ │
    22
    │ │ │ │ -
    23#include <gtsam/geometry/Cal3DS2_Base.h>
    │ │ │ │ -
    24#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    │ │ │ │ -
    35class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
    │ │ │ │ -
    36 using Base = Cal3DS2_Base;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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);
    │ │ │ │ +
    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 <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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};
    │ │ │ │
    │ │ │ │ -
    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}
    │ │ │ │ +
    103} // \ namespace gtsam
    │ │ │ │ +
    104
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
    Definition Cal3DS2.h:35
    │ │ │ │ -
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3DS2.h:87
    │ │ │ │ -
    boost::shared_ptr< Base > clone() const override
    Definition Cal3DS2.h:97
    │ │ │ │ -
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3DS2.h:90
    │ │ │ │ -
    Cal3DS2()=default
    Default Constructor with only unit focal length.
    │ │ │ │ -
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    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
    │ │ │ │ +
    Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
    Calculate the two means of a set of Point2 pairs.
    Definition Point2.cpp:116
    │ │ │ │ +
    double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
    distance between two points
    Definition Point2.cpp:39
    │ │ │ │ +
    double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
    Distance of the point from the origin, with Jacobian.
    Definition Point2.cpp:27
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Definition BearingRange.h:40
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,126 +1,140 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3DS2.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 │ │ │ │ │ -21#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ 22 │ │ │ │ │ -23#include │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 │ │ │ │ │ +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); │ │ │ │ │ +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 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} // \ 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_:_:_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_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_:_:_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_:_:_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 │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_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/a00422_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00818_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/linear/SubgraphSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Line3.h
    │ │ │ │ +
    SubgraphSolver.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>
    │ │ │ │ - │ │ │ │ +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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ +
    25#include <map>
    │ │ │ │ +
    26#include <utility> // pair
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    30// Forward declarations
    │ │ │ │ +
    31class GaussianFactorGraph;
    │ │ │ │ +
    32class GaussianBayesNet;
    │ │ │ │ +
    33class SubgraphPreconditioner;
    │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ +
    35struct GTSAM_EXPORT SubgraphSolverParameters
    │ │ │ │ + │ │ │ │ +
    37 SubgraphBuilderParameters builderParams;
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    79
    │ │ │ │ +
    80 protected:
    │ │ │ │ +
    81 Parameters parameters_;
    │ │ │ │ +
    82 boost::shared_ptr<SubgraphPreconditioner> pc_;
    │ │ │ │ +
    83
    │ │ │ │ +
    84 public:
    │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    93 SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
    │ │ │ │ +
    94 const Ordering &ordering);
    │ │ │ │ +
    95
    │ │ │ │ + │ │ │ │ +
    103 const Parameters &parameters, const Ordering &ordering);
    │ │ │ │ + │ │ │ │ +
    109 const Parameters &parameters);
    │ │ │ │ +
    110
    │ │ │ │ +
    112 ~SubgraphSolver() override {}
    │ │ │ │ +
    113
    │ │ │ │ +
    117
    │ │ │ │ +
    119 VectorValues optimize() const;
    │ │ │ │ +
    120
    │ │ │ │ + │ │ │ │ +
    123 const KeyInfo &keyInfo,
    │ │ │ │ +
    124 const std::map<Key, Vector> &lambda,
    │ │ │ │ +
    125 const VectorValues &initial) override;
    │ │ │ │
    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,
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    152};
    │ │ │ │ -
    │ │ │ │ -
    153
    │ │ │ │ -
    154template<>
    │ │ │ │ -
    155struct traits<Line3> : public internal::Manifold<Line3> {};
    │ │ │ │ -
    156
    │ │ │ │ -
    157template<>
    │ │ │ │ -
    158struct traits<const Line3> : public internal::Manifold<Line3> {};
    │ │ │ │ -
    159}
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ +
    130
    │ │ │ │ +
    132 std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
    │ │ │ │ +
    133 const GaussianFactorGraph &gfg);
    │ │ │ │ +
    134
    │ │ │ │ +
    136};
    │ │ │ │ +
    │ │ │ │ +
    137
    │ │ │ │ +
    138} // namespace gtsam
    │ │ │ │ +
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
    Definition Line3.h:44
    │ │ │ │ -
    Rot3 R() const
    Return the rotation of the line.
    Definition Line3.h:123
    │ │ │ │ -
    Line3()
    Default constructor is the Z axis.
    Definition Line3.h:53
    │ │ │ │ -
    Line3(const Rot3 &R, const double a, const double b)
    Constructor for general line from (R, a, b)
    Definition Line3.h:57
    │ │ │ │ -
    double b() const
    Return the y-coordinate of the intersection of the line with the xy plane.
    Definition Line3.h:137
    │ │ │ │ -
    double a() const
    Return the x-coordinate of the intersection of the line with the xy plane.
    Definition Line3.h:130
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ +
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Base class for Iterative Solvers like SubgraphSolver.
    Definition IterativeSolver.h:86
    │ │ │ │ +
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ +
    Definition SubgraphBuilder.h:96
    │ │ │ │ +
    Definition SubgraphSolver.h:36
    │ │ │ │ +
    This class implements the linear SPCG solver presented in Dellaert et al in IROS'10.
    Definition SubgraphSolver.h:76
    │ │ │ │ +
    ~SubgraphSolver() override
    Destructor.
    Definition SubgraphSolver.h:112
    │ │ │ │ +
    boost::shared_ptr< SubgraphPreconditioner > pc_
    preconditioner object
    Definition SubgraphSolver.h:82
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,163 +1,139 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Line3.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 * 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> │ │ │ │ │ +11 │ │ │ │ │ +20#pragma once │ │ │ │ │ 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 │ │ │ │ │ -_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; │ │ │ │ │ +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; │ │ │ │ │ +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 = │ │ │ │ │ +_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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _L_i_n_e_3_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a00431_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00551_source.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Similarity2.h
    │ │ │ │ +
    HybridFactor.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/Lie.h>
    │ │ │ │ -
    21#include <gtsam/base/Manifold.h>
    │ │ │ │ -
    22#include <gtsam/dllexport.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25#include <gtsam/geometry/Rot2.h>
    │ │ │ │ +
    20#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    29// Forward declarations
    │ │ │ │ -
    30class Pose2;
    │ │ │ │ -
    31
    │ │ │ │ -
    │ │ │ │ -
    35class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
    │ │ │ │ -
    38 typedef Rot2 Rotation;
    │ │ │ │ -
    39 typedef Point2 Translation;
    │ │ │ │ +
    27#include <cstddef>
    │ │ │ │ +
    28#include <string>
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │ +
    30
    │ │ │ │ +
    31class HybridValues;
    │ │ │ │ +
    32
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    42 private:
    │ │ │ │ -
    43 Rot2 R_;
    │ │ │ │ -
    44 Point2 t_;
    │ │ │ │ -
    45 double s_;
    │ │ │ │ -
    46
    │ │ │ │ -
    47 public:
    │ │ │ │ -
    50
    │ │ │ │ - │ │ │ │ -
    53
    │ │ │ │ -
    55 Similarity2(double s);
    │ │ │ │ -
    56
    │ │ │ │ -
    58 Similarity2(const Rot2& R, const Point2& t, double s);
    │ │ │ │ -
    59
    │ │ │ │ -
    61 Similarity2(const Matrix2& R, const Vector2& t, double s);
    │ │ │ │ -
    62
    │ │ │ │ -
    64 Similarity2(const Matrix3& T);
    │ │ │ │ -
    65
    │ │ │ │ -
    69
    │ │ │ │ -
    71 bool equals(const Similarity2& sim, double tol) const;
    │ │ │ │ -
    72
    │ │ │ │ -
    74 bool operator==(const Similarity2& other) const;
    │ │ │ │ -
    75
    │ │ │ │ -
    77 void print(const std::string& s) const;
    │ │ │ │ -
    78
    │ │ │ │ -
    79 friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
    │ │ │ │ -
    80
    │ │ │ │ -
    84
    │ │ │ │ -
    86 static Similarity2 Identity();
    │ │ │ │ -
    87
    │ │ │ │ -
    89 Similarity2 operator*(const Similarity2& S) const;
    │ │ │ │ +
    │ │ │ │ +
    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_;
    │ │ │ │ + │ │ │ │ +
    63
    │ │ │ │ +
    64 public:
    │ │ │ │ +
    65 // typedefs needed to play nice with gtsam
    │ │ │ │ + │ │ │ │ +
    67 typedef boost::shared_ptr<HybridFactor>
    │ │ │ │ + │ │ │ │ +
    69 typedef Factor Base;
    │ │ │ │ +
    70
    │ │ │ │ +
    73
    │ │ │ │ +
    75 HybridFactor() = default;
    │ │ │ │ +
    76
    │ │ │ │ +
    82 explicit HybridFactor(const KeyVector &keys);
    │ │ │ │ +
    83
    │ │ │ │ +
    89 explicit HybridFactor(const DiscreteKeys &discreteKeys);
    │ │ │ │
    90
    │ │ │ │ -
    92 Similarity2 inverse() const;
    │ │ │ │ -
    93
    │ │ │ │ -
    97
    │ │ │ │ -
    99 Point2 transformFrom(const Point2& p) const;
    │ │ │ │ -
    100
    │ │ │ │ -
    112 Pose2 transformFrom(const Pose2& T) const;
    │ │ │ │ -
    113
    │ │ │ │ -
    114 /* syntactic sugar for transformFrom */
    │ │ │ │ -
    115 Point2 operator*(const Point2& p) const;
    │ │ │ │ -
    116
    │ │ │ │ -
    120 static Similarity2 Align(const Point2Pairs& abPointPairs);
    │ │ │ │ +
    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
    │ │ │ │ -
    135 static Similarity2 Align(const Pose2Pairs& abPosePairs);
    │ │ │ │ +
    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
    │ │ │ │ -
    140
    │ │ │ │ -
    145 static Vector4 Logmap(const Similarity2& S, //
    │ │ │ │ -
    146 OptionalJacobian<4, 4> Hm = boost::none);
    │ │ │ │ -
    147
    │ │ │ │ -
    149 static Similarity2 Expmap(const Vector4& v, //
    │ │ │ │ -
    150 OptionalJacobian<4, 4> Hm = boost::none);
    │ │ │ │ -
    151
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    154 static Similarity2 Retract(const Vector4& v,
    │ │ │ │ -
    155 ChartJacobian H = boost::none) {
    │ │ │ │ -
    156 return Similarity2::Expmap(v, H);
    │ │ │ │ -
    157 }
    │ │ │ │ -
    158 static Vector4 Local(const Similarity2& other,
    │ │ │ │ -
    159 ChartJacobian H = boost::none) {
    │ │ │ │ -
    160 return Similarity2::Logmap(other, H);
    │ │ │ │ -
    161 }
    │ │ │ │ -
    162 };
    │ │ │ │ -
    │ │ │ │ -
    163
    │ │ │ │ -
    165 Matrix4 AdjointMap() const;
    │ │ │ │ -
    166
    │ │ │ │ -
    167 using LieGroup<Similarity2, 4>::inverse;
    │ │ │ │ -
    168
    │ │ │ │ -
    172
    │ │ │ │ -
    174 Matrix3 matrix() const;
    │ │ │ │ -
    175
    │ │ │ │ -
    177 Rot2 rotation() const { return R_; }
    │ │ │ │ -
    178
    │ │ │ │ -
    180 Point2 translation() const { return t_; }
    │ │ │ │ -
    181
    │ │ │ │ -
    183 double scale() const { return s_; }
    │ │ │ │ -
    184
    │ │ │ │ -
    186 inline static size_t Dim() { return 4; }
    │ │ │ │ -
    187
    │ │ │ │ -
    189 inline size_t dim() const { return 4; }
    │ │ │ │ -
    190
    │ │ │ │ -
    192};
    │ │ │ │ -
    │ │ │ │ -
    193
    │ │ │ │ -
    194template <>
    │ │ │ │ -
    195struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
    │ │ │ │ -
    196
    │ │ │ │ -
    197template <>
    │ │ │ │ -
    198struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
    │ │ │ │ -
    199
    │ │ │ │ -
    200} // namespace gtsam
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    2D Pose
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ +
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    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
    │ │ │ │ -
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ -
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ -
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ -
    2D similarity transform
    Definition Similarity2.h:35
    │ │ │ │ -
    Point2 translation() const
    Return a GTSAM translation.
    Definition Similarity2.h:180
    │ │ │ │ -
    double scale() const
    Return the scale.
    Definition Similarity2.h:183
    │ │ │ │ -
    static size_t Dim()
    Dimensionality of tangent space = 4 DOF - used to autodetect sizes.
    Definition Similarity2.h:186
    │ │ │ │ -
    Rot2 rotation() const
    Return a GTSAM rotation.
    Definition Similarity2.h:177
    │ │ │ │ -
    size_t dim() const
    Dimensionality of tangent space = 4 DOF.
    Definition Similarity2.h:189
    │ │ │ │ -
    Chart at the origin.
    Definition Similarity2.h:153
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ +
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ +
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ +
    size_t nrContinuous() const
    Return the number of continuous variables in this factor.
    Definition HybridFactor.h:129
    │ │ │ │ +
    HybridFactor This
    This class.
    Definition HybridFactor.h:66
    │ │ │ │ +
    boost::shared_ptr< HybridFactor > shared_ptr
    shared_ptr to this class
    Definition HybridFactor.h:68
    │ │ │ │ +
    bool isDiscrete() const
    True if this is a factor of discrete variables only.
    Definition HybridFactor.h:120
    │ │ │ │ +
    bool isHybrid() const
    True is this is a Discrete-Continuous factor.
    Definition HybridFactor.h:126
    │ │ │ │ +
    HybridFactor()=default
    Default constructor creates empty factor.
    │ │ │ │ +
    Factor Base
    Our base class.
    Definition HybridFactor.h:69
    │ │ │ │ +
    const KeyVector & continuousKeys() const
    Return only the continuous keys for this factor.
    Definition HybridFactor.h:135
    │ │ │ │ +
    bool isContinuous() const
    True if this is a factor of continuous variables only.
    Definition HybridFactor.h:123
    │ │ │ │ +
    KeyVector continuousKeys_
    Record continuous keys for book-keeping.
    Definition HybridFactor.h:62
    │ │ │ │ +
    virtual ~HybridFactor()=default
    Virtual destructor.
    │ │ │ │ +
    const DiscreteKeys & discreteKeys() const
    Return the discrete keys for this factor.
    Definition HybridFactor.h:132
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    the error.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Similarity2.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 │ │ │ │ │ @@ -16,197 +16,198 @@ │ │ │ │ │ 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_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +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 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -29// Forward declarations │ │ │ │ │ -30class Pose2; │ │ │ │ │ -31 │ │ │ │ │ -_3_5class GTSAM_EXPORT _S_i_m_i_l_a_r_i_t_y_2 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ -38 typedef _R_o_t_2 _R_o_t_a_t_i_o_n; │ │ │ │ │ -39 typedef _P_o_i_n_t_2 Translation; │ │ │ │ │ +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 │ │ │ │ │ -42 private: │ │ │ │ │ -43 _R_o_t_2 R_; │ │ │ │ │ -44 _P_o_i_n_t_2 t_; │ │ │ │ │ -45 double s_; │ │ │ │ │ -46 │ │ │ │ │ -47 public: │ │ │ │ │ -50 │ │ │ │ │ -52 _S_i_m_i_l_a_r_i_t_y_2(); │ │ │ │ │ -53 │ │ │ │ │ -55 _S_i_m_i_l_a_r_i_t_y_2(double s); │ │ │ │ │ -56 │ │ │ │ │ -58 _S_i_m_i_l_a_r_i_t_y_2(const _R_o_t_2& R, const _P_o_i_n_t_2& t, double s); │ │ │ │ │ -59 │ │ │ │ │ -61 _S_i_m_i_l_a_r_i_t_y_2(const Matrix2& R, const Vector2& t, double s); │ │ │ │ │ -62 │ │ │ │ │ -64 _S_i_m_i_l_a_r_i_t_y_2(const Matrix3& T); │ │ │ │ │ -65 │ │ │ │ │ -69 │ │ │ │ │ -71 bool _e_q_u_a_l_s(const _S_i_m_i_l_a_r_i_t_y_2& sim, double tol) const; │ │ │ │ │ -72 │ │ │ │ │ -74 bool _o_p_e_r_a_t_o_r_=_=(const _S_i_m_i_l_a_r_i_t_y_2& other) const; │ │ │ │ │ -75 │ │ │ │ │ -77 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ -78 │ │ │ │ │ -79 friend std::ostream& operator<<(std::ostream& os, const _S_i_m_i_l_a_r_i_t_y_2& p); │ │ │ │ │ -80 │ │ │ │ │ -84 │ │ │ │ │ -86 static _S_i_m_i_l_a_r_i_t_y_2 Identity(); │ │ │ │ │ -87 │ │ │ │ │ -89 _S_i_m_i_l_a_r_i_t_y_2 _o_p_e_r_a_t_o_r_*(const _S_i_m_i_l_a_r_i_t_y_2& S) const; │ │ │ │ │ +_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 │ │ │ │ │ -92 _S_i_m_i_l_a_r_i_t_y_2 inverse() const; │ │ │ │ │ -93 │ │ │ │ │ -97 │ │ │ │ │ -99 _P_o_i_n_t_2 transformFrom(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -100 │ │ │ │ │ -112 _P_o_s_e_2 transformFrom(const _P_o_s_e_2& T) const; │ │ │ │ │ -113 │ │ │ │ │ -114 /* syntactic sugar for transformFrom */ │ │ │ │ │ -115 _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -116 │ │ │ │ │ -120 static _S_i_m_i_l_a_r_i_t_y_2 Align(const Point2Pairs& abPointPairs); │ │ │ │ │ +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 │ │ │ │ │ -135 static _S_i_m_i_l_a_r_i_t_y_2 Align(const Pose2Pairs& abPosePairs); │ │ │ │ │ +_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 │ │ │ │ │ -140 │ │ │ │ │ -145 static Vector4 Logmap(const _S_i_m_i_l_a_r_i_t_y_2& S, // │ │ │ │ │ -146 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Hm = boost::none); │ │ │ │ │ -147 │ │ │ │ │ -149 static _S_i_m_i_l_a_r_i_t_y_2 Expmap(const Vector4& v, // │ │ │ │ │ -150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> 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_2 Retract(const Vector4& v, │ │ │ │ │ -155 ChartJacobian H = boost::none) { │ │ │ │ │ -156 return Similarity2::Expmap(v, H); │ │ │ │ │ -157 } │ │ │ │ │ -158 static Vector4 Local(const _S_i_m_i_l_a_r_i_t_y_2& other, │ │ │ │ │ -159 ChartJacobian H = boost::none) { │ │ │ │ │ -160 return Similarity2::Logmap(other, H); │ │ │ │ │ -161 } │ │ │ │ │ -162 }; │ │ │ │ │ -163 │ │ │ │ │ -165 Matrix4 AdjointMap() const; │ │ │ │ │ -166 │ │ │ │ │ -167 using _L_i_e_G_r_o_u_p<_S_i_m_i_l_a_r_i_t_y_2, 4>::inverse; │ │ │ │ │ -168 │ │ │ │ │ -172 │ │ │ │ │ -174 Matrix3 matrix() const; │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 _R_o_t_2 _r_o_t_a_t_i_o_n() const { return R_; } │ │ │ │ │ -178 │ │ │ │ │ -_1_8_0 _P_o_i_n_t_2 _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ -181 │ │ │ │ │ -_1_8_3 double _s_c_a_l_e() const { return s_; } │ │ │ │ │ -184 │ │ │ │ │ -_1_8_6 inline static size_t _D_i_m() { return 4; } │ │ │ │ │ -187 │ │ │ │ │ -_1_8_9 inline size_t _d_i_m() const { return 4; } │ │ │ │ │ -190 │ │ │ │ │ -192}; │ │ │ │ │ -193 │ │ │ │ │ -194template <> │ │ │ │ │ -_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. │ │ │ │ │ -_P_o_s_e_2_._h │ │ │ │ │ -2D Pose │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +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_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_._h │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_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_:_:_S_i_m_i_l_a_r_i_t_y_2 │ │ │ │ │ -2D similarity transform │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -Point2 translation() const │ │ │ │ │ -Return a GTSAM translation. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_s_c_a_l_e │ │ │ │ │ -double scale() const │ │ │ │ │ -Return the scale. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:183 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Dimensionality of tangent space = 4 DOF - used to autodetect sizes. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -Rot2 rotation() const │ │ │ │ │ -Return a GTSAM rotation. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality of tangent space = 4 DOF. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -Chart at the origin. │ │ │ │ │ -DDeeffiinniittiioonn Similarity2.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_:_:_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_2_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _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/a00437.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00950.html │ │ │ │┄ Files 11% similar despite different names │ │ │ │ @@ -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/navigation/CombinedImuFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ +Macros | │ │ │ │ Functions
    │ │ │ │ -
    SO3.h File Reference
    │ │ │ │ +
    CombinedImuFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    3*3 matrix representation of SO(3) │ │ │ │ -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 >
     

    │ │ │ │ +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

    │ │ │ │ -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
     
    │ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const CombinedImuFactor &f)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    3*3 matrix representation of SO(3)

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │
    │ │ │ │ -Luca Carlone
    │ │ │ │ +Stephen Williams │ │ │ │
    │ │ │ │ -Duy Nguyen Ta
    │ │ │ │ -
    Date
    December 2014
    │ │ │ │ -

    Function Documentation

    │ │ │ │ - │ │ │ │ -

    ◆ compose()

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
    const SO3R,
    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.

    │ │ │ │ - │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ +Richard Roberts │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │ +
    │ │ │ │ +David Jensen
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,61 +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 | _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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ - 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_._._. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  DD__RR__RR(H)   (H)->block<3,3>(0,0) │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_3_ _> │ │ │ │ │ +#define  DD__RR__tt(H)   (H)->block<3,3>(0,3) │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_3_ _> │ │ │ │ │ +#define  DD__RR__vv(H)   (H)->block<3,3>(0,6) │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +#define  DD__tt__RR(H)   (H)->block<3,3>(3,0) │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSOO33 = _S_O< 3 > │ │ │ │ │ +#define  DD__tt__tt(H)   (H)->block<3,3>(3,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. │ │ │ │ │ +#define  DD__tt__vv(H)   (H)->block<3,3>(3,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__RR(H)   (H)->block<3,3>(6,0) │ │ │ │ │   │ │ │ │ │ -GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ -  (constant) Jacobian of compose wrpt M │ │ │ │ │ +#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 ********** │ │ │ │ │ -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. │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_3_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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/a00086_source.html │ │ │ │┄ Files 6% 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/base/ConcurrentMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Cal3DS2_Base.h
    │ │ │ │ +
    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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ -
    22#include <gtsam/geometry/Cal3.h>
    │ │ │ │ - │ │ │ │ -
    24#include <boost/shared_ptr.hpp>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23// Change class depending on whether we are using TBB
    │ │ │ │ +
    24#ifdef GTSAM_USE_TBB
    │ │ │ │
    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 };
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    48
    │ │ │ │ +
    49#endif
    │ │ │ │
    50
    │ │ │ │ -
    52 using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
    │ │ │ │ -
    53
    │ │ │ │ +
    51#include <boost/serialization/nvp.hpp>
    │ │ │ │ +
    52#include <boost/serialization/split_member.hpp>
    │ │ │ │ +
    53#include <boost/static_assert.hpp>
    │ │ │ │ +
    54
    │ │ │ │ + │ │ │ │
    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);
    │ │ │ │ +
    57namespace gtsam {
    │ │ │ │ +
    58
    │ │ │ │ +
    67template<typename KEY, typename VALUE>
    │ │ │ │ +
    │ │ │ │ +
    68class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {
    │ │ │ │ +
    69
    │ │ │ │ +
    70public:
    │ │ │ │ +
    71
    │ │ │ │ + │ │ │ │ +
    73
    │ │ │ │ + │ │ │ │ +
    76
    │ │ │ │ +
    78 template<typename INPUTITERATOR>
    │ │ │ │ +
    79 ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
    │ │ │ │ +
    80
    │ │ │ │ + │ │ │ │ +
    83
    │ │ │ │ +
    85 ConcurrentMap(const Base& x) : Base(x) {}
    │ │ │ │ +
    86
    │ │ │ │ +
    88 bool exists(const KEY& e) const { return this->count(e); }
    │ │ │ │
    89
    │ │ │ │ -
    91 void print(const std::string& s = "") const override;
    │ │ │ │ -
    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_; }
    │ │ │ │ -
    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}
    │ │ │ │ -
    Common code for all Calibration models.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    119 ar & BOOST_SERIALIZATION_NVP(map);
    │ │ │ │ +
    120 this->insert(map.begin(), map.end());
    │ │ │ │ +
    121 }
    │ │ │ │ +
    122 BOOST_SERIALIZATION_SPLIT_MEMBER()
    │ │ │ │ +
    123};
    │ │ │ │ +
    │ │ │ │ +
    124
    │ │ │ │ +
    125}
    │ │ │ │ +
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ -
    Calibration of a camera with radial distortion.
    Definition Cal3DS2_Base.h:42
    │ │ │ │ -
    Cal3DS2_Base()=default
    Default Constructor with only unit focal length.
    │ │ │ │ -
    double p2() const
    Second tangential distortion coefficient.
    Definition Cal3DS2_Base.h:110
    │ │ │ │ -
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3DS2_Base.h:142
    │ │ │ │ -
    virtual boost::shared_ptr< Cal3DS2_Base > clone() const
    Definition Cal3DS2_Base.h:149
    │ │ │ │ -
    double k2() const
    Second distortion coefficient.
    Definition Cal3DS2_Base.h:104
    │ │ │ │ -
    double k1() const
    First distortion coefficient.
    Definition Cal3DS2_Base.h:101
    │ │ │ │ -
    size_t dim() const override
    return DOF, dimensionality of tangent space
    Definition Cal3DS2_Base.h:139
    │ │ │ │ -
    double p1() const
    First tangential distortion coefficient.
    Definition Cal3DS2_Base.h:107
    │ │ │ │ -
    Vector4 k() const
    return distortion parameter vector
    Definition Cal3DS2_Base.h:113
    │ │ │ │ +
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition ConcurrentMap.h:68
    │ │ │ │ +
    ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last)
    Constructor from a range, passes through to base class.
    Definition ConcurrentMap.h:79
    │ │ │ │ +
    ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x)
    Copy constructor from another ConcurrentMap.
    Definition ConcurrentMap.h:82
    │ │ │ │ +
    bool exists(const KEY &e) const
    Handy 'exists' function.
    Definition ConcurrentMap.h:88
    │ │ │ │ +
    ConcurrentMap(const Base &x)
    Copy constructor from the base map class.
    Definition ConcurrentMap.h:85
    │ │ │ │ +
    ConcurrentMap()
    Default constructor.
    Definition ConcurrentMap.h:75
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition ConcurrentMap.h:104
    │ │ │ │ +
    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,184 +1,168 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3DS2_Base.h │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ 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 }; │ │ │ │ │ +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 │ │ │ │ │ -52 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -53 │ │ │ │ │ +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 │ │ │ │ │ -_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); │ │ │ │ │ +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 │ │ │ │ │ -91 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ -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_; } │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * CCaall33DDSS22__BBaassee..hh │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * 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/a00446_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00911_source.html │ │ │ │┄ Files 17% similar despite different names │ │ │ │ @@ -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/navigation/AttitudeFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    CalibratedCamera.h
    │ │ │ │ +
    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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <gtsam/base/concepts.h>
    │ │ │ │ -
    25#include <gtsam/base/Manifold.h>
    │ │ │ │ - │ │ │ │ -
    27#include <gtsam/dllexport.h>
    │ │ │ │ -
    28#include <boost/serialization/nvp.hpp>
    │ │ │ │ -
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ -
    31
    │ │ │ │ -
    │ │ │ │ -
    32class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
    │ │ │ │ -
    33public:
    │ │ │ │ - │ │ │ │ -
    35 : CheiralityException(std::numeric_limits<Key>::max()) {}
    │ │ │ │ -
    36
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    54public:
    │ │ │ │ -
    55
    │ │ │ │ -
    57 typedef Rot3 Rotation;
    │ │ │ │ -
    58 typedef Point3 Translation;
    │ │ │ │ -
    59
    │ │ │ │ - │ │ │ │ -
    65 typedef Point2Vector MeasurementVector;
    │ │ │ │ -
    66
    │ │ │ │ -
    67private:
    │ │ │ │ -
    68
    │ │ │ │ -
    69 Pose3 pose_;
    │ │ │ │ -
    70
    │ │ │ │ -
    71protected:
    │ │ │ │ -
    72
    │ │ │ │ -
    75
    │ │ │ │ -
    81 static Matrix26 Dpose(const Point2& pn, double d);
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22#include <gtsam/geometry/Unit3.h>
    │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │ +
    25
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ +
    36protected:
    │ │ │ │ +
    37
    │ │ │ │ +
    38 Unit3 nZ_, bRef_;
    │ │ │ │ +
    39
    │ │ │ │ +
    40public:
    │ │ │ │ +
    41
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    44 }
    │ │ │ │ +
    │ │ │ │ +
    45
    │ │ │ │ +
    │ │ │ │ +
    51 AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ +
    52 nZ_(nZ), bRef_(bRef) {
    │ │ │ │ +
    53 }
    │ │ │ │ +
    │ │ │ │ +
    54
    │ │ │ │ +
    56 Vector attitudeError(const Rot3& p,
    │ │ │ │ +
    57 OptionalJacobian<2,3> H = boost::none) const;
    │ │ │ │ +
    58
    │ │ │ │ +
    59 const Unit3& nZ() const {
    │ │ │ │ +
    60 return nZ_;
    │ │ │ │ +
    61 }
    │ │ │ │ +
    62 const Unit3& bRef() const {
    │ │ │ │ +
    63 return bRef_;
    │ │ │ │ +
    64 }
    │ │ │ │ +
    65
    │ │ │ │ + │ │ │ │ +
    68 template<class ARCHIVE>
    │ │ │ │ +
    69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    70 ar & boost::serialization::make_nvp("nZ_", nZ_);
    │ │ │ │ +
    71 ar & boost::serialization::make_nvp("bRef_", bRef_);
    │ │ │ │ +
    72 }
    │ │ │ │ +
    73};
    │ │ │ │ +
    │ │ │ │ +
    74
    │ │ │ │ +
    │ │ │ │ +
    79class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
    │ │ │ │ +
    80
    │ │ │ │ + │ │ │ │
    82
    │ │ │ │ -
    89 static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
    │ │ │ │ +
    83public:
    │ │ │ │ +
    84
    │ │ │ │ +
    86 typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
    │ │ │ │ +
    87
    │ │ │ │ + │ │ │ │
    90
    │ │ │ │ -
    92
    │ │ │ │ -
    93public:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    93 }
    │ │ │ │ +
    │ │ │ │
    94
    │ │ │ │ +
    95 ~Rot3AttitudeFactor() override {
    │ │ │ │ +
    96 }
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    140
    │ │ │ │ -
    142 bool equals(const PinholeBase &camera, double tol = 1e-9) const;
    │ │ │ │ -
    143
    │ │ │ │ -
    145 virtual void print(const std::string& s = "PinholeBase") const;
    │ │ │ │ -
    146
    │ │ │ │ -
    150
    │ │ │ │ -
    │ │ │ │ -
    152 const Pose3& pose() const {
    │ │ │ │ -
    153 return pose_;
    │ │ │ │ -
    154 }
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    105 Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
    │ │ │ │ +
    106 const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ +
    107 Base(model, key), AttitudeFactor(nZ, bRef) {
    │ │ │ │ +
    108 }
    │ │ │ │ +
    │ │ │ │ +
    109
    │ │ │ │ +
    │ │ │ │ +
    111 gtsam::NonlinearFactor::shared_ptr clone() const override {
    │ │ │ │ +
    112 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
    │ │ │ │ +
    113 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
    │ │ │ │ +
    114 }
    │ │ │ │ +
    │ │ │ │ +
    115
    │ │ │ │ +
    117 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
    │ │ │ │ +
    118 DefaultKeyFormatter) const override;
    │ │ │ │ +
    119
    │ │ │ │ +
    121 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
    │ │ │ │ +
    122
    │ │ │ │ +
    │ │ │ │ +
    124 Vector evaluateError(const Rot3& nRb, //
    │ │ │ │ +
    125 boost::optional<Matrix&> H = boost::none) const override {
    │ │ │ │ +
    126 return attitudeError(nRb, H);
    │ │ │ │ +
    127 }
    │ │ │ │ +
    │ │ │ │ +
    128
    │ │ │ │ +
    129private:
    │ │ │ │ +
    130
    │ │ │ │ +
    132 friend class boost::serialization::access;
    │ │ │ │ +
    133 template<class ARCHIVE>
    │ │ │ │ +
    134 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    135 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    144};
    │ │ │ │ +
    │ │ │ │ +
    145
    │ │ │ │ +
    147template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
    │ │ │ │ +
    148
    │ │ │ │ +
    │ │ │ │ +
    153class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
    │ │ │ │ +
    154 public AttitudeFactor {
    │ │ │ │
    155
    │ │ │ │ -
    │ │ │ │ -
    157 const Rot3& rotation() const {
    │ │ │ │ -
    158 return pose_.rotation();
    │ │ │ │ -
    159 }
    │ │ │ │ -
    │ │ │ │ -
    160
    │ │ │ │ -
    │ │ │ │ -
    162 const Point3& translation() const {
    │ │ │ │ -
    163 return pose_.translation();
    │ │ │ │ -
    164 }
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ +
    157
    │ │ │ │ +
    158public:
    │ │ │ │ +
    159
    │ │ │ │ +
    161 typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
    │ │ │ │ +
    162
    │ │ │ │ + │ │ │ │
    165
    │ │ │ │ -
    167 const Pose3& getPose(OptionalJacobian<6, 6> H) const;
    │ │ │ │ -
    168
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    168 }
    │ │ │ │ +
    │ │ │ │ +
    169
    │ │ │ │ +
    170 ~Pose3AttitudeFactor() override {
    │ │ │ │ +
    171 }
    │ │ │ │
    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 }
    │ │ │ │ +
    │ │ │ │ +
    180 Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
    │ │ │ │ +
    181 const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ +
    182 Base(model, key), AttitudeFactor(nZ, bRef) {
    │ │ │ │ +
    183 }
    │ │ │ │ +
    │ │ │ │ +
    184
    │ │ │ │ +
    │ │ │ │ +
    186 gtsam::NonlinearFactor::shared_ptr clone() const override {
    │ │ │ │ +
    187 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
    │ │ │ │ +
    188 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
    │ │ │ │ +
    189 }
    │ │ │ │ +
    │ │ │ │ +
    190
    │ │ │ │ +
    192 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
    │ │ │ │ +
    193 DefaultKeyFormatter) const override;
    │ │ │ │ +
    194
    │ │ │ │ +
    196 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
    │ │ │ │ +
    197
    │ │ │ │ +
    │ │ │ │ +
    199 Vector evaluateError(const Pose3& nTb, //
    │ │ │ │ +
    200 boost::optional<Matrix&> H = boost::none) const override {
    │ │ │ │ +
    201 Vector e = attitudeError(nTb.rotation(), H);
    │ │ │ │ +
    202 if (H) {
    │ │ │ │ +
    203 Matrix H23 = *H;
    │ │ │ │ +
    204 *H = Matrix::Zero(2,6);
    │ │ │ │ +
    205 H->block<2,3>(0,0) = H23;
    │ │ │ │ +
    206 }
    │ │ │ │ +
    207 return e;
    │ │ │ │ +
    208 }
    │ │ │ │ +
    │ │ │ │ +
    209
    │ │ │ │ +
    210private:
    │ │ │ │ +
    211
    │ │ │ │ +
    213 friend class boost::serialization::access;
    │ │ │ │ +
    214 template<class ARCHIVE>
    │ │ │ │ +
    215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    216 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    Bearing-Range product.
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    228template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
    │ │ │ │ +
    229
    │ │ │ │ +
    230}
    │ │ │ │ +
    231
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    3D Pose
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Point2_ project(const Point3_ &p_cam)
    Expression version of PinholeBase::Project.
    Definition expressions.h:131
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    Definition ThreadsafeException.h:42
    │ │ │ │ -
    Definition BearingRange.h:40
    │ │ │ │ -
    Definition BearingRange.h:194
    │ │ │ │ -
    Definition CalibratedCamera.h:32
    │ │ │ │ -
    A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
    Definition CalibratedCamera.h:52
    │ │ │ │ -
    PinholeBase()
    Default constructor.
    Definition CalibratedCamera.h:123
    │ │ │ │ -
    PinholeBase(const Pose3 &pose)
    Constructor with pose.
    Definition CalibratedCamera.h:126
    │ │ │ │ -
    const Point3 & translation() const
    get translation
    Definition CalibratedCamera.h:162
    │ │ │ │ -
    const Rot3 & rotation() const
    get rotation
    Definition CalibratedCamera.h:157
    │ │ │ │ -
    Point2 Measurement
    Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
    Definition CalibratedCamera.h:64
    │ │ │ │ -
    const Pose3 & pose() const
    return pose, constant version
    Definition CalibratedCamera.h:152
    │ │ │ │ -
    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
    │ │ │ │ -
    Rot3 Rotation
    Pose Concept requirements.
    Definition CalibratedCamera.h:57
    │ │ │ │ -
    virtual ~PinholeBase()=default
    Default destructor.
    │ │ │ │ -
    A Calibrated camera class [R|-R't], calibration K=I.
    Definition CalibratedCamera.h:247
    │ │ │ │ -
    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
    │ │ │ │ -
    CalibratedCamera()
    default constructor
    Definition CalibratedCamera.h:259
    │ │ │ │ -
    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
    │ │ │ │ -
    size_t dim() const
    Definition CalibratedCamera.h:331
    │ │ │ │ -
    CalibratedCamera(const Vector &v)
    construct from vector
    Definition CalibratedCamera.h:303
    │ │ │ │ -
    virtual ~CalibratedCamera()
    destructor
    Definition CalibratedCamera.h:312
    │ │ │ │ -
    static size_t Dim()
    Definition CalibratedCamera.h:336
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &s="CalibratedCamera") const override
    print
    Definition CalibratedCamera.h:326
    │ │ │ │ -
    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
    │ │ │ │ -
    CalibratedCamera(const Pose3 &pose)
    construct with pose
    Definition CalibratedCamera.h:263
    │ │ │ │ -
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    Base class for prior on attitude Example:
    Definition AttitudeFactor.h:34
    │ │ │ │ +
    Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const
    vector of errors
    Definition AttitudeFactor.cpp:26
    │ │ │ │ +
    Unit3 bRef_
    Position measurement in.
    Definition AttitudeFactor.h:38
    │ │ │ │ +
    AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:51
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition AttitudeFactor.h:67
    │ │ │ │ +
    AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:43
    │ │ │ │ +
    Version of AttitudeFactor for Rot3.
    Definition AttitudeFactor.h:79
    │ │ │ │ +
    Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition AttitudeFactor.h:124
    │ │ │ │ +
    Rot3AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:92
    │ │ │ │ +
    boost::shared_ptr< Rot3AttitudeFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition AttitudeFactor.h:86
    │ │ │ │ +
    Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:105
    │ │ │ │ +
    Rot3AttitudeFactor This
    Typedef to this class.
    Definition AttitudeFactor.h:89
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AttitudeFactor.h:111
    │ │ │ │ +
    Version of AttitudeFactor for Pose3.
    Definition AttitudeFactor.h:154
    │ │ │ │ +
    Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition AttitudeFactor.h:199
    │ │ │ │ +
    boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition AttitudeFactor.h:161
    │ │ │ │ +
    Pose3AttitudeFactor This
    Typedef to this class.
    Definition AttitudeFactor.h:164
    │ │ │ │ +
    Pose3AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:167
    │ │ │ │ +
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AttitudeFactor.h:186
    │ │ │ │ +
    Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:180
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,460 +1,332 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CalibratedCamera.h │ │ │ │ │ +AttitudeFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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> │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -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_;} │ │ │ │ │ -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 │ │ │ │ │ -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_; │ │ │ │ │ -70 │ │ │ │ │ -71protected: │ │ │ │ │ -72 │ │ │ │ │ -75 │ │ │ │ │ -81 static Matrix26 Dpose(const _P_o_i_n_t_2& pn, double d); │ │ │ │ │ +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_/_P_o_s_e_3_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_3_4class _A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ +35 │ │ │ │ │ +36protected: │ │ │ │ │ +37 │ │ │ │ │ +_3_8 _U_n_i_t_3 nZ_, _b_R_e_f__; │ │ │ │ │ +39 │ │ │ │ │ +40public: │ │ │ │ │ +41 │ │ │ │ │ +_4_3 _A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ +44 } │ │ │ │ │ +45 │ │ │ │ │ +_5_1 _A_t_t_i_t_u_d_e_F_a_c_t_o_r(const _U_n_i_t_3& nZ, const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ +52 nZ_(nZ), _b_R_e_f__(bRef) { │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +56 Vector _a_t_t_i_t_u_d_e_E_r_r_o_r(const _R_o_t_3& p, │ │ │ │ │ +57 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_3_> H = boost::none) const; │ │ │ │ │ +58 │ │ │ │ │ +59 const _U_n_i_t_3& nZ() const { │ │ │ │ │ +60 return nZ_; │ │ │ │ │ +61 } │ │ │ │ │ +62 const Unit3& bRef() const { │ │ │ │ │ +63 return _b_R_e_f__; │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +_6_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; │ │ │ │ │ +68 template │ │ │ │ │ +69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +70 ar & boost::serialization::make_nvp("nZ_", nZ_); │ │ │ │ │ +71 ar & boost::serialization::make_nvp("bRef_", _b_R_e_f__); │ │ │ │ │ +72 } │ │ │ │ │ +73}; │ │ │ │ │ +74 │ │ │ │ │ +_7_9class GTSAM_EXPORT _R_o_t_3_A_t_t_i_t_u_d_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, public │ │ │ │ │ +_A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ +80 │ │ │ │ │ +81 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; │ │ │ │ │ 82 │ │ │ │ │ -89 static Matrix23 Dpoint(const _P_o_i_n_t_2& pn, double d, const Matrix3& Rt); │ │ │ │ │ +83public: │ │ │ │ │ +84 │ │ │ │ │ +_8_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +87 │ │ │ │ │ +_8_9 typedef _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ 90 │ │ │ │ │ -92 │ │ │ │ │ -93public: │ │ │ │ │ +_9_2 _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ +93 } │ │ │ │ │ 94 │ │ │ │ │ +95 _~_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() override { │ │ │ │ │ +96 } │ │ │ │ │ 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 │ │ │ │ │ -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; │ │ │ │ │ -146 │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ -153 return pose_; │ │ │ │ │ -154 } │ │ │ │ │ +_1_0_5 _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r(_K_e_y key, const _U_n_i_t_3& nZ, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +106 const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ +107 _B_a_s_e(model, key), _A_t_t_i_t_u_d_e_F_a_c_t_o_r(nZ, bRef) { │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +112 return boost::static_pointer_cast( │ │ │ │ │ +113 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +117 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +118 DefaultKeyFormatter) const override; │ │ │ │ │ +119 │ │ │ │ │ +121 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; │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& nRb, // │ │ │ │ │ +125 boost::optional H = boost::none) const override { │ │ │ │ │ +126 return attitudeError(nRb, H); │ │ │ │ │ +127 } │ │ │ │ │ +128 │ │ │ │ │ +129private: │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 friend class boost::serialization::access; │ │ │ │ │ +133 template │ │ │ │ │ +134 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +135 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +136 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +137 boost::serialization::base_object(*this)); │ │ │ │ │ +138 ar & boost::serialization::make_nvp("AttitudeFactor", │ │ │ │ │ +139 boost::serialization::base_object(*this)); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +142public: │ │ │ │ │ +143 _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 │ │ │ │ │ +144}; │ │ │ │ │ +145 │ │ │ │ │ +_1_4_7template<> struct _t_r_a_i_t_s<_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +148 │ │ │ │ │ +_1_5_3class GTSAM_EXPORT _P_o_s_e_3_A_t_t_i_t_u_d_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, │ │ │ │ │ +154 public _A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ 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 } │ │ │ │ │ +156 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; │ │ │ │ │ +157 │ │ │ │ │ +158public: │ │ │ │ │ +159 │ │ │ │ │ +_1_6_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +162 │ │ │ │ │ +_1_6_4 typedef _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ 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 │ │ │ │ │ +_1_6_7 _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +170 _~_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() override { │ │ │ │ │ +171 } │ │ │ │ │ 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 } │ │ │ │ │ +_1_8_0 _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r(_K_e_y key, const _U_n_i_t_3& nZ, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model, │ │ │ │ │ +181 const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ +182 _B_a_s_e(model, key), _A_t_t_i_t_u_d_e_F_a_c_t_o_r(nZ, bRef) { │ │ │ │ │ +183 } │ │ │ │ │ +184 │ │ │ │ │ +_1_8_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +187 return boost::static_pointer_cast( │ │ │ │ │ +188 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +189 } │ │ │ │ │ +190 │ │ │ │ │ +192 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +193 DefaultKeyFormatter) const override; │ │ │ │ │ +194 │ │ │ │ │ +196 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; │ │ │ │ │ +197 │ │ │ │ │ +_1_9_9 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& nTb, // │ │ │ │ │ +200 boost::optional H = boost::none) const override { │ │ │ │ │ +201 Vector e = attitudeError(nTb._r_o_t_a_t_i_o_n(), H); │ │ │ │ │ +202 if (H) { │ │ │ │ │ +203 Matrix H23 = *H; │ │ │ │ │ +204 *H = Matrix::Zero(2,6); │ │ │ │ │ +205 H->block<2,3>(0,0) = H23; │ │ │ │ │ +206 } │ │ │ │ │ +207 return e; │ │ │ │ │ +208 } │ │ │ │ │ +209 │ │ │ │ │ +210private: │ │ │ │ │ +211 │ │ │ │ │ +_2_1_3 friend class boost::serialization::access; │ │ │ │ │ +214 template │ │ │ │ │ +215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +216 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +217 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +218 boost::serialization::base_object(*this)); │ │ │ │ │ +219 ar & boost::serialization::make_nvp("AttitudeFactor", │ │ │ │ │ +220 boost::serialization::base_object(*this)); │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +223public: │ │ │ │ │ +224 _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 │ │ │ │ │ +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. │ │ │ │ │ +_2_2_8template<> struct _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> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +229 │ │ │ │ │ +230} │ │ │ │ │ +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 │ │ │ │ │ +#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. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +_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_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_:_:_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_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_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_:_:_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_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_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +Base class for prior on attitude Example: │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_a_t_t_i_t_u_d_e_E_r_r_o_r │ │ │ │ │ +Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) │ │ │ │ │ +const │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.cpp:26 │ │ │ │ │ +_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_b_R_e_f__ │ │ │ │ │ +Unit3 bRef_ │ │ │ │ │ +Position measurement in. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +AttitudeFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:43 │ │ │ │ │ +_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 AttitudeFactor for Rot3. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_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 AttitudeFactor.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +Rot3AttitudeFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< Rot3AttitudeFactor > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, │ │ │ │ │ +const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +Rot3AttitudeFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:111 │ │ │ │ │ +_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 AttitudeFactor for Pose3. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< Pose3AttitudeFactor > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +Pose3AttitudeFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +Pose3AttitudeFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, │ │ │ │ │ +const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn AttitudeFactor.h:180 │ │ │ │ │ +_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_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._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_._h │ │ │ │ │ * Generated on Tue Jan 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/a01049_source.html │ │ │ │┄ Files 14% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    34#include <gtsam/slam/TriangulationFactor.h>
    │ │ │ │ -
    35
    │ │ │ │ -
    36namespace gtsam {
    │ │ │ │ -
    37
    │ │ │ │ -
    │ │ │ │ -
    39class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
    │ │ │ │ -
    40public:
    │ │ │ │ - │ │ │ │ -
    42 std::runtime_error("Triangulation Underconstrained Exception.") {
    │ │ │ │ -
    43 }
    │ │ │ │ -
    44};
    │ │ │ │ -
    │ │ │ │ -
    45
    │ │ │ │ -
    │ │ │ │ -
    47class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
    │ │ │ │ -
    48public:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46 public:
    │ │ │ │ +
    48 typedef typename GncParameters::OptimizerType BaseOptimizer;
    │ │ │ │ +
    49
    │ │ │ │ +
    50 private:
    │ │ │ │ + │ │ │ │ +
    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_
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    561
    │ │ │ │ - │ │ │ │ -
    567
    │ │ │ │ - │ │ │ │ -
    574
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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)
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    722 && maxReprojError > params.dynamicOutlierRejectionThreshold)
    │ │ │ │ -
    723 return TriangulationResult::Outlier();
    │ │ │ │ -
    724
    │ │ │ │ -
    725 // all good!
    │ │ │ │ -
    726 return TriangulationResult(point);
    │ │ │ │ - │ │ │ │ -
    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();
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    │ │ │ │ -
    Base class to create smart factors on poses or cameras.
    │ │ │ │ -
    Unified Calibration Model, see Mei07icra for details.
    │ │ │ │ -
    Calibration used by Bundler.
    │ │ │ │ -
    Calibrated camera with spherical projection.
    │ │ │ │ -
    Base class for all pinhole cameras.
    │ │ │ │ -
    Calibration of a fisheye camera.
    │ │ │ │ -
    2D Pose
    │ │ │ │ -
    Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
    │ │ │ │ -
    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)
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
    Create a pinhole calibration from a different Cal3 object, removing distortion.
    Definition triangulation.h:253
    │ │ │ │ -
    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
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    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
    │ │ │ │ -
    TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
    triangulateSafe: extensive checking of the outcome
    Definition triangulation.h:680
    │ │ │ │ -
    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
    │ │ │ │ -
    double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
    distance between two points
    Definition Point3.cpp:27
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ -
    A set of cameras, all with their own calibration.
    Definition CameraSet.h:36
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ -
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    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
    │ │ │ │ -
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │ -
    Exception thrown by triangulateDLT when SVD returns rank < 3.
    Definition triangulation.h:39
    │ │ │ │ -
    Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
    Definition triangulation.h:47
    │ │ │ │ -
    Definition triangulation.h:556
    │ │ │ │ -
    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
    │ │ │ │ -
    double dynamicOutlierRejectionThreshold
    If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
    Definition triangulation.h:573
    │ │ │ │ -
    double rankTolerance
    threshold to decide whether triangulation is result.degenerate
    Definition triangulation.h:558
    │ │ │ │ -
    double landmarkDistanceThreshold
    if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
    Definition triangulation.h:566
    │ │ │ │ -
    bool enableEPI
    if set to true, will refine triangulation using LM
    Definition triangulation.h:560
    │ │ │ │ -
    SharedNoiseModel noiseModel
    used in the nonlinear triangulation
    Definition triangulation.h:575
    │ │ │ │ -
    TriangulationResult is an optional point, along with the reasons why it is invalid.
    Definition triangulation.h:626
    │ │ │ │ -
    TriangulationResult()
    Default constructor, only for serialization.
    Definition triangulation.h:638
    │ │ │ │ -
    TriangulationResult(const Point3 &p)
    Constructor.
    Definition triangulation.h:643
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition triangulation.h:671
    │ │ │ │ -
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ -
    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
    │ │ │ │ -
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    bool equal(const T &obj1, const T &obj2, double tol)
    Call equal on the object.
    Definition Testable.h:84
    │ │ │ │ +
    virtual void resize(size_t size)
    Directly resize the number of factors in the graph.
    Definition FactorGraph.h:381
    │ │ │ │ +
    size_t size() const
    return the number of factors (including any null factors set by remove() ).
    Definition FactorGraph.h:326
    │ │ │ │ +
    static shared_ptr Information(const Matrix &M, bool smart=true)
    A Gaussian noise model created by specifying an information matrix.
    Definition NoiseModel.cpp:100
    │ │ │ │ +
    Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
    Definition NoiseModel.h:642
    │ │ │ │ +
    Definition GncOptimizer.h:45
    │ │ │ │ +
    bool checkWeightsConvergence(const Vector &weights) const
    Check convergence of weights to binary values.
    Definition GncOptimizer.h:364
    │ │ │ │ +
    void setWeights(const Vector w)
    Set weights for each factor.
    Definition GncOptimizer.h:144
    │ │ │ │ +
    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
    │ │ │ │ +
    GncParameters::OptimizerType BaseOptimizer
    For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
    Definition GncOptimizer.h:48
    │ │ │ │ +
    Values optimize()
    Compute optimal solution using graduated non-convexity.
    Definition GncOptimizer.h:185
    │ │ │ │ +
    const Vector & getInlierCostThresholds() const
    Get the inlier threshold.
    Definition GncOptimizer.h:166
    │ │ │ │ +
    bool checkCostConvergence(const double cost, const double prev_cost) const
    Check convergence of relative cost differences.
    Definition GncOptimizer.h:353
    │ │ │ │ +
    const Values & getState() const
    Access a copy of the internal values.
    Definition GncOptimizer.h:157
    │ │ │ │ +
    void setInlierCostThresholds(const Vector &inthVec)
    Set the maximum weighted residual error for an inlier (one for each factor).
    Definition GncOptimizer.h:125
    │ │ │ │ +
    Vector calculateWeights(const Values &currentEstimate, const double mu)
    Calculate gnc weights.
    Definition GncOptimizer.h:423
    │ │ │ │ +
    double initializeMu() const
    Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
    Definition GncOptimizer.h:274
    │ │ │ │ +
    double updateMu(const double mu) const
    Update the gnc parameter mu to gradually increase nonconvexity.
    Definition GncOptimizer.h:319
    │ │ │ │ +
    bool equals(const GncOptimizer &other, double tol=1e-9) const
    Equals.
    Definition GncOptimizer.h:169
    │ │ │ │ +
    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
    │ │ │ │ +
    void setInlierCostThresholds(const double inth)
    Set the maximum weighted residual error for an inlier (same for all factors).
    Definition GncOptimizer.h:117
    │ │ │ │ +
    void setInlierCostThresholdsAtProbability(const double alpha)
    Set the maximum weighted residual error threshold by specifying the probability alpha that the inlier...
    Definition GncOptimizer.h:132
    │ │ │ │ +
    NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
    Create a graph where each factor is weighted by the gnc weights.
    Definition GncOptimizer.h:398
    │ │ │ │ +
    const NonlinearFactorGraph & getFactors() const
    Access a copy of the internal factor graph.
    Definition GncOptimizer.h:154
    │ │ │ │ +
    const Vector & getWeights() const
    Access a copy of the GNC weights.
    Definition GncOptimizer.h:163
    │ │ │ │ +
    const GncParameters & getParams() const
    Access a copy of the parameters.
    Definition GncOptimizer.h:160
    │ │ │ │ +
    GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters &params=GncParameters())
    Constructor.
    Definition GncOptimizer.h:59
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Noise model.
    Definition NonlinearFactor.h:186
    │ │ │ │
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
    Test equality.
    Definition NonlinearFactorGraph.cpp:97
    │ │ │ │ +
    double error(const Values &values) const
    unnormalized error, in the most common case
    Definition NonlinearFactorGraph.cpp:170
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    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
    │ │ │ │ -
    Non-linear factor for a constraint derived from a 2D measurement.
    Definition TriangulationFactor.h:33
    │ │ │ │ -
    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/a00470.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00890.html │ │ │ │┄ Files 5% 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/linear/GaussianBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    Rot3M.cpp File Reference
    │ │ │ │ +
    GaussianBayesNet.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Rotation (internal: 3*3 matrix representation*) │ │ │ │ +

    Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ 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
    │ │ │ │ -
    │ │ │ │ -Richard Roberts
    │ │ │ │ +

    Chordal Bayes Net, the result of eliminating a factor graph.

    │ │ │ │ +
    Author
    Frank Dellaert, Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,20 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Rotation (internal: 3*3 matrix representation*) │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert, Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_M_._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_._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/a00200.html │ │ │ │┄ Files 23% 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/discrete/DiscreteFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    Point2.cpp File Reference
    │ │ │ │ +
    DiscreteFactorGraph.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    2D Point │ │ │ │ -More...

    │ │ │ │ +

    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

    │ │ │ │ -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< Point2gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
     
    │ │ │ │ -list< Point2gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
     
    list< Point2gtsam::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)
     
    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

    │ │ │ │ -

    2D Point

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    Feb 14, 2011
    │ │ │ │ +
    Author
    Duy-Nguyen Ta
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +1,45 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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_._._. │ │ │ │ │ +  │ │ │ │ │ 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. │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ - Point2Pair &p) │ │ │ │ │ +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 ********** │ │ │ │ │ -2D Point │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._c_p_p │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01007_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/nonlinear/WhiteNoiseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianMixture.h
    │ │ │ │ +
    WhiteNoiseFactor.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/DecisionTree-inl.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>;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    61
    │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <cmath>
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    28 const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
    │ │ │ │ +
    29
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    42
    │ │ │ │ +
    43 private:
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │
    64
    │ │ │ │ -
    65 private:
    │ │ │ │ -
    66 Conditionals conditionals_;
    │ │ │ │ -
    67 double logConstant_;
    │ │ │ │ -
    68
    │ │ │ │ -
    72 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
    │ │ │ │ -
    73
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    │ │ │ │ +
    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();
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    116 GaussianMixture(KeyVector &&continuousFrontals, KeyVector &&continuousParents,
    │ │ │ │ -
    117 DiscreteKeys &&discreteParents,
    │ │ │ │ -
    118 std::vector<GaussianConditional::shared_ptr> &&conditionals);
    │ │ │ │ -
    119
    │ │ │ │ - │ │ │ │ -
    129 const KeyVector &continuousFrontals, const KeyVector &continuousParents,
    │ │ │ │ -
    130 const DiscreteKeys &discreteParents,
    │ │ │ │ -
    131 const std::vector<GaussianConditional::shared_ptr> &conditionals);
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    151 const DiscreteValues &discreteValues) const;
    │ │ │ │ +
    │ │ │ │ +
    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 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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ +
    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
    │ │ │ │ +
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
    Return the DiscreteKey vector as a set.
    Definition GaussianMixture.cpp:221
    │ │ │ │ +
    const double logSqrt2PI
    constant needed below
    Definition WhiteNoiseFactor.h:28
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ -
    An assignment from labels to value index (size_t).
    Definition Assignment.h:37
    │ │ │ │ - │ │ │ │ -
    A discrete probabilistic factor.
    Definition DecisionTreeFactor.h:45
    │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network.
    Definition GaussianMixture.h:55
    │ │ │ │ -
    double logNormalizationConstant() const override
    The log normalization constant is max of the the individual log-normalization constants.
    Definition GaussianMixture.h:161
    │ │ │ │ -
    double operator()(const HybridValues &values) const
    Evaluate probability density, sugar.
    Definition GaussianMixture.h:231
    │ │ │ │ -
    GaussianMixture()=default
    Default constructor, mainly for serialization.
    │ │ │ │ -
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Definition Conditional.h:64
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianConditional.h:46
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    the error.
    │ │ │ │ +
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    A shared_ptr to this class.
    Definition HessianFactor.h:110
    │ │ │ │ +
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactor.cpp:37
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    Binary factor to estimate parameters of zero-mean Gaussian white noise.
    Definition WhiteNoiseFactor.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    ~WhiteNoiseFactor() override
    Destructor.
    Definition WhiteNoiseFactor.h:105
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition WhiteNoiseFactor.h:113
    │ │ │ │ +
    WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
    Construct from measurement.
    Definition WhiteNoiseFactor.h:96
    │ │ │ │ +
    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
    │ │ │ │ +
    static double f(double z, double u, double p)
    negative log likelihood as a function of mean and precision
    Definition WhiteNoiseFactor.h:61
    │ │ │ │ +
    double error(const Values &x) const override
    Calculate the error of the factor, typically equal to log-likelihood.
    Definition WhiteNoiseFactor.h:129
    │ │ │ │ +
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition WhiteNoiseFactor.h:124
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianMixture.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 │ │ │ │ │ @@ -16,221 +16,189 @@ │ │ │ │ │ 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> │ │ │ │ │ -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_>; │ │ │ │ │ +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 private: │ │ │ │ │ +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 } │ │ │ │ │ 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 │ │ │ │ │ -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); │ │ │ │ │ +_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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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_._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. │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_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 │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00506_source.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │
    26// Forward declarations
    │ │ │ │
    27class HybridEliminationTree;
    │ │ │ │
    28
    │ │ │ │
    │ │ │ │ @@ -138,28 +138,28 @@ │ │ │ │
    59 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │
    60
    │ │ │ │
    70 HybridJunctionTree(const HybridEliminationTree& eliminationTree);
    │ │ │ │
    71};
    │ │ │ │
    │ │ │ │
    72
    │ │ │ │
    73} // namespace gtsam
    │ │ │ │ -
    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ -
    The junction tree.
    │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
    │ │ │ │ +
    The junction tree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    Elimination Tree type for Hybrid Factor Graphs.
    Definition HybridEliminationTree.h:32
    │ │ │ │
    An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition HybridJunctionTree.h:53
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition HybridJunctionTree.h:59
    │ │ │ │
    JunctionTree< HybridBayesTree, HybridGaussianFactorGraph > Base
    Base class.
    Definition HybridJunctionTree.h:57
    │ │ │ │
    HybridJunctionTree This
    This class.
    Definition HybridJunctionTree.h:58
    │ │ │ │
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -38,18 +38,18 @@ │ │ │ │ │ _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. │ │ │ │ │ +_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. │ │ │ │ │ _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_:_:_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. │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00494_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00809_source.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/linear/GaussianBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridBayesTree.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
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28
    │ │ │ │ -
    29#include <string>
    │ │ │ │ +
    19// \callgraph
    │ │ │ │ +
    20
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    27
    │ │ │ │ +
    28#include <utility>
    │ │ │ │ +
    29namespace gtsam {
    │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    50 Base;
    │ │ │ │ -
    51 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    52 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ - │ │ │ │ -
    54 HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
    │ │ │ │ -
    55 : Base(conditional) {}
    │ │ │ │ -
    57 HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {}
    │ │ │ │ -
    58
    │ │ │ │ -
    59 virtual ~HybridBayesTreeClique() {}
    │ │ │ │ -
    60};
    │ │ │ │ +
    │ │ │ │ +
    35 class GTSAM_EXPORT GaussianBayesNet: public BayesNet<GaussianConditional>
    │ │ │ │ +
    36 {
    │ │ │ │ +
    37 public:
    │ │ │ │ +
    38
    │ │ │ │ + │ │ │ │ +
    40 typedef GaussianBayesNet This;
    │ │ │ │ + │ │ │ │ +
    42 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    43 typedef boost::shared_ptr<ConditionalType> sharedConditional;
    │ │ │ │ +
    44
    │ │ │ │ +
    47
    │ │ │ │ + │ │ │ │ +
    50
    │ │ │ │ +
    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
    │ │ │ │ -
    62/* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    64class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
    │ │ │ │ -
    65 private:
    │ │ │ │ - │ │ │ │ +
    64 template <class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    66 : Base(graph) {}
    │ │ │ │ +
    │ │ │ │
    67
    │ │ │ │ -
    68 public:
    │ │ │ │ -
    69 typedef HybridBayesTree This;
    │ │ │ │ -
    70 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    71
    │ │ │ │ -
    74
    │ │ │ │ -
    75 HybridBayesTree() = default;
    │ │ │ │ +
    72 template <class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    74 std::initializer_list<boost::shared_ptr<DERIVEDCONDITIONAL> > conditionals)
    │ │ │ │ +
    75 : Base(conditionals) {}
    │ │ │ │ +
    │ │ │ │
    76
    │ │ │ │ -
    78 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ +
    78 virtual ~GaussianBayesNet() = default;
    │ │ │ │
    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};
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    126template <>
    │ │ │ │ -
    127struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};
    │ │ │ │ -
    128
    │ │ │ │ -
    139template <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    141 public:
    │ │ │ │ - │ │ │ │ -
    143 typedef HybridConditional Base;
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Base class for conditional densities.
    │ │ │ │ -
    Base class for cliques of a BayesTree.
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ +
    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
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Bayes network.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    A clique in a HybridBayesTree which is a HybridConditional internally.
    Definition HybridBayesTree.h:46
    │ │ │ │ -
    HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > &conditional)
    Copy constructor.
    Definition HybridBayesTree.h:54
    │ │ │ │ -
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ -
    HybridBayesTree()=default
    Default constructor, creates an empty Bayes tree.
    │ │ │ │ -
    BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
    Construct a new Bayes Tree Orphan Wrapper object.
    Definition HybridBayesTree.h:152
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print utility
    Definition HybridBayesTree.h:163
    │ │ │ │ -
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    Definition BayesTree.h:276
    │ │ │ │ -
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ -
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ +
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ +
    double operator()(const VectorValues &x) const
    Evaluate probability density, sugar.
    Definition GaussianBayesNet.h:114
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print graph
    Definition GaussianBayesNet.h:89
    │ │ │ │ +
    GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > > conditionals)
    Constructor that takes an initializer list of shared pointers.
    Definition GaussianBayesNet.h:73
    │ │ │ │ +
    GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition GaussianBayesNet.h:65
    │ │ │ │ +
    virtual ~GaussianBayesNet()=default
    Destructor.
    │ │ │ │ +
    GaussianBayesNet(const CONTAINER &conditionals)
    Construct from container of factors (shared_ptr or plain objects)
    Definition GaussianBayesNet.h:58
    │ │ │ │ +
    GaussianBayesNet()
    Construct empty bayes net.
    Definition GaussianBayesNet.h:49
    │ │ │ │ +
    GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    Construct from iterator over conditionals.
    Definition GaussianBayesNet.h:53
    │ │ │ │ +
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,140 +1,156 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridBayesTree.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 │ │ │ │ │ -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 │ │ │ │ │ +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_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 │ │ │ │ │ -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}; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -78 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ +_7_8 virtual _~_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ 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}; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_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. │ │ │ │ │ +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_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ landmarkKey) │ │ │ │ │ Optimize for triangulation. │ │ │ │ │ @@ -149,60 +165,64 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 │ │ │ │ │ -_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_B_a_y_e_s_T_r_e_e_._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/a00518_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01205_source.html │ │ │ │┄ Files 6% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25#include <boost/format.hpp>
    │ │ │ │ -
    26#include <unordered_map>
    │ │ │ │ +
    12#pragma once
    │ │ │ │ +
    13
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/geometry/Unit3.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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    43 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    44
    │ │ │ │ - │ │ │ │ -
    46 using Indices = KeyVector;
    │ │ │ │ -
    47
    │ │ │ │ -
    48 public:
    │ │ │ │ -
    51
    │ │ │ │ -
    53 HybridFactorGraph() = default;
    │ │ │ │ +
    │ │ │ │ +
    42class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
    │ │ │ │ +
    43 private:
    │ │ │ │ + │ │ │ │ +
    45 Point3 measured_w_aZb_;
    │ │ │ │ +
    46
    │ │ │ │ +
    47 public:
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    3D Point
    │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ - │ │ │ │ -
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ -
    HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridFactorGraph.h:61
    │ │ │ │ -
    KeySet discreteKeySet() const
    Get all the discrete keys in the factor graph, as a set.
    Definition HybridFactorGraph.cpp:46
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridFactorGraph.h:43
    │ │ │ │ -
    std::unordered_map< Key, DiscreteKey > discreteKeyMap() const
    Get a map from Key to corresponding DiscreteKey.
    Definition HybridFactorGraph.cpp:56
    │ │ │ │ -
    const KeySet continuousKeySet() const
    Get all the continuous keys in the factor graph.
    Definition HybridFactorGraph.cpp:65
    │ │ │ │ -
    HybridFactorGraph()=default
    Default constructor.
    │ │ │ │ -
    std::set< DiscreteKey > discreteKeys() const
    Get all the discrete keys in the factor graph.
    Definition HybridFactorGraph.cpp:28
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Binary factor for a relative translation direction measurement w_aZb.
    Definition TranslationFactor.h:42
    │ │ │ │ +
    TranslationFactor()
    default constructor
    Definition TranslationFactor.h:49
    │ │ │ │ +
    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/a00527_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00545_source.html │ │ │ │┄ Files 1% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ │ │ │ │
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ │ │ │ │ @@ -377,13 +377,13 @@ │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    Symbol.h was moved to inference directory, this header was retained for compatibility.
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -212,18 +212,18 @@ │ │ │ │ │ 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. │ │ │ │ │ _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/a00521_source.html │ │ │ │┄ Files 2% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    HybridEliminationTree.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    23
    │ │ │ │
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │
    │ │ │ │
    31class GTSAM_EXPORT HybridEliminationTree
    │ │ │ │
    32 : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
    │ │ │ │
    33 private:
    │ │ │ │ @@ -145,17 +145,17 @@ │ │ │ │
    64
    │ │ │ │
    66
    │ │ │ │
    68 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │
    69};
    │ │ │ │
    │ │ │ │
    70
    │ │ │ │
    71} // namespace gtsam
    │ │ │ │ -
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │ -
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ - │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    Elimination Tree type for Hybrid Factor Graphs.
    Definition HybridEliminationTree.h:32
    │ │ │ │
    HybridEliminationTree This
    This class.
    Definition HybridEliminationTree.h:39
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition HybridEliminationTree.h:40
    │ │ │ │
    EliminationTree< HybridBayesNet, HybridGaussianFactorGraph > Base
    Base class.
    Definition HybridEliminationTree.h:38
    │ │ │ │
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ @@ -163,13 +163,13 @@ │ │ │ │
    Definition Ordering.h:34
    │ │ │ │
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -45,18 +45,18 @@ │ │ │ │ │ 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. │ │ │ │ │ +_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_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00548_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01388_source.html │ │ │ │┄ Files 7% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 <>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    71 typedef HybridConditional
    │ │ │ │ - │ │ │ │ -
    73 typedef HybridBayesNet
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    87 const FactorGraphType& graph,
    │ │ │ │ -
    88 boost::optional<const VariableIndex&> variableIndex) {
    │ │ │ │ -
    89 return HybridOrdering(graph);
    │ │ │ │ -
    90 }
    │ │ │ │ -
    │ │ │ │ -
    91};
    │ │ │ │ -
    │ │ │ │ -
    92
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    114 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ -
    115
    │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    37 {
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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); }
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    70 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    71
    │ │ │ │ +
    74
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    89
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    95 std::initializer_list<boost::shared_ptr<SymbolicFactor>> sharedFactors)
    │ │ │ │ +
    96 : Base(sharedFactors) {}
    │ │ │ │ +
    │ │ │ │ +
    97
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    100 push_back(boost::make_shared<SymbolicFactor>(c));
    │ │ │ │ +
    101 }
    │ │ │ │ +
    │ │ │ │ +
    102
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    110 push_back(boost::make_shared<SymbolicFactor>(c));
    │ │ │ │ +
    111 return *this;
    │ │ │ │ +
    112 }
    │ │ │ │ +
    │ │ │ │ +
    113
    │ │ │ │ + │ │ │ │ +
    116
    │ │ │ │
    118
    │ │ │ │
    121
    │ │ │ │ - │ │ │ │ -
    124
    │ │ │ │ -
    130 template <class DERIVEDFACTOR>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    170 const VectorValues& continuousValues) const;
    │ │ │ │ -
    171
    │ │ │ │ -
    178 double probPrime(const HybridValues& values) const;
    │ │ │ │ -
    179
    │ │ │ │ -
    189 GaussianFactorGraphTree assembleGraphTree() const;
    │ │ │ │ -
    190
    │ │ │ │ -
    192};
    │ │ │ │ -
    │ │ │ │ -
    193
    │ │ │ │ -
    194} // namespace gtsam
    │ │ │ │ -
    Factor graph with utilities for hybrid factors.
    │ │ │ │ - │ │ │ │ -
    A set of GaussianFactors, indexed by a set of discrete keys.
    │ │ │ │ -
    Variable elimination algorithms for factor graphs.
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    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
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ +
    Variable elimination algorithms for factor graphs.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    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
    │ │ │ │ -
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ -
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ -
    A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
    Definition HybridBayesNet.h:35
    │ │ │ │ -
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ -
    Hybrid Conditional Density.
    Definition HybridConditional.h:62
    │ │ │ │ -
    Elimination Tree type for Hybrid Factor Graphs.
    Definition HybridEliminationTree.h:32
    │ │ │ │ -
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ -
    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
    │ │ │ │ -
    HybridConditional ConditionalType
    Type of conditionals from elimination.
    Definition HybridGaussianFactorGraph.h:72
    │ │ │ │ -
    HybridGaussianFactorGraph FactorGraphType
    Type of the factor graph (e.g.
    Definition HybridGaussianFactorGraph.h:69
    │ │ │ │ -
    HybridJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition HybridGaussianFactorGraph.h:78
    │ │ │ │ -
    HybridEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition HybridGaussianFactorGraph.h:76
    │ │ │ │ -
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition HybridGaussianFactorGraph.h:86
    │ │ │ │ -
    Factor FactorType
    Type of factors in factor graph.
    Definition HybridGaussianFactorGraph.h:67
    │ │ │ │ -
    HybridBayesTree BayesTreeType
    Type of Bayes tree.
    Definition HybridGaussianFactorGraph.h:77
    │ │ │ │ -
    HybridBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition HybridGaussianFactorGraph.h:74
    │ │ │ │ -
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridGaussianFactorGraph.h:114
    │ │ │ │ -
    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
    │ │ │ │ -
    KeyVector Indices
    map from keys to values
    Definition HybridGaussianFactorGraph.h:117
    │ │ │ │ -
    EliminateableFactorGraph< This > BaseEliminateable
    for elimination
    Definition HybridGaussianFactorGraph.h:113
    │ │ │ │ -
    HybridGaussianFactorGraph()=default
    Default constructor.
    │ │ │ │ -
    HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridGaussianFactorGraph.h:131
    │ │ │ │ -
    An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition HybridJunctionTree.h:53
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    the error.
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    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
    │ │ │ │ +
    A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
    Definition SymbolicBayesNet.h:32
    │ │ │ │ +
    A Bayes tree that represents the connectivity between variables but is not associated with any probab...
    Definition SymbolicBayesTree.h:51
    │ │ │ │ +
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │ +
    Definition SymbolicEliminationTree.h:29
    │ │ │ │ +
    SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
    Definition SymbolicFactor.h:39
    │ │ │ │ +
    SymbolicFactor FactorType
    Type of factors in factor graph.
    Definition SymbolicFactorGraph.h:38
    │ │ │ │ +
    SymbolicFactorGraph FactorGraphType
    Type of the factor graph (e.g. GaussianFactorGraph)
    Definition SymbolicFactorGraph.h:39
    │ │ │ │ +
    SymbolicEliminationTree EliminationTreeType
    Type of elimination tree.
    Definition SymbolicFactorGraph.h:42
    │ │ │ │ +
    SymbolicBayesNet BayesNetType
    Type of Bayes net from sequential elimination.
    Definition SymbolicFactorGraph.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
    The default ordering generation function.
    Definition SymbolicFactorGraph.h:50
    │ │ │ │ +
    SymbolicJunctionTree JunctionTreeType
    Type of Junction tree.
    Definition SymbolicFactorGraph.h:44
    │ │ │ │ +
    SymbolicBayesTree BayesTreeType
    Type of Bayes tree.
    Definition SymbolicFactorGraph.h:43
    │ │ │ │ +
    SymbolicConditional ConditionalType
    Type of conditionals from elimination.
    Definition SymbolicFactorGraph.h:40
    │ │ │ │ +
    Symbolic Factor Graph.
    Definition SymbolicFactorGraph.h:64
    │ │ │ │ +
    SymbolicFactorGraph(SymbolicFactor &&c)
    Construct from a single factor.
    Definition SymbolicFactorGraph.h:99
    │ │ │ │ +
    SymbolicFactorGraph & operator()(SymbolicFactor &&c)
    Add a single factor and return a reference.
    Definition SymbolicFactorGraph.h:109
    │ │ │ │ +
    SymbolicFactorGraph()
    Construct empty factor graph.
    Definition SymbolicFactorGraph.h:76
    │ │ │ │ +
    SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition SymbolicFactorGraph.h:88
    │ │ │ │ +
    SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > > sharedFactors)
    Constructor that takes an initializer list of shared pointers.
    Definition SymbolicFactorGraph.h:94
    │ │ │ │ +
    void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print
    Definition SymbolicFactorGraph.h:125
    │ │ │ │ +
    EliminateableFactorGraph< This > BaseEliminateable
    Typedef to base elimination class.
    Definition SymbolicFactorGraph.h:69
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition SymbolicFactorGraph.h:70
    │ │ │ │ +
    SymbolicFactorGraph This
    Typedef to this class.
    Definition SymbolicFactorGraph.h:67
    │ │ │ │ +
    SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Construct from iterator over factors.
    Definition SymbolicFactorGraph.h:80
    │ │ │ │ +
    virtual ~SymbolicFactorGraph()
    Destructor.
    Definition SymbolicFactorGraph.h:115
    │ │ │ │ +
    FactorGraph< SymbolicFactor > Base
    Typedef to base factor graph type.
    Definition SymbolicFactorGraph.h:68
    │ │ │ │ +
    SymbolicFactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition SymbolicFactorGraph.h:84
    │ │ │ │ +
    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/a00557_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01208_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/sfm/SfmData.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    HybridFactor.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/Testable.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/sfm/SfmTrack.h>
    │ │ │ │
    26
    │ │ │ │ -
    27#include <cstddef>
    │ │ │ │ -
    28#include <string>
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    31class HybridValues;
    │ │ │ │ -
    32
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    27#include <string>
    │ │ │ │ +
    28#include <vector>
    │ │ │ │ +
    29
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ + │ │ │ │ +
    34
    │ │ │ │ +
    │ │ │ │ +
    39struct GTSAM_EXPORT SfmData {
    │ │ │ │ +
    40 std::vector<SfmCamera> cameras;
    │ │ │ │
    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_;
    │ │ │ │ - │ │ │ │ -
    63
    │ │ │ │ -
    64 public:
    │ │ │ │ -
    65 // typedefs needed to play nice with gtsam
    │ │ │ │ - │ │ │ │ -
    67 typedef boost::shared_ptr<HybridFactor>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    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); }
    │ │ │ │ +
    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
    │ │ │ │ -
    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};
    │ │ │ │ +
    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};
    │ │ │ │
    │ │ │ │ -
    152// HybridFactor
    │ │ │ │ +
    149
    │ │ │ │ +
    151template <>
    │ │ │ │ +
    152struct traits<SfmData> : public Testable<SfmData> {};
    │ │ │ │
    153
    │ │ │ │ -
    154// traits
    │ │ │ │ -
    155template <>
    │ │ │ │ -
    156struct traits<HybridFactor> : public Testable<HybridFactor> {};
    │ │ │ │ -
    157
    │ │ │ │ -
    158} // namespace gtsam
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ -
    The base class for all factors.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    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
    │ │ │ │ +
    Calibration used by Bundler.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    A simple data structure for a track in Structure from Motion.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Values initialCamerasAndPointsEstimate(const SfmData &db)
    This function creates initial values for cameras and points from db.
    Definition SfmData.cpp:449
    │ │ │ │ +
    PinholeCamera< Cal3Bundler > SfmCamera
    Define the structure for the camera poses.
    Definition SfmData.h:33
    │ │ │ │ +
    Values initialCamerasEstimate(const SfmData &db)
    This function creates initial values for cameras from db.
    Definition SfmData.cpp:441
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    a decision tree is a function from assignments to values.
    Definition DecisionTree.h:61
    │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ -
    size_t nrContinuous() const
    Return the number of continuous variables in this factor.
    Definition HybridFactor.h:129
    │ │ │ │ -
    HybridFactor This
    This class.
    Definition HybridFactor.h:66
    │ │ │ │ -
    boost::shared_ptr< HybridFactor > shared_ptr
    shared_ptr to this class
    Definition HybridFactor.h:68
    │ │ │ │ -
    bool isDiscrete() const
    True if this is a factor of discrete variables only.
    Definition HybridFactor.h:120
    │ │ │ │ -
    bool isHybrid() const
    True is this is a Discrete-Continuous factor.
    Definition HybridFactor.h:126
    │ │ │ │ -
    HybridFactor()=default
    Default constructor creates empty factor.
    │ │ │ │ -
    Factor Base
    Our base class.
    Definition HybridFactor.h:69
    │ │ │ │ -
    const KeyVector & continuousKeys() const
    Return only the continuous keys for this factor.
    Definition HybridFactor.h:135
    │ │ │ │ -
    bool isContinuous() const
    True if this is a factor of continuous variables only.
    Definition HybridFactor.h:123
    │ │ │ │ -
    KeyVector continuousKeys_
    Record continuous keys for book-keeping.
    Definition HybridFactor.h:62
    │ │ │ │ -
    virtual ~HybridFactor()=default
    Virtual destructor.
    │ │ │ │ -
    const DiscreteKeys & discreteKeys() const
    Return the discrete keys for this factor.
    Definition HybridFactor.h:132
    │ │ │ │ -
    Definition Factor.h:68
    │ │ │ │ -
    the error.
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a Calibration.
    Definition PinholeCamera.h:33
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    SfmData stores a bunch of SfmTracks.
    Definition SfmData.h:39
    │ │ │ │ +
    const std::vector< SfmCamera > & cameraList() const
    Getters.
    Definition SfmData.h:86
    │ │ │ │ +
    void addCamera(const SfmCamera &cam)
    Add a camera to SfmData.
    Definition SfmData.h:71
    │ │ │ │ +
    size_t numberCameras() const
    The number of cameras.
    Definition SfmData.h:77
    │ │ │ │ +
    size_t numberTracks() const
    The number of reconstructed 3D points.
    Definition SfmData.h:74
    │ │ │ │ +
    const SfmTrack & track(size_t idx) const
    The track formed by series of landmark measurements.
    Definition SfmData.h:80
    │ │ │ │ +
    void addTrack(const SfmTrack &t)
    Add a track to SfmData.
    Definition SfmData.h:68
    │ │ │ │ +
    const SfmCamera & camera(size_t idx) const
    The camera pose at frame index idx
    Definition SfmData.h:83
    │ │ │ │ +
    std::vector< SfmCamera > cameras
    Set of cameras.
    Definition SfmData.h:40
    │ │ │ │ +
    std::vector< SfmTrack > tracks
    Sparse set of points.
    Definition SfmData.h:42
    │ │ │ │ +
    Definition SfmTrack.h:126
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,213 +1,238 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridFactor.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_/_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_/_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 │ │ │ │ │ -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); │ │ │ │ │ +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; │ │ │ │ │ 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 │ │ │ │ │ +_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); } │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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// 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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_._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/a00563_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00884_source.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/linear/KalmanFilter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianMixtureFactor.h
    │ │ │ │ +
    KalmanFilter.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25
    │ │ │ │ +
    26#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
    │ │ │ │ +
    27#define KALMANFILTER_DEFAULT_FACTORIZATION QR
    │ │ │ │ +
    28#endif
    │ │ │ │
    29
    │ │ │ │
    30namespace gtsam {
    │ │ │ │
    31
    │ │ │ │ -
    32class HybridValues;
    │ │ │ │ -
    33class DiscreteValues;
    │ │ │ │ -
    34class VectorValues;
    │ │ │ │ -
    35
    │ │ │ │ -
    │ │ │ │ -
    47class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    │ │ │ │ -
    48 public:
    │ │ │ │ -
    49 using Base = HybridFactor;
    │ │ │ │ - │ │ │ │ -
    51 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    │ │ │ │ +
    41class GTSAM_EXPORT KalmanFilter {
    │ │ │ │ +
    42
    │ │ │ │ +
    43public:
    │ │ │ │ +
    44
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    50 QR, CHOLESKY
    │ │ │ │ +
    51 };
    │ │ │ │ +
    │ │ │ │
    52
    │ │ │ │ -
    53 using sharedFactor = boost::shared_ptr<GaussianFactor>;
    │ │ │ │ -
    54
    │ │ │ │ - │ │ │ │ +
    56 typedef GaussianDensity::shared_ptr State;
    │ │ │ │
    57
    │ │ │ │ -
    58 private:
    │ │ │ │ -
    60 Factors factors_;
    │ │ │ │ -
    61
    │ │ │ │ -
    68 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
    │ │ │ │ -
    69
    │ │ │ │ -
    70 public:
    │ │ │ │ -
    73
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ +
    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) :
    │ │ │ │ + │ │ │ │ +
    75 }
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    130
    │ │ │ │ -
    138 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
    │ │ │ │ -
    139
    │ │ │ │ -
    144 double error(const HybridValues &values) const override;
    │ │ │ │ -
    145
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Decision Tree for use in DiscreteFactors.
    │ │ │ │ -
    Algebraic Decision Trees.
    │ │ │ │ -
    specialized key for discrete variables
    │ │ │ │ - │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    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} // \namespace gtsam
    │ │ │ │ +
    149
    │ │ │ │ +
    150/* ************************************************************************* */
    │ │ │ │ +
    151
    │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    A Gaussian Density.
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    An algebraic decision tree fixes the range of a DecisionTree to double.
    Definition AlgebraicDecisionTree.h:38
    │ │ │ │ - │ │ │ │ -
    DiscreteKeys is a set of keys that can be assembled using the & operator.
    Definition DiscreteKey.h:39
    │ │ │ │ -
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ -
    Implementation of a discrete conditional mixture factor.
    Definition GaussianMixtureFactor.h:47
    │ │ │ │ -
    GaussianMixtureFactor()=default
    Default constructor, mainly for serialization.
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor)
    Add MixtureFactor to a Sum, syntactic sugar.
    Definition GaussianMixtureFactor.h:147
    │ │ │ │ -
    Base class for truly hybrid probabilistic factors.
    Definition HybridFactor.h:52
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    the error.
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    Kalman Filter class.
    Definition KalmanFilter.h:41
    │ │ │ │ +
    Factorization
    This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
    Definition KalmanFilter.h:49
    │ │ │ │ +
    static Key step(const State &p)
    Return step index k, starts at 0, incremented at each predict.
    Definition KalmanFilter.h:92
    │ │ │ │ +
    GaussianDensity::shared_ptr State
    The Kalman filter state is simply a GaussianDensity.
    Definition KalmanFilter.h:56
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,191 +1,167 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianMixtureFactor.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 │ │ │ │ │ -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_/_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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_>; │ │ │ │ │ +_5_6 typedef GaussianDensity::shared_ptr _S_t_a_t_e; │ │ │ │ │ 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; │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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} // \namespace gtsam │ │ │ │ │ +149 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a00566.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00266.html │ │ │ │┄ Files 12% similar despite different names │ │ │ │ @@ -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/discrete/DiscreteLookupDAG.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    HybridFactor.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    DiscreteLookupDAG.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ + │ │ │ │ +

    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.
     
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

    │ │ │ │ -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
    January, 2022
    │ │ │ │ +
    Author
    Frank dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -HybridFactor.cpp File Reference │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +_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_._._. │ │ │ │ │   │ │ │ │ │ -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) │ │ │ │ │ + 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_._._. │ │ │ │ │   │ │ │ │ │ - _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) │ │ │ │ │ +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_ _> │ │ │ │ │   │ │ │ │ │ -_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) │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + January, 2022 │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Frank dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00143.html │ │ │ │┄ Files 17% similar despite different names │ │ │ │ @@ -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/base/serializationTestHelpers.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
    │ │ │ │ -
    Key.h File Reference
    │ │ │ │ +Functions | │ │ │ │ +Variables
    │ │ │ │ +
    serializationTestHelpers.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    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.
     
    │ │ │ │ +template<class 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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ +
    Author
    Alex Cunningham
    │ │ │ │
    │ │ │ │ -Alex Cunningham
    │ │ │ │ -
    Date
    Feb 20, 2012
    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Feb 20, 2012
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    Date
    Feb 7, 2012
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,72 +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 │ │ │ │ │ -Key.h File Reference │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +template │ │ │ │ │ + T  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::ccrreeaattee () │ │ │ │ │   │ │ │ │ │ - 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. │ │ │ │ │ +boost::filesystem::path  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rreesseettFFiilleessyysstteemm │ │ │ │ │ + (boost::filesystem::path folder="actual") │ │ │ │ │   │ │ │ │ │ - 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. │ │ │ │ │ +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()) │ │ │ │ │   │ │ │ │ │ - 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. │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +const bool  vveerrbboossee = false │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Alex Cunningham │ │ │ │ │ - Date │ │ │ │ │ - Feb 20, 2012 │ │ │ │ │ - Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Feb 20, 2012 │ │ │ │ │ + Feb 7, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _K_e_y_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00719_source.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/linear/BinaryJacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    Key.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#pragma once
    │ │ │ │ -
    19
    │ │ │ │ -
    20#include <gtsam/base/FastList.h>
    │ │ │ │ -
    21#include <gtsam/base/FastMap.h>
    │ │ │ │ -
    22#include <gtsam/base/FastSet.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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    87
    │ │ │ │ -
    88// TODO(frank): Nothing fast about these :-(
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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} // namespace gtsam
    │ │ │ │ -
    127
    │ │ │ │ -
    128
    │ │ │ │ -
    129
    │ │ │ │ -
    130
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ -
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    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
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/base/timing.h>
    │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │ +
    28
    │ │ │ │ +
    32template<int M, int N1, int N2>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 utilities.
    │ │ │ │ +
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    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
    │ │ │ │ -
    void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
    Utility function to print one key with optional prefix.
    Definition Key.cpp:40
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
    Definition Key.h:58
    │ │ │ │ -
    Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter,...
    Definition Key.h:72
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
    Update an off diagonal block.
    Definition SymmetricBlockMatrix.h:228
    │ │ │ │ +
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ +
    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
    │ │ │ │ +
    DenseIndex nBlocks() const
    Block count.
    Definition SymmetricBlockMatrix.h:120
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    const Matrix & matrix() const
    Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
    Definition VerticalBlockMatrix.h:188
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    const_iterator begin() const
    Iterator at beginning of involved variable keys.
    Definition Factor.h:143
    │ │ │ │ +
    const_iterator end() const
    Iterator at end of involved variable keys.
    Definition Factor.h:146
    │ │ │ │ +
    A binary JacobianFactor specialization that uses fixed matrix math for speed.
    Definition BinaryJacobianFactor.h:33
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ +
    const SharedDiagonal & get_model() const
    get a copy of model
    Definition JacobianFactor.h:291
    │ │ │ │ +
    constABlock getA() const
    Get a view of the A matrix, not weighted by noise.
    Definition JacobianFactor.h:303
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,187 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Key.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#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); │ │ │ │ │ -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 │ │ │ │ │ -127 │ │ │ │ │ -128 │ │ │ │ │ -129 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _K_e_y_._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/a00602.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01277.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/slam/RegularImplicitSchurFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    EliminateableFactorGraph.h File Reference
    │ │ │ │ +
    RegularImplicitSchurFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Variable elimination algorithms for factor graphs. │ │ │ │ +

    A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    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::RegularImplicitSchurFactor< CAMERA >
     RegularImplicitSchurFactor. More...
     
    class  gtsam::EliminateableFactorGraph< FACTOR_GRAPH >
     EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
    struct  gtsam::traits< RegularImplicitSchurFactor< CAMERA > >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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
    │ │ │ │ +

    A subclass of GaussianFactor specialized to structureless SFM.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Luca Carlone
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -EliminateableFactorGraph.h File Reference │ │ │ │ │ -Variable elimination algorithms for factor graphs. _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 │ │ │ │ │ -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_:_:_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_:_:_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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ +A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Apr 21, 2013 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00887_source.html │ │ │ │┄ Files 22% similar despite different names │ │ │ │ @@ -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/linear/HessianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    EliminateableFactorGraph.h
    │ │ │ │ +
    HessianFactor.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 <boost/shared_ptr.hpp>
    │ │ │ │ -
    22#include <functional>
    │ │ │ │ -
    23#include <boost/variant.hpp>
    │ │ │ │ -
    24#include <boost/optional.hpp>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    25
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    34 template<class GRAPH>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    57 {
    │ │ │ │ -
    58 private:
    │ │ │ │ - │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ -
    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> >
    │ │ │ │ - │ │ │ │ -
    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> >
    │ │ │ │ - │ │ │ │ -
    192 const KeyVector& variables,
    │ │ │ │ -
    193 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
    │ │ │ │ -
    194 OptionalVariableIndex variableIndex = boost::none) const;
    │ │ │ │ +
    26#include <boost/make_shared.hpp>
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │ +
    29
    │ │ │ │ +
    30 // Forward declarations
    │ │ │ │ +
    31 class Ordering;
    │ │ │ │ +
    32 class JacobianFactor;
    │ │ │ │ +
    33 class HessianFactor;
    │ │ │ │ + │ │ │ │ +
    35 class GaussianBayesNet;
    │ │ │ │ +
    36 class GaussianFactorGraph;
    │ │ │ │ +
    37
    │ │ │ │ +
    │ │ │ │ +
    101 class GTSAM_EXPORT HessianFactor : public GaussianFactor {
    │ │ │ │ +
    102 protected:
    │ │ │ │ +
    103
    │ │ │ │ + │ │ │ │ +
    105
    │ │ │ │ +
    106 public:
    │ │ │ │ +
    107
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    110 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    111 typedef SymmetricBlockMatrix::Block Block;
    │ │ │ │ +
    112 typedef SymmetricBlockMatrix::constBlock constBlock;
    │ │ │ │ +
    113
    │ │ │ │ +
    114
    │ │ │ │ + │ │ │ │ +
    117
    │ │ │ │ +
    123 HessianFactor(Key j, const Matrix& G, const Vector& g, double f);
    │ │ │ │ +
    124
    │ │ │ │ +
    128 HessianFactor(Key j, const Vector& mu, const Matrix& Sigma);
    │ │ │ │ +
    129
    │ │ │ │ +
    145 HessianFactor(Key j1, Key j2,
    │ │ │ │ +
    146 const Matrix& G11, const Matrix& G12, const Vector& g1,
    │ │ │ │ +
    147 const Matrix& G22, const Vector& g2, double f);
    │ │ │ │ +
    148
    │ │ │ │ +
    153 HessianFactor(Key j1, Key j2, Key j3,
    │ │ │ │ +
    154 const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
    │ │ │ │ +
    155 const Matrix& G22, const Matrix& G23, const Vector& g2,
    │ │ │ │ +
    156 const Matrix& G33, const Vector& g3, double f);
    │ │ │ │ +
    157
    │ │ │ │ +
    162 HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
    │ │ │ │ +
    163 const std::vector<Vector>& gs, double f);
    │ │ │ │ +
    164
    │ │ │ │ +
    167 template<typename KEYS>
    │ │ │ │ +
    168 HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation);
    │ │ │ │ +
    169
    │ │ │ │ +
    171 explicit HessianFactor(const JacobianFactor& cg);
    │ │ │ │ +
    172
    │ │ │ │ +
    175 explicit HessianFactor(const GaussianFactor& factor);
    │ │ │ │ +
    176
    │ │ │ │ +
    178 explicit HessianFactor(const GaussianFactorGraph& factors,
    │ │ │ │ +
    179 const Scatter& scatter);
    │ │ │ │ +
    180
    │ │ │ │ +
    │ │ │ │ +
    182 explicit HessianFactor(const GaussianFactorGraph& factors)
    │ │ │ │ +
    183 : HessianFactor(factors, Scatter(factors)) {}
    │ │ │ │ +
    │ │ │ │ +
    184
    │ │ │ │ +
    186 ~HessianFactor() override {}
    │ │ │ │ +
    187
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    190 return boost::make_shared<HessianFactor>(*this); }
    │ │ │ │ +
    │ │ │ │ +
    191
    │ │ │ │ +
    193 void print(const std::string& s = "",
    │ │ │ │ +
    194 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
    │ │ │ │
    195
    │ │ │ │ -
    200 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
    │ │ │ │ - │ │ │ │ -
    202 const Ordering& ordering,
    │ │ │ │ -
    203 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
    │ │ │ │ -
    204 OptionalVariableIndex variableIndex = boost::none) const;
    │ │ │ │ -
    205
    │ │ │ │ -
    210 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    197 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
    │ │ │ │ +
    198
    │ │ │ │ +
    200 using GaussianFactor::error;
    │ │ │ │ +
    201
    │ │ │ │ +
    206 double error(const VectorValues& c) const override;
    │ │ │ │ +
    207
    │ │ │ │ +
    │ │ │ │ +
    213 DenseIndex getDim(const_iterator variable) const override {
    │ │ │ │ +
    214 return info_.getDim(std::distance(begin(), variable));
    │ │ │ │ +
    215 }
    │ │ │ │ +
    │ │ │ │ +
    216
    │ │ │ │ +
    218 size_t rows() const { return info_.rows(); }
    │ │ │ │ +
    219
    │ │ │ │ +
    225 GaussianFactor::shared_ptr negate() const override;
    │ │ │ │ +
    226
    │ │ │ │ +
    │ │ │ │ +
    230 double constantTerm() const {
    │ │ │ │ +
    231 const auto view = info_.diagonalBlock(size());
    │ │ │ │ +
    232 return view(0, 0);
    │ │ │ │ +
    233 }
    │ │ │ │ +
    │ │ │ │ +
    234
    │ │ │ │ +
    238 double& constantTerm() { return info_.diagonalBlock(size())(0, 0); }
    │ │ │ │ +
    239
    │ │ │ │ +
    │ │ │ │ +
    244 SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
    │ │ │ │ +
    245 assert(!empty());
    │ │ │ │ +
    246 return info_.aboveDiagonalBlock(j - begin(), size());
    │ │ │ │ +
    247 }
    │ │ │ │ +
    │ │ │ │ +
    248
    │ │ │ │ +
    │ │ │ │ +
    251 SymmetricBlockMatrix::constBlock linearTerm() const {
    │ │ │ │ +
    252 assert(!empty());
    │ │ │ │ +
    253 // get the last column (except the bottom right block)
    │ │ │ │ +
    254 return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    │ │ │ │ +
    255 }
    │ │ │ │ +
    │ │ │ │ +
    256
    │ │ │ │ +
    │ │ │ │ +
    259 SymmetricBlockMatrix::Block linearTerm() {
    │ │ │ │ +
    260 assert(!empty());
    │ │ │ │ +
    261 return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    │ │ │ │ +
    262 }
    │ │ │ │ +
    │ │ │ │ +
    263
    │ │ │ │ +
    265 const SymmetricBlockMatrix& info() const { return info_; }
    │ │ │ │ +
    266
    │ │ │ │ +
    269 SymmetricBlockMatrix& info() { return info_; }
    │ │ │ │ +
    270
    │ │ │ │ +
    286 Matrix augmentedInformation() const override;
    │ │ │ │ +
    287
    │ │ │ │ +
    289 Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper> informationView() const;
    │ │ │ │ +
    290
    │ │ │ │ +
    294 Matrix information() const override;
    │ │ │ │ +
    295
    │ │ │ │ +
    297 void hessianDiagonalAdd(VectorValues& d) const override;
    │ │ │ │ +
    298
    │ │ │ │ +
    300 using Base::hessianDiagonal;
    │ │ │ │ +
    301
    │ │ │ │ +
    303 void hessianDiagonal(double* d) const override;
    │ │ │ │ +
    304
    │ │ │ │ +
    306 std::map<Key,Matrix> hessianBlockDiagonal() const override;
    │ │ │ │ +
    307
    │ │ │ │ +
    309 std::pair<Matrix, Vector> jacobian() const override;
    │ │ │ │ +
    310
    │ │ │ │ +
    316 Matrix augmentedJacobian() const override;
    │ │ │ │
    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}
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ - │ │ │ │ +
    323 void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
    │ │ │ │ +
    324
    │ │ │ │ +
    │ │ │ │ +
    328 void updateHessian(HessianFactor* other) const {
    │ │ │ │ +
    329 assert(other);
    │ │ │ │ +
    330 updateHessian(other->keys_, &other->info_);
    │ │ │ │ +
    331 }
    │ │ │ │ +
    │ │ │ │ +
    332
    │ │ │ │ +
    334 void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const override;
    │ │ │ │ +
    335
    │ │ │ │ +
    337 VectorValues gradientAtZero() const override;
    │ │ │ │ +
    338
    │ │ │ │ +
    340 void gradientAtZero(double* d) const override;
    │ │ │ │ +
    341
    │ │ │ │ +
    346 Vector gradient(Key key, const VectorValues& x) const override;
    │ │ │ │ +
    347
    │ │ │ │ +
    352 boost::shared_ptr<GaussianConditional> eliminateCholesky(const Ordering& keys);
    │ │ │ │ +
    353
    │ │ │ │ +
    355 VectorValues solve();
    │ │ │ │ +
    356
    │ │ │ │ +
    357 private:
    │ │ │ │ +
    359 void Allocate(const Scatter& scatter);
    │ │ │ │ +
    360
    │ │ │ │ +
    362 HessianFactor(const Scatter& scatter);
    │ │ │ │ +
    363
    │ │ │ │ +
    364 friend class NonlinearFactorGraph;
    │ │ │ │ +
    365 friend class NonlinearClusterTree;
    │ │ │ │ +
    366
    │ │ │ │ +
    368 friend class boost::serialization::access;
    │ │ │ │ +
    369 template<class ARCHIVE>
    │ │ │ │ +
    370 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ +
    371 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
    │ │ │ │ +
    372 ar & BOOST_SERIALIZATION_NVP(info_);
    │ │ │ │ +
    373 }
    │ │ │ │ +
    374 };
    │ │ │ │ +
    │ │ │ │ +
    375
    │ │ │ │ +
    392GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
    │ │ │ │ +
    393 EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
    │ │ │ │ +
    394
    │ │ │ │ +
    410GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
    │ │ │ │ +
    411 EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
    │ │ │ │ +
    412
    │ │ │ │ +
    414template<>
    │ │ │ │ +
    415struct traits<HessianFactor> : public Testable<HessianFactor> {};
    │ │ │ │ +
    416
    │ │ │ │ +
    417} // \ namespace gtsam
    │ │ │ │ +
    418
    │ │ │ │ +
    419
    │ │ │ │ + │ │ │ │ +
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ +
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ +
    Maps global variable indices to slot indices.
    │ │ │ │ +
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ +
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
    Definition EliminateableFactorGraph.h:36
    │ │ │ │ -
    EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
    Definition EliminateableFactorGraph.h:57
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::JunctionTreeType JunctionTreeType
    Junction tree type that can do multifrontal elimination of this graph.
    Definition EliminateableFactorGraph.h:82
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::BayesTreeType BayesTreeType
    Bayes tree type produced by multifrontal elimination.
    Definition EliminateableFactorGraph.h:79
    │ │ │ │ -
    EliminationTraitsType::BayesNetType BayesNetType
    Bayes net type produced by sequential elimination.
    Definition EliminateableFactorGraph.h:73
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraitsType::ConditionalType ConditionalType
    Conditional type stored in the Bayes net produced by elimination.
    Definition EliminateableFactorGraph.h:70
    │ │ │ │ -
    EliminationTraitsType::EliminationTreeType EliminationTreeType
    Elimination tree type that can do sequential elimination of this graph.
    Definition EliminateableFactorGraph.h:76
    │ │ │ │ -
    boost::optional< const VariableIndex & > OptionalVariableIndex
    Typedef for an optional variable index as an argument to elimination functions.
    Definition EliminateableFactorGraph.h:92
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    EliminationTraits< FactorGraphType > EliminationTraitsType
    Typedef to the specific EliminationTraits for this graph.
    Definition EliminateableFactorGraph.h:67
    │ │ │ │ -
    boost::optional< Ordering::OrderingType > OptionalOrderingType
    Typedef for an optional ordering type.
    Definition EliminateableFactorGraph.h:95
    │ │ │ │ +
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ +
    DenseIndex getDim(DenseIndex block) const
    Number of dimensions for variable on this diagonal block.
    Definition SymmetricBlockMatrix.h:123
    │ │ │ │ +
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ +
    DenseIndex rows() const
    Row size.
    Definition SymmetricBlockMatrix.h:114
    │ │ │ │ +
    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
    │ │ │ │ +
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ +
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ +
    HessianFactor This
    Typedef to this class.
    Definition HessianFactor.h:109
    │ │ │ │ +
    void updateHessian(HessianFactor *other) const
    Update another Hessian factor.
    Definition HessianFactor.h:328
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    A shared_ptr to this class.
    Definition HessianFactor.h:110
    │ │ │ │ +
    GaussianFactor::shared_ptr clone() const override
    Clone this HessianFactor.
    Definition HessianFactor.h:189
    │ │ │ │ +
    const SymmetricBlockMatrix & info() const
    Return underlying information matrix.
    Definition HessianFactor.h:265
    │ │ │ │ +
    SymmetricBlockMatrix::Block linearTerm()
    Return the complete linear term as described above.
    Definition HessianFactor.h:259
    │ │ │ │ +
    double constantTerm() const
    Return the constant term as described above.
    Definition HessianFactor.h:230
    │ │ │ │ +
    SymmetricBlockMatrix::constBlock linearTerm() const
    Return the complete linear term as described above.
    Definition HessianFactor.h:251
    │ │ │ │ +
    ~HessianFactor() override
    Destructor.
    Definition HessianFactor.h:186
    │ │ │ │ +
    HessianFactor(const GaussianFactorGraph &factors)
    Combine a set of factors into a single dense HessianFactor.
    Definition HessianFactor.h:182
    │ │ │ │ +
    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 HessianFactor.h:213
    │ │ │ │ +
    SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const
    Return the part of linear term as described above corresponding to the requested variable.
    Definition HessianFactor.h:244
    │ │ │ │ +
    SymmetricBlockMatrix info_
    The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
    Definition HessianFactor.h:104
    │ │ │ │ +
    GaussianFactor Base
    Typedef to base class.
    Definition HessianFactor.h:108
    │ │ │ │ +
    size_t rows() const
    Return the number of columns and rows of the Hessian matrix, including the information vector.
    Definition HessianFactor.h:218
    │ │ │ │ +
    SymmetricBlockMatrix::constBlock constBlock
    A block from the Hessian matrix (const version)
    Definition HessianFactor.h:112
    │ │ │ │ +
    SymmetricBlockMatrix & info()
    Return non-const information matrix.
    Definition HessianFactor.h:269
    │ │ │ │ +
    SymmetricBlockMatrix::Block Block
    A block from the Hessian matrix.
    Definition HessianFactor.h:111
    │ │ │ │ +
    double & constantTerm()
    Return the constant term as described above.
    Definition HessianFactor.h:238
    │ │ │ │ +
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ +
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ +
    is the normalization constant.
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminateableFactorGraph.h │ │ │ │ │ +HessianFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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,332 +16,398 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +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_/_S_c_a_t_t_e_r_._h> │ │ │ │ │ +23#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> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ 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> │ │ │ │ │ -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; │ │ │ │ │ -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; │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30 // Forward declarations │ │ │ │ │ +31 class Ordering; │ │ │ │ │ +32 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ +33 class HessianFactor; │ │ │ │ │ +34 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +35 class GaussianBayesNet; │ │ │ │ │ +36 class GaussianFactorGraph; │ │ │ │ │ +37 │ │ │ │ │ +_1_0_1 class GTSAM_EXPORT _H_e_s_s_i_a_n_F_a_c_t_o_r : public _G_a_u_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +102 protected: │ │ │ │ │ +103 │ │ │ │ │ +_1_0_4 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _i_n_f_o__; │ │ │ │ │ +105 │ │ │ │ │ +106 public: │ │ │ │ │ +107 │ │ │ │ │ +_1_0_8 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +_1_0_9 typedef _H_e_s_s_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_1_1_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_1_1_1 typedef SymmetricBlockMatrix::Block _B_l_o_c_k; │ │ │ │ │ +_1_1_2 typedef SymmetricBlockMatrix::constBlock _c_o_n_s_t_B_l_o_c_k; │ │ │ │ │ +113 │ │ │ │ │ +114 │ │ │ │ │ +116 _H_e_s_s_i_a_n_F_a_c_t_o_r(); │ │ │ │ │ +117 │ │ │ │ │ +123 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j, const Matrix& G, const Vector& g, double f); │ │ │ │ │ +124 │ │ │ │ │ +128 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j, const Vector& mu, const Matrix& Sigma); │ │ │ │ │ +129 │ │ │ │ │ +145 _H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, │ │ │ │ │ +146 const Matrix& G11, const Matrix& G12, const Vector& g1, │ │ │ │ │ +147 const Matrix& G22, const Vector& g2, double f); │ │ │ │ │ +148 │ │ │ │ │ +153 _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, │ │ │ │ │ +154 const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, │ │ │ │ │ +155 const Matrix& G22, const Matrix& G23, const Vector& g2, │ │ │ │ │ +156 const Matrix& G33, const Vector& g3, double f); │ │ │ │ │ +157 │ │ │ │ │ +162 _H_e_s_s_i_a_n_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& js, const std::vector& Gs, │ │ │ │ │ +163 const std::vector& gs, double f); │ │ │ │ │ +164 │ │ │ │ │ +167 template │ │ │ │ │ +168 _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); │ │ │ │ │ +169 │ │ │ │ │ +171 explicit _H_e_s_s_i_a_n_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& cg); │ │ │ │ │ +172 │ │ │ │ │ +175 explicit _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& factor); │ │ │ │ │ +176 │ │ │ │ │ +178 explicit _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, │ │ │ │ │ +179 const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ +180 │ │ │ │ │ +_1_8_2 explicit _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) │ │ │ │ │ +183 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors, _S_c_a_t_t_e_r(factors)) {} │ │ │ │ │ +184 │ │ │ │ │ +_1_8_6 _~_H_e_s_s_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::make_shared(*this); } │ │ │ │ │ +191 │ │ │ │ │ +193 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +194 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ 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; │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ +200 using GaussianFactor::error; │ │ │ │ │ +201 │ │ │ │ │ +206 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const override; │ │ │ │ │ +207 │ │ │ │ │ +_2_1_3 _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 { │ │ │ │ │ +214 return info_._g_e_t_D_i_m(std::distance(begin(), variable)); │ │ │ │ │ +215 } │ │ │ │ │ +216 │ │ │ │ │ +_2_1_8 size_t _r_o_w_s() const { return info_._r_o_w_s(); } │ │ │ │ │ +219 │ │ │ │ │ +225 _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; │ │ │ │ │ +226 │ │ │ │ │ +_2_3_0 double _c_o_n_s_t_a_n_t_T_e_r_m() const { │ │ │ │ │ +231 const auto view = info_._d_i_a_g_o_n_a_l_B_l_o_c_k(size()); │ │ │ │ │ +232 return view(0, 0); │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +_2_3_8 double& _c_o_n_s_t_a_n_t_T_e_r_m() { return info_._d_i_a_g_o_n_a_l_B_l_o_c_k(size())(0, 0); } │ │ │ │ │ +239 │ │ │ │ │ +_2_4_4 SymmetricBlockMatrix::constBlock _l_i_n_e_a_r_T_e_r_m(_c_o_n_s_t___i_t_e_r_a_t_o_r j) const { │ │ │ │ │ +245 assert(!empty()); │ │ │ │ │ +246 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j - begin(), size()); │ │ │ │ │ +247 } │ │ │ │ │ +248 │ │ │ │ │ +_2_5_1 SymmetricBlockMatrix::constBlock _l_i_n_e_a_r_T_e_r_m() const { │ │ │ │ │ +252 assert(!empty()); │ │ │ │ │ +253 // get the last column (except the bottom right block) │ │ │ │ │ +254 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(0, size(), size(), size() + 1); │ │ │ │ │ +255 } │ │ │ │ │ +256 │ │ │ │ │ +_2_5_9 SymmetricBlockMatrix::Block _l_i_n_e_a_r_T_e_r_m() { │ │ │ │ │ +260 assert(!empty()); │ │ │ │ │ +261 return info_._a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(0, size(), size(), size() + 1); │ │ │ │ │ +262 } │ │ │ │ │ +263 │ │ │ │ │ +_2_6_5 const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _i_n_f_o() const { return info_; } │ │ │ │ │ +266 │ │ │ │ │ +_2_6_9 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _i_n_f_o() { return info_; } │ │ │ │ │ +270 │ │ │ │ │ +286 Matrix augmentedInformation() const override; │ │ │ │ │ +287 │ │ │ │ │ +289 Eigen::SelfAdjointView │ │ │ │ │ +informationView() const; │ │ │ │ │ +290 │ │ │ │ │ +294 Matrix information() const override; │ │ │ │ │ +295 │ │ │ │ │ +297 void hessianDiagonalAdd(_V_e_c_t_o_r_V_a_l_u_e_s& d) const override; │ │ │ │ │ +298 │ │ │ │ │ +300 using Base::hessianDiagonal; │ │ │ │ │ +301 │ │ │ │ │ +303 void hessianDiagonal(double* d) const override; │ │ │ │ │ +304 │ │ │ │ │ +306 std::map hessianBlockDiagonal() const override; │ │ │ │ │ +307 │ │ │ │ │ +309 std::pair jacobian() const override; │ │ │ │ │ +310 │ │ │ │ │ +316 Matrix augmentedJacobian() const override; │ │ │ │ │ 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} │ │ │ │ │ -_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 │ │ │ │ │ +323 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; │ │ │ │ │ +324 │ │ │ │ │ +_3_2_8 void _u_p_d_a_t_e_H_e_s_s_i_a_n(_H_e_s_s_i_a_n_F_a_c_t_o_r* other) const { │ │ │ │ │ +329 assert(other); │ │ │ │ │ +330 updateHessian(other->_k_e_y_s__, &other->_i_n_f_o__); │ │ │ │ │ +331 } │ │ │ │ │ +332 │ │ │ │ │ +334 void multiplyHessianAdd(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 override; │ │ │ │ │ +335 │ │ │ │ │ +337 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const override; │ │ │ │ │ +338 │ │ │ │ │ +340 void gradientAtZero(double* d) const override; │ │ │ │ │ +341 │ │ │ │ │ +346 Vector gradient(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override; │ │ │ │ │ +347 │ │ │ │ │ +352 boost::shared_ptr eliminateCholesky(const _O_r_d_e_r_i_n_g& │ │ │ │ │ +keys); │ │ │ │ │ +353 │ │ │ │ │ +355 _V_e_c_t_o_r_V_a_l_u_e_s solve(); │ │ │ │ │ +356 │ │ │ │ │ +357 private: │ │ │ │ │ +359 void Allocate(const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ +360 │ │ │ │ │ +362 _H_e_s_s_i_a_n_F_a_c_t_o_r(const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ +363 │ │ │ │ │ +364 friend class _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +365 friend class NonlinearClusterTree; │ │ │ │ │ +366 │ │ │ │ │ +_3_6_8 friend class boost::serialization::access; │ │ │ │ │ +369 template │ │ │ │ │ +370 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +371 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_G_a_u_s_s_i_a_n_F_a_c_t_o_r); │ │ │ │ │ +372 ar & BOOST_SERIALIZATION_NVP(info_); │ │ │ │ │ +373 } │ │ │ │ │ +374 }; │ │ │ │ │ +375 │ │ │ │ │ +392GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +393 _E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y(const GaussianFactorGraph& factors, const Ordering& │ │ │ │ │ +keys); │ │ │ │ │ +394 │ │ │ │ │ +410GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +411 _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y(const GaussianFactorGraph& factors, const Ordering& │ │ │ │ │ +keys); │ │ │ │ │ +412 │ │ │ │ │ +414template<> │ │ │ │ │ +_4_1_5struct _t_r_a_i_t_s<_H_e_s_s_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +416 │ │ │ │ │ +417} // \ namespace gtsam │ │ │ │ │ +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. │ │ │ │ │ +_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 │ │ │ │ │ +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_:_:_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_:_:_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_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +HessianFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ +void updateHessian(HessianFactor *other) const │ │ │ │ │ +Update another Hessian factor. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:328 │ │ │ │ │ +_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_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +GaussianFactor::shared_ptr clone() const override │ │ │ │ │ +Clone this HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o │ │ │ │ │ +const SymmetricBlockMatrix & info() const │ │ │ │ │ +Return underlying information matrix. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:265 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ +SymmetricBlockMatrix::Block linearTerm() │ │ │ │ │ +Return the complete linear term as described above. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:259 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_a_n_t_T_e_r_m │ │ │ │ │ +double constantTerm() const │ │ │ │ │ +Return the constant term as described above. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ +SymmetricBlockMatrix::constBlock linearTerm() const │ │ │ │ │ +Return the complete linear term as described above. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:251 │ │ │ │ │ +_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() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:186 │ │ │ │ │ +_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(const GaussianFactorGraph &factors) │ │ │ │ │ +Combine a set of factors into a single dense HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_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 HessianFactor.h:213 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_T_e_r_m │ │ │ │ │ +SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const │ │ │ │ │ +Return the part of linear term as described above corresponding to the │ │ │ │ │ +requested variable. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:244 │ │ │ │ │ +_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_:_:_B_a_s_e │ │ │ │ │ +GaussianFactor Base │ │ │ │ │ +Typedef to base class. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_r_o_w_s │ │ │ │ │ +size_t rows() const │ │ │ │ │ +Return the number of columns and rows of the Hessian matrix, including the │ │ │ │ │ +information vector. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_B_l_o_c_k │ │ │ │ │ +SymmetricBlockMatrix::constBlock constBlock │ │ │ │ │ +A block from the Hessian matrix (const version) │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o │ │ │ │ │ +SymmetricBlockMatrix & info() │ │ │ │ │ +Return non-const information matrix. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:269 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_B_l_o_c_k │ │ │ │ │ +SymmetricBlockMatrix::Block Block │ │ │ │ │ +A block from the Hessian matrix. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_c_o_n_s_t_a_n_t_T_e_r_m │ │ │ │ │ +double & constantTerm() │ │ │ │ │ +Return the constant term as described above. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:238 │ │ │ │ │ +_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_:_:_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_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_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. │ │ │ │ │ * _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 │ │ │ │ │ + * _l_i_n_e_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/a00611_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00320_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/geometry/PinholePose.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    FactorGraph.h
    │ │ │ │ +
    PinholePose.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// \callgraph
    │ │ │ │ -
    22
    │ │ │ │ -
    23#pragma once
    │ │ │ │ -
    24
    │ │ │ │ - │ │ │ │ -
    26#include <gtsam/inference/Key.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
    │ │ │ │ -
    42namespace gtsam {
    │ │ │ │ -
    44typedef FastVector<FactorIndex> FactorIndices;
    │ │ │ │ -
    45
    │ │ │ │ -
    46// Forward declarations
    │ │ │ │ -
    47template <class CLIQUE>
    │ │ │ │ -
    48class BayesTree;
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    24#include <boost/make_shared.hpp>
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    33template<typename CALIBRATION>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    35
    │ │ │ │ +
    36private:
    │ │ │ │ +
    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;
    │ │ │ │ +
    42
    │ │ │ │ +
    43public:
    │ │ │ │ +
    44
    │ │ │ │ +
    45 typedef CALIBRATION CalibrationType;
    │ │ │ │ +
    46
    │ │ │ │
    49
    │ │ │ │ -
    50class HybridValues;
    │ │ │ │ -
    51
    │ │ │ │ -
    53template <class C>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    98 public:
    │ │ │ │ -
    99 typedef FACTOR FactorType;
    │ │ │ │ -
    100 typedef boost::shared_ptr<FACTOR>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    132
    │ │ │ │ -
    │ │ │ │ -
    134 bool isEqual(const FactorGraph& other) const {
    │ │ │ │ -
    135 return factors_ == other.factors_;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    52 }
    │ │ │ │ +
    │ │ │ │ +
    53
    │ │ │ │ +
    │ │ │ │ +
    55 explicit PinholeBaseK(const Pose3& 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 }
    │ │ │ │ +
    │ │ │ │ +
    87
    │ │ │ │ +
    88
    │ │ │ │ +
    95 template <class POINT>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    98 OptionalJacobian<2, DimK> Dcal) const {
    │ │ │ │ +
    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;
    │ │ │ │ +
    113
    │ │ │ │ +
    114 return pi;
    │ │ │ │ +
    115 }
    │ │ │ │ +
    │ │ │ │ +
    116
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    133 OptionalJacobian<2, 2> Dpoint = boost::none,
    │ │ │ │ +
    134 OptionalJacobian<2, DimK> Dcal = boost::none) const {
    │ │ │ │ +
    135 return _project(pw, Dpose, Dpoint, Dcal);
    │ │ │ │
    136 }
    │ │ │ │
    │ │ │ │
    137
    │ │ │ │ -
    140
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    166 return result;
    │ │ │ │ +
    167 }
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ +
    168
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    232};
    │ │ │ │ +
    │ │ │ │ +
    233// end of class PinholeBaseK
    │ │ │ │ +
    234
    │ │ │ │ +
    242template<typename CALIBRATION>
    │ │ │ │ +
    │ │ │ │ +
    243class PinholePose: public PinholeBaseK<CALIBRATION> {
    │ │ │ │
    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>
    │ │ │ │ +
    245private:
    │ │ │ │ +
    246
    │ │ │ │ + │ │ │ │ +
    248 boost::shared_ptr<CALIBRATION> K_;
    │ │ │ │ +
    249
    │ │ │ │ +
    250public:
    │ │ │ │ +
    251
    │ │ │ │ +
    252 enum {
    │ │ │ │ +
    253 dimension = 6
    │ │ │ │ +
    254 };
    │ │ │ │ +
    255
    │ │ │ │ +
    258
    │ │ │ │
    │ │ │ │ -
    260 HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
    │ │ │ │ -
    261 push_back(container.begin(), container.end());
    │ │ │ │ -
    262 }
    │ │ │ │ + │ │ │ │ +
    261 }
    │ │ │ │ +
    │ │ │ │ +
    262
    │ │ │ │ +
    │ │ │ │ +
    264 explicit PinholePose(const Pose3& pose) :
    │ │ │ │ +
    265 Base(pose), K_(new CALIBRATION()) {
    │ │ │ │ +
    266 }
    │ │ │ │
    │ │ │ │ -
    263
    │ │ │ │ -
    268 template <class FACTOR_OR_CONTAINER>
    │ │ │ │ +
    267
    │ │ │ │
    │ │ │ │ -
    269 void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
    │ │ │ │ -
    270 push_back(factorOrContainer);
    │ │ │ │ +
    269 PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
    │ │ │ │ +
    270 Base(pose), K_(K) {
    │ │ │ │
    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(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    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(); }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    362 sharedFactor back() const { return factors_.back(); }
    │ │ │ │ +
    361 ~PinholePose() override {
    │ │ │ │ +
    362 }
    │ │ │ │
    363
    │ │ │ │ -
    365 double error(const HybridValues &values) const;
    │ │ │ │ -
    366
    │ │ │ │ -
    370
    │ │ │ │ -
    372 iterator begin() { return factors_.begin(); }
    │ │ │ │ +
    │ │ │ │ +
    365 const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
    │ │ │ │ +
    366 return K_;
    │ │ │ │ +
    367 }
    │ │ │ │ +
    │ │ │ │ +
    368
    │ │ │ │ +
    │ │ │ │ +
    370 const CALIBRATION& calibration() const override {
    │ │ │ │ +
    371 return *K_;
    │ │ │ │ +
    372 }
    │ │ │ │ +
    │ │ │ │
    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; }
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    380 OptionalJacobian<2, 3> Dpoint = boost::none) const {
    │ │ │ │ +
    381 return Base::project(pw, Dpose, Dpoint);
    │ │ │ │ +
    382 }
    │ │ │ │ +
    │ │ │ │ +
    383
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    386 OptionalJacobian<2, 2> Dpoint = boost::none) const {
    │ │ │ │ +
    387 return Base::project(pw, Dpose, Dpoint);
    │ │ │ │ +
    388 }
    │ │ │ │ +
    │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ -
    439 template <class ARCHIVE>
    │ │ │ │ -
    440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    │ │ │ │ -
    441 ar& BOOST_SERIALIZATION_NVP(factors_);
    │ │ │ │ -
    442 }
    │ │ │ │ -
    443
    │ │ │ │ -
    445}; // FactorGraph
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    427 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
    │ │ │ │ +
    428 }
    │ │ │ │ +
    │ │ │ │ +
    430
    │ │ │ │ +
    431private:
    │ │ │ │ +
    432
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    445};
    │ │ │ │
    │ │ │ │ -
    446} // namespace gtsam
    │ │ │ │ +
    446// end of class PinholePose
    │ │ │ │
    447
    │ │ │ │ - │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ - │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Graphviz formatter.
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Calibrated camera for which only pose is unknown.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    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
    │ │ │ │ -
    bool isEqual(const FactorGraph &other) const
    Check exact equality of the factor pointers. Useful for derived ==.
    Definition FactorGraph.h:134
    │ │ │ │ -
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ -
    bool empty() const
    Check if the graph is empty (null factors set by remove() will cause this to return false).
    Definition FactorGraph.h:330
    │ │ │ │ -
    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
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ -
    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
    │ │ │ │ -
    iterator erase(iterator item)
    Erase factor and rearrange other factors to take up the empty space.
    Definition FactorGraph.h:391
    │ │ │ │ -
    void add(const FACTOR_OR_CONTAINER &factorOrContainer)
    Add a factor or container of factors, including STL collections, BayesTrees, etc.
    Definition FactorGraph.h:269
    │ │ │ │ -
    iterator erase(iterator first, iterator last)
    Erase factors and rearrange other factors to take up the empty space.
    Definition FactorGraph.h:394
    │ │ │ │ -
    sharedFactor back() const
    Get the last factor.
    Definition FactorGraph.h:362
    │ │ │ │ -
    FactorGraph(const CONTAINER &factors)
    Construct from container of factors (shared_ptr or plain objects)
    Definition FactorGraph.h:152
    │ │ │ │ -
    void remove(size_t i)
    delete factor without re-arranging indexes by inserting a nullptr pointer
    Definition FactorGraph.h:385
    │ │ │ │ -
    KeyVector keyVector() const
    Potentially slow function to return all keys involved, sorted, as a vector.
    Definition FactorGraph-inst.h:95
    │ │ │ │ -
    HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
    Push back non-pointer objects in a container (factors are copied).
    Definition FactorGraph.h:260
    │ │ │ │ -
    FactorGraph()
    Default constructor.
    Definition FactorGraph.h:142
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
    Emplace a shared pointer to factor of given type.
    Definition FactorGraph.h:192
    │ │ │ │ -
    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
    │ │ │ │ -
    sharedFactor & at(size_t i)
    Get a specific factor by index (this checks array bounds and may throw an exception,...
    Definition FactorGraph.h:340
    │ │ │ │ -
    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
    │ │ │ │ -
    double error(const HybridValues &values) const
    Add error for all factors.
    Definition FactorGraph-inst.h:66
    │ │ │ │ -
    FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
    Constructor from iterator over factors (shared_ptr or plain objects)
    Definition FactorGraph.h:146
    │ │ │ │ -
    virtual void resize(size_t size)
    Directly resize the number of factors in the graph.
    Definition FactorGraph.h:381
    │ │ │ │ -
    size_t nrFactors() const
    return the number of non-null factors
    Definition FactorGraph-inst.h:76
    │ │ │ │ -
    size_t size() const
    return the number of factors (including any null factors set by remove() ).
    Definition FactorGraph.h:326
    │ │ │ │ -
    HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
    Push back many factors with an iterator (factors are copied)
    Definition FactorGraph.h:240
    │ │ │ │ -
    sharedFactor front() const
    Get the first factor.
    Definition FactorGraph.h:359
    │ │ │ │ -
    const_iterator end() const
    Iterator to end of factors.
    Definition FactorGraph.h:356
    │ │ │ │ -
    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
    │ │ │ │ -
    FACTOR FactorType
    factor type
    Definition FactorGraph.h:99
    │ │ │ │ -
    void replace(size_t index, sharedFactor factor)
    replace a factor by index
    Definition FactorGraph.h:388
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    iterator begin()
    non-const STL-style begin()
    Definition FactorGraph.h:372
    │ │ │ │ -
    const_iterator begin() const
    Iterator to beginning of factors.
    Definition FactorGraph.h:353
    │ │ │ │ -
    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
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
    add is a synonym for push_back.
    Definition FactorGraph.h:210
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition FactorGraph.h:438
    │ │ │ │ -
    boost::shared_ptr< FACTOR > sharedFactor
    Shared pointer to a factor.
    Definition FactorGraph.h:101
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
    Constructor that takes an initializer list of shared pointers.
    Definition FactorGraph.h:171
    │ │ │ │ -
    iterator end()
    non-const STL-style end()
    Definition FactorGraph.h:375
    │ │ │ │ -
    FastVector< sharedFactor > factors_
    concept check, makes sure FACTOR defines print and equals
    Definition FactorGraph.h:131
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual ~FactorGraph()=default
    Default destructor Public and virtual so boost serialization can call it.
    │ │ │ │ -
    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
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    void addFactorsToGraph(FactorGraph< FactorType > *graph) const
    Add all cliques in this BayesTree to the specified factor graph.
    Definition BayesTree-inst.h:168
    │ │ │ │ -
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ -
    Helper.
    Definition FactorGraph.h:54
    │ │ │ │ -
    Helper.
    Definition FactorGraph.h:67
    │ │ │ │ -
    Helper.
    Definition FactorGraph.h:80
    │ │ │ │ -
    the error.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    Give fixed size dimension of a type, fails at compile time if dynamic.
    Definition Manifold.h:164
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
    Definition CalibratedCamera.h:52
    │ │ │ │ +
    static Matrix26 Dpose(const Point2 &pn, double d)
    Calculate Jacobian with respect to pose.
    Definition CalibratedCamera.cpp:27
    │ │ │ │ +
    virtual void print(const std::string &s="PinholeBase") const
    print
    Definition CalibratedCamera.cpp:74
    │ │ │ │ +
    std::pair< Point2, bool > projectSafe(const Point3 &pw) const
    Project a point into the image and check depth.
    Definition CalibratedCamera.cpp:109
    │ │ │ │ +
    const Pose3 & pose() const
    return pose, constant version
    Definition CalibratedCamera.h:152
    │ │ │ │ +
    static Pose3 LevelPose(const Pose2 &pose2, double height)
    Create a level pose at the given 2D pose and height.
    Definition CalibratedCamera.cpp:49
    │ │ │ │ +
    bool equals(const PinholeBase &camera, double tol=1e-9) const
    assert equality up to a tolerance
    Definition CalibratedCamera.cpp:69
    │ │ │ │ +
    static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
    Calculate Jacobian with respect to point.
    Definition CalibratedCamera.cpp:37
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A Calibrated camera class [R|-R't], calibration K=I.
    Definition CalibratedCamera.h:247
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    std::pair< Point2, bool > projectSafe(const Point3 &pw) const
    Project a point into the image and check depth.
    Definition PinholePose.h:82
    │ │ │ │ +
    PinholeBaseK()
    default constructor
    Definition PinholePose.h:51
    │ │ │ │ +
    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
    │ │ │ │ +
    virtual const CALIBRATION & calibration() const =0
    return calibration
    │ │ │ │ +
    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
    │ │ │ │ +
    PinholeBaseK(const Pose3 &pose)
    constructor with pose
    Definition PinholePose.h:55
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Unit3 backprojectPointAtInfinity(const Point2 &p) const
    backproject a 2-dimensional point to a 3-dimensional point at infinity
    Definition PinholePose.h:170
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition PinholePose.h:222
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A pinhole camera class that has a Pose3 and a fixed Calibration.
    Definition PinholePose.h:243
    │ │ │ │ +
    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
    │ │ │ │ +
    PinholePose()
    default constructor
    Definition PinholePose.h:260
    │ │ │ │ +
    Matrix34 cameraProjectionMatrix() const
    for Linear Triangulation
    Definition PinholePose.h:420
    │ │ │ │ +
    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
    │ │ │ │ +
    const CALIBRATION & calibration() const override
    return calibration
    Definition PinholePose.h:370
    │ │ │ │ +
    Vector defaultErrorWhenTriangulatingBehindCamera() const
    for Nonlinear Triangulation
    Definition PinholePose.h:426
    │ │ │ │ +
    friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
    stream operator
    Definition PinholePose.h:339
    │ │ │ │ +
    static PinholePose Identity()
    for Canonical
    Definition PinholePose.h:415
    │ │ │ │ +
    PinholePose(const Vector &v, const Vector &K)
    Init from Vector and calibration.
    Definition PinholePose.h:319
    │ │ │ │ +
    const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
    return shared pointer to calibration
    Definition PinholePose.h:365
    │ │ │ │ +
    size_t dim() const
    Definition PinholePose.h:395
    │ │ │ │ +
    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
    │ │ │ │ +
    PinholePose retract(const Vector6 &d) const
    move a cameras according to d
    Definition PinholePose.h:405
    │ │ │ │ +
    PinholePose(const Vector &v)
    Init from 6D vector.
    Definition PinholePose.h:314
    │ │ │ │ +
    PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
    constructor with pose and calibration
    Definition PinholePose.h:269
    │ │ │ │ +
    static size_t Dim()
    Definition PinholePose.h:400
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition PinholePose.h:434
    │ │ │ │ +
    bool equals(const Base &camera, double tol=1e-9) const
    assert equality up to a tolerance
    Definition PinholePose.h:333
    │ │ │ │ +
    void print(const std::string &s="PinholePose") const override
    print
    Definition PinholePose.h:349
    │ │ │ │ +
    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
    │ │ │ │ +
    PinholePose(const Pose3 &pose)
    constructor with pose, uses default calibration
    Definition PinholePose.h:264
    │ │ │ │ +
    static PinholePose Level(const Pose2 &pose2, double height)
    PinholePose::level with default calibration.
    Definition PinholePose.h:290
    │ │ │ │ +
    Vector6 localCoordinates(const PinholePose &p) const
    return canonical coordinate
    Definition PinholePose.h:410
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get translation
    Definition Pose3.cpp:308
    │ │ │ │ +
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
    Use RQ to calculate roll-pitch-yaw angle representation.
    Definition Rot3.cpp:192
    │ │ │ │ +
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,591 +1,662 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FactorGraph.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// \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> │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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: │ │ │ │ │ +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; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +45 typedef CALIBRATION CalibrationType; │ │ │ │ │ +46 │ │ │ │ │ 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; │ │ │ │ │ -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 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 │ │ │ │ │ +_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 } │ │ │ │ │ +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 │ │ │ │ │ +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 // 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_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_; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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(); } │ │ │ │ │ +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 │ │ │ │ │ -_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(); } │ │ │ │ │ +_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 │ │ │ │ │ -_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(); } │ │ │ │ │ +361 _~_P_i_n_h_o_l_e_P_o_s_e() override { │ │ │ │ │ +362 } │ │ │ │ │ 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(); } │ │ │ │ │ +_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_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; } │ │ │ │ │ +_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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ -_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. │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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 │ │ │ │ │ +_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_:_:_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 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. │ │ │ │ │ +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_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a00620_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00584_source.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    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
    │ │ │ │
    21#pragma once
    │ │ │ │
    22
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    27
    │ │ │ │
    28namespace gtsam {
    │ │ │ │
    29
    │ │ │ │
    30template<class BAYESTREE, class GRAPH, class ETREE_NODE>
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ @@ -255,18 +255,18 @@ │ │ │ │
    152 Base::remainingFactors_ = eliminationTree.remainingFactors();
    │ │ │ │
    153}
    │ │ │ │
    │ │ │ │
    │ │ │ │
    154
    │ │ │ │
    155} // namespace gtsam
    │ │ │ │
    │ │ │ │ -
    The junction tree.
    │ │ │ │ -
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    The junction tree.
    │ │ │ │ +
    Collects factorgraph fragments defined on variable clusters, arranged in a tree.
    │ │ │ │ + │ │ │ │ + │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    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
    │ │ │ │
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │
    const FastVector< sharedFactor > & remainingFactors() const
    Return the remaining factors that are not pulled into elimination.
    Definition EliminationTree.h:154
    │ │ │ │ @@ -278,13 +278,13 @@ │ │ │ │
    boost::shared_ptr< This > shared_ptr
    Typedef to the conditional base class.
    Definition SymbolicConditional.h:44
    │ │ │ │
    boost::shared_ptr< This > shared_ptr
    Overriding the shared_ptr typedef.
    Definition SymbolicFactor.h:48
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -160,16 +160,16 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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. │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00626_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/MetisIndex.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    inference-inst.h
    │ │ │ │ +
    MetisIndex.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
    │ │ │ │ -
    9
    │ │ │ │ -
    10* -------------------------------------------------------------------------- */
    │ │ │ │ -
    11
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ -
    22#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    23#include <utility>
    │ │ │ │ +
    8 * See LICENSE for the license information
    │ │ │ │ +
    9 * -------------------------------------------------------------------------- */
    │ │ │ │ +
    10
    │ │ │ │ +
    17#pragma once
    │ │ │ │ +
    18
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/inference/Key.h>
    │ │ │ │ + │ │ │ │ +
    22#include <gtsam/base/types.h>
    │ │ │ │ +
    23#include <gtsam/base/timing.h>
    │ │ │ │
    24
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    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}
    │ │ │ │ - │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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_;
    │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Timing utilities.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Factor Graph Base Class.
    │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
    Definition MetisIndex.h:45
    │ │ │ │ +
    MetisIndex()
    Default constructor, creates empty MetisIndex.
    Definition MetisIndex.h:61
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,127 +1,117 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -inference-inst.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) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights 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 * See LICENSE for the license information │ │ │ │ │ +9 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ +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#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 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +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_; │ │ │ │ │ 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 │ │ │ │ │ +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. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +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 │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ + * _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/a00632.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00608.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/EliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    JunctionTree.h File Reference
    │ │ │ │ +
    EliminationTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    The junction tree. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    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...
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    The junction tree.

    │ │ │ │ -
    Date
    Feb 4, 2010
    │ │ │ │ -
    Author
    Kai Ni
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ Richard Roberts
    │ │ │ │ +
    Date
    Oct 13, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +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 │ │ │ │ │ -JunctionTree.h File Reference │ │ │ │ │ -The junction tree. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The junction tree. │ │ │ │ │ - Date │ │ │ │ │ - Feb 4, 2010 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 13, 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_._h │ │ │ │ │ + * _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/a00644_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00800_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/linear/PowerMethod.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    BayesNet.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,
    │ │ │ │ -
    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-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
    │ │ │ │ +
    8 * See LICENSE for the license information
    │ │ │ │
    9
    │ │ │ │ -
    10* -------------------------------------------------------------------------- */
    │ │ │ │ +
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    24#include <string>
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28class HybridValues;
    │ │ │ │ +
    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
    │ │ │ │ -
    34template <class CONDITIONAL>
    │ │ │ │ -
    │ │ │ │ -
    35class BayesNet : public FactorGraph<CONDITIONAL> {
    │ │ │ │ -
    36 private:
    │ │ │ │ - │ │ │ │ -
    38
    │ │ │ │ -
    39 public:
    │ │ │ │ -
    40 typedef typename boost::shared_ptr<CONDITIONAL>
    │ │ │ │ - │ │ │ │ -
    42
    │ │ │ │ -
    43 protected:
    │ │ │ │ -
    46
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    30namespace gtsam {
    │ │ │ │ +
    31
    │ │ │ │ +
    32using Sparse = Eigen::SparseMatrix<double>;
    │ │ │ │ +
    33
    │ │ │ │ +
    56template <class Operator>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    58 protected:
    │ │ │ │ +
    63 const Operator &A_;
    │ │ │ │
    64
    │ │ │ │ -
    65 public:
    │ │ │ │ +
    65 const int dim_; // dimension of Matrix A
    │ │ │ │ +
    66
    │ │ │ │ +
    67 size_t nrIterations_; // number of iterations
    │ │ │ │
    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;
    │ │ │ │ -
    82
    │ │ │ │ -
    84 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
    │ │ │ │ -
    85 const DotWriter& writer = DotWriter()) const;
    │ │ │ │ +
    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
    │ │ │ │ -
    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};
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    104
    │ │ │ │ -
    105} // namespace gtsam
    │ │ │ │ +
    100
    │ │ │ │ +
    105 Vector powerIteration() const { return powerIteration(ritzVector_); }
    │ │ │ │
    106
    │ │ │ │ -
    107#include <gtsam/inference/BayesNet-inst.h>
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ -
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ -
    BayesNet(std::initializer_list< sharedConditional > conditionals)
    Constructor that takes an initializer list of shared pointers.
    Definition BayesNet.h:60
    │ │ │ │ -
    void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print out graph
    Definition BayesNet-inst.h:32
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< CONDITIONAL > sharedConditional
    A shared pointer to a conditional.
    Definition BayesNet.h:41
    │ │ │ │ -
    BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    Construct from iterator over conditionals.
    Definition BayesNet.h:52
    │ │ │ │ -
    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
    │ │ │ │ -
    BayesNet()
    Default constructor as an empty BayesNet.
    Definition BayesNet.h:48
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ -
    the error.
    │ │ │ │ +
    Compute maximum Eigenpair with power method.
    Definition PowerMethod.h:57
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector eigenvector() const
    Return the eigenvector.
    Definition PowerMethod.h:149
    │ │ │ │ +
    const Operator & A_
    Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
    Definition PowerMethod.h:63
    │ │ │ │ +
    double eigenvalue() const
    Return the eigenvalue.
    Definition PowerMethod.h:146
    │ │ │ │ +
    PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none)
    Construct from the aim matrix and intial ritz vector.
    Definition PowerMethod.h:77
    │ │ │ │ +
    Vector powerIteration() const
    Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
    Definition PowerMethod.h:105
    │ │ │ │ +
    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
    │ │ │ │ +
    bool compute(size_t maxIterations, double tol)
    Start the power/accelerated iteration, after performing the power/accelerated iteration,...
    Definition PowerMethod.h:129
    │ │ │ │ +
    size_t nrIterations() const
    Return the number of iterations.
    Definition PowerMethod.h:121
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,158 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesNet.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, │ │ │ │ │ -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-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 │ │ │ │ │ +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> │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 public: │ │ │ │ │ +65 const int dim_; // dimension of Matrix A │ │ │ │ │ +66 │ │ │ │ │ +67 size_t nrIterations_; // number of iterations │ │ │ │ │ 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; │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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_); } │ │ │ │ │ 106 │ │ │ │ │ -107#include │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ +_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 │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_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_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_:_:_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 │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_N_e_t_._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/a00650.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00617.html │ │ │ │┄ Files 4% similar despite different names │ │ │ │ @@ -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/Ordering.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    VariableIndex.cpp File Reference
    │ │ │ │ +
    Ordering.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    March 26, 2013
    │ │ │ │ +
    Author
    Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Andrew Melim
    │ │ │ │ +
    Date
    Sep 2, 2010
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableIndex.cpp File Reference │ │ │ │ │ +Ordering.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Andrew Melim │ │ │ │ │ Date │ │ │ │ │ - March 26, 2013 │ │ │ │ │ + Sep 2, 2010 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00656.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01304.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/slam/InitializePose3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    VariableIndex-inl.h File Reference
    │ │ │ │ +
    InitializePose3.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    March 26, 2013
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    August, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _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_. │ │ │ │ │ +InitializePose3.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - March 26, 2013 │ │ │ │ │ + August, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00680.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01007.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/nonlinear/WhiteNoiseFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    BayesTreeCliqueBase.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Variables
    │ │ │ │ +
    WhiteNoiseFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Base class for cliques of a BayesTree. │ │ │ │ +

    Binary white noise factor. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >
     This is the base class for BayesTree cliques. More...
    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

    │ │ │ │ -

    Base class for cliques of a BayesTree.

    │ │ │ │ -
    Author
    Richard Roberts and Frank Dellaert
    │ │ │ │ +

    Binary white noise factor.

    │ │ │ │ +
    Author
    Chris Beall
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    Date
    September 2011
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +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 │ │ │ │ │ -BayesTreeCliqueBase.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 | _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_:_:_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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ +Binary white noise factor. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts and Frank Dellaert │ │ │ │ │ + Chris Beall │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + September 2011 │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01016.html │ │ │ │┄ Files 4% similar despite different names │ │ │ │ @@ -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/nonlinear/ISAM2Params.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    DotWriter.cpp File Reference
    │ │ │ │ +
    ISAM2Params.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Graphviz formatting for factor graphs. │ │ │ │ +

    Parameters for iSAM 2. │ │ │ │ More...

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

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Graphviz formatting for factor graphs.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    December, 2021
    │ │ │ │ +

    Parameters for iSAM 2.

    │ │ │ │ +
    Author
    Michael Kaess, Richard Roberts, Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,20 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Graphviz formatting for factor graphs. │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00707.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │┄ Files 15% 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/geometry/SO3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
    │ │ │ │ -
    HessianFactor.cpp File Reference
    │ │ │ │ +
    SO3.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

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

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

    │ │ │ │ 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.
     
    │ │ │ │ +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

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Dec 8, 2010
    │ │ │ │ -
    │ │ │ │ +

    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 SO3R,
    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,36 +1,41 @@ │ │ │ │ │ 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 │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ +  (constant) Jacobian of compose wrpt M │ │ │ │ │   │ │ │ │ │ - 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. │ │ │ │ │ + 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 │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Duy Nguyen Ta │ │ │ │ │ Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ + 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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _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/a00731_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00062_source.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/base/Lie.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    LossFunctions.h
    │ │ │ │ +
    Lie.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>
    │ │ │ │ +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 <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:
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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 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
    │ │ │ │ -
    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_; }
    │ │ │ │ +
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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> {
    │ │ │ │ + │ │ │ │ +
    176
    │ │ │ │ + │ │ │ │ +
    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 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};
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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};
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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};
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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_; }
    │ │ │ │ +
    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(
    │ │ │ │ + │ │ │ │ +
    345}
    │ │ │ │ +
    │ │ │ │ +
    346
    │ │ │ │ +
    351template<class T>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    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>;
    │ │ │ │ +
    Concept check class for variable types with Group properties.
    │ │ │ │ +
    Base class and basic functions for Manifold types.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    Pure virtual class for all robust error function classes.
    Definition LossFunctions.h:63
    │ │ │ │ -
    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
    │ │ │ │ -
    ReweightScheme reweight_
    Strategy for reweighting.
    Definition LossFunctions.h:72
    │ │ │ │ -
    ReweightScheme reweightScheme() const
    Returns the reweight scheme, as explained in ReweightScheme.
    Definition LossFunctions.h:79
    │ │ │ │ -
    virtual double weight(double distance) const =0
    This method is responsible for returning the weight function for a given amount of error.
    │ │ │ │ -
    ReweightScheme
    the rows can be weighted independently according to the error or uniformly with the norm of the right...
    Definition LossFunctions.h:67
    │ │ │ │ -
    "Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain least-squares (non-ro...
    Definition LossFunctions.h:146
    │ │ │ │ -
    double weight(double) const override
    This method is responsible for returning the weight function for a given amount of error.
    Definition LossFunctions.h:152
    │ │ │ │ -
    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
    │ │ │ │ -
    Implementation of the "Fair" robust error model (Zhang97ivc)
    Definition LossFunctions.h:175
    │ │ │ │ -
    The "Huber" robust error model (Zhang97ivc).
    Definition LossFunctions.h:208
    │ │ │ │ -
    Implementation of the "Cauchy" robust error model (Lee2013IROS).
    Definition LossFunctions.h:246
    │ │ │ │ -
    Implementation of the "Tukey" robust error model (Zhang97ivc).
    Definition LossFunctions.h:280
    │ │ │ │ -
    Implementation of the "Welsch" robust error model (Zhang97ivc).
    Definition LossFunctions.h:313
    │ │ │ │ -
    Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
    Definition LossFunctions.h:349
    │ │ │ │ -
    DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
    Definition LossFunctions.h:387
    │ │ │ │ -
    L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,...
    Definition LossFunctions.h:426
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector logmap_default(const Class &l0, const Class &lp)
    Log map centered at l0, s.t.
    Definition Lie.h:246
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Class expmap_default(const Class &t, const Vector &d)
    Exponential map centered at l0, s.t.
    Definition Lie.h:252
    │ │ │ │ +
    Matrix wedge(const Vector &x)
    Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    tag to assert a type is a group
    Definition Group.h:34
    │ │ │ │ +
    Group operator syntax flavors.
    Definition Group.h:37
    │ │ │ │ +
    Group Concept.
    Definition Group.h:46
    │ │ │ │ +
    A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
    Definition Lie.h:37
    │ │ │ │ +
    static Class Retract(const TangentVector &v, ChartJacobian H)
    Retract at origin with optional derivative.
    Definition Lie.h:121
    │ │ │ │ +
    TangentVector localCoordinates(const Class &g) const
    localCoordinates as required by manifold concept: finds tangent vector between *this and g
    Definition Lie.h:136
    │ │ │ │ +
    Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
    expmap with optional derivatives
    Definition Lie.h:89
    │ │ │ │ +
    TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
    localCoordinates with optional derivatives
    Definition Lie.h:152
    │ │ │ │ +
    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
    │ │ │ │ +
    TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
    logmap with optional derivatives
    Definition Lie.h:100
    │ │ │ │ +
    static TangentVector LocalCoordinates(const Class &g)
    LocalCoordinates at origin: possible in Lie group because it has an identity.
    Definition Lie.h:116
    │ │ │ │ +
    Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
    retract with optional derivatives
    Definition Lie.h:141
    │ │ │ │ +
    static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
    LocalCoordinates at origin with optional derivative.
    Definition Lie.h:126
    │ │ │ │ +
    Class retract(const TangentVector &v) const
    retract as required by manifold concept: applies v at *this
    Definition Lie.h:131
    │ │ │ │ +
    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
    │ │ │ │ +
    static Class Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ +
    tag to assert a type is a Lie group
    Definition Lie.h:164
    │ │ │ │ +
    A helper class that implements the traits interface for GTSAM lie groups.
    Definition Lie.h:174
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    Lie Group Concept.
    Definition Lie.h:260
    │ │ │ │ +
    Functor for transforming covariance of T.
    Definition Lie.h:353
    │ │ │ │ +
    tag to assert a type is a manifold
    Definition Manifold.h:33
    │ │ │ │ +
    Extra manifold traits for fixed-dimension types.
    Definition Manifold.h:75
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,385 +1,440 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LossFunctions.h │ │ │ │ │ -1 │ │ │ │ │ -2/* --------------------------------------------------------------------------- │ │ │ │ │ +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/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -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 │ │ │ │ │ +11 │ │ │ │ │ +24#pragma once │ │ │ │ │ 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; │ │ │ │ │ +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 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 │ │ │ │ │ -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_; } │ │ │ │ │ +_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 } │ │ │ │ │ +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 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}; │ │ │ │ │ +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 │ │ │ │ │ -_2_4_6class GTSAM_EXPORT _C_a_u_c_h_y : public _B_a_s_e { │ │ │ │ │ -247 protected: │ │ │ │ │ -248 double k_, ksquared_; │ │ │ │ │ +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 │ │ │ │ │ -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}; │ │ │ │ │ +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 │ │ │ │ │ -_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}; │ │ │ │ │ +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 │ │ │ │ │ -_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_; } │ │ │ │ │ +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 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 │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * LLoossssFFuunnccttiioonnss..hh │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _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/a00734.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │┄ Files 8% 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/hybrid/MixtureFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces | │ │ │ │ -Functions
    │ │ │ │ -
    GaussianBayesTree.cpp File Reference
    │ │ │ │ +Classes | │ │ │ │ +Namespaces
    │ │ │ │ +
    MixtureFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ +

    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

    │ │ │ │ -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
    │ │ │ │ +

    Nonlinear Mixture factor of continuous and discrete.

    │ │ │ │ +
    Author
    Kevin Doherty, kdohe.nosp@m.rty@.nosp@m.mit.e.nosp@m.du
    │ │ │ │
    │ │ │ │ -Richard Roberts
    │ │ │ │ +Varun Agrawal
    │ │ │ │ +
    Date
    December 2021
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -LogDeterminantData &  ggttssaamm::::iinntteerrnnaall::::llooggDDeetteerrmmiinnaanntt (const │ │ │ │ │ - GaussianBayesTreeClique::shared_ptr &clique, │ │ │ │ │ - LogDeterminantData &parentSum) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ +Nonlinear Mixture factor of continuous and discrete. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + 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 │ │ │ │ │ + Date │ │ │ │ │ + December 2021 │ │ │ │ │ * _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 │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _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/a00737.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00824.html │ │ │ │┄ Files 3% 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/linear/GaussianConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GaussianConditional-inl.h File Reference
    │ │ │ │ +
    GaussianConditional.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    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

    │ │ │ │

    Conditional Gaussian Base class.

    │ │ │ │ -
    Author
    Christian Potthast
    │ │ │ │ +
    Author
    Christian Potthast, Frank Dellaert
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianConditional-inl.h File Reference │ │ │ │ │ +GaussianConditional.cpp 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 ********** │ │ │ │ │ Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Christian Potthast, Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00743.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01028.html │ │ │ │┄ Files 4% similar despite different names │ │ │ │ @@ -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/nonlinear/GraphvizFormatting.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    SubgraphPreconditioner.cpp File Reference
    │ │ │ │ +
    GraphvizFormatting.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

    Graphviz formatter for NonlinearFactorGraph. │ │ │ │ +More...

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

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    Dec 31, 2009
    │ │ │ │ -
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │ +

    Graphviz formatter for NonlinearFactorGraph.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    December, 2021
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00755_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01037_source.html │ │ │ │┄ Files 12% 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/nonlinear/Values.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NoiseModel.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
    │ │ │ │ -
    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 {
    │ │ │ │ -
    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 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;
    │ │ │ │ +
    25#pragma once
    │ │ │ │ +
    26
    │ │ │ │ + │ │ │ │ +
    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_(); }
    │ │ │ │ + │ │ │ │ +
    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>,
    │ │ │ │ + │ │ │ │ +
    79 KeyValuePtrPairAllocator > KeyValueMap;
    │ │ │ │
    80
    │ │ │ │ -
    82 virtual Vector sigmas() const;
    │ │ │ │ +
    81 // The member to store the values, see just above
    │ │ │ │ +
    82 KeyValueMap values_;
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ -
    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;
    │ │ │ │ +
    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;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    180
    │ │ │ │ -
    181 public:
    │ │ │ │ +
    181 size_t size() const { return values_.size(); }
    │ │ │ │
    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) {}
    │ │ │ │ -
    │ │ │ │ +
    184 bool empty() const { return values_.empty(); }
    │ │ │ │ +
    185
    │ │ │ │
    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;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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 virtual Matrix R() const { return thisR();}
    │ │ │ │ +
    255 void update(Key j, const Value& val);
    │ │ │ │
    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 //---------------------------------------------------------------------------------------
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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();
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    404 Constrained(const Vector& sigmas = Z_1x1);
    │ │ │ │ +
    399 template <class ValueType = Value>
    │ │ │ │ +
    400 class Filtered;
    │ │ │ │ +
    401
    │ │ │ │ +
    403 template <class ValueType = Value>
    │ │ │ │ +
    404 class ConstFiltered;
    │ │ │ │
    405
    │ │ │ │ -
    406 ~Constrained() override {}
    │ │ │ │ -
    407
    │ │ │ │ -
    409 bool isConstrained() const override { return true; }
    │ │ │ │ -
    410
    │ │ │ │ -
    412 bool constrained(size_t i) const;
    │ │ │ │ +
    407 template <class ValueType>
    │ │ │ │ +
    408 Values(const Filtered<ValueType>& view);
    │ │ │ │ +
    409
    │ │ │ │ +
    411 template <class ValueType>
    │ │ │ │ +
    412 Values(const ConstFiltered<ValueType>& view);
    │ │ │ │
    413
    │ │ │ │ -
    415 const Vector& mu() const { return mu_; }
    │ │ │ │ -
    416
    │ │ │ │ -
    421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ +
    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 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;
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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) {}
    │ │ │ │ +
    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:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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 {}
    │ │ │ │ +
    525 GTSAM_EXPORT const char* what() const noexcept override;
    │ │ │ │ +
    526 };
    │ │ │ │ +
    │ │ │ │ +
    527
    │ │ │ │ +
    528 /* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ +
    529 class DynamicValuesMismatched : public std::exception {
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    562 /* ************************************************************************* */
    │ │ │ │ +
    564 template<>
    │ │ │ │ +
    │ │ │ │ +
    565 struct traits<Values> : public Testable<Values> {
    │ │ │ │ +
    566 };
    │ │ │ │ +
    │ │ │ │ +
    567
    │ │ │ │ +
    568} //\ namespace gtsam
    │ │ │ │ +
    569
    │ │ │ │ +
    570
    │ │ │ │ +
    571#include <gtsam/nonlinear/Values-inl.h>
    │ │ │ │ +
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ +
    bool operator!=(const Matrix &A, const Matrix &B)
    inequality
    Definition Matrix.h:107
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Default allocator for list, map, and set types.
    Definition FastDefaultAllocator.h:50
    │ │ │ │ + │ │ │ │ +
    Wraps any type T so it can play as a Value.
    Definition GenericValue.h:47
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Pure virtual class for all robust error function classes.
    Definition LossFunctions.h:63
    │ │ │ │ -
    noiseModel::Base is the abstract base class for all noise models.
    Definition NoiseModel.h:53
    │ │ │ │ -
    virtual bool isConstrained() const
    true if a constrained noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:69
    │ │ │ │ -
    virtual void whitenInPlace(Vector &v) const
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:112
    │ │ │ │ -
    size_t dim() const
    Dimensionality.
    Definition NoiseModel.h:75
    │ │ │ │ -
    virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:127
    │ │ │ │ -
    virtual void whitenInPlace(Eigen::Block< Vector > &v) const
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:122
    │ │ │ │ -
    virtual Vector whiten(const Vector &v) const =0
    Whiten an error vector.
    │ │ │ │ -
    virtual double mahalanobisDistance(const Vector &v) const
    Mahalanobis distance.
    Definition NoiseModel.h:97
    │ │ │ │ -
    virtual bool isUnit() const
    true if a unit noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:72
    │ │ │ │ -
    virtual double weight(const Vector &v) const
    get the weight from the effective loss function on residual vector v
    Definition NoiseModel.h:137
    │ │ │ │ -
    virtual Vector unweightedWhiten(const Vector &v) const
    Useful function for robust noise models to get the unweighted but whitened error.
    Definition NoiseModel.h:132
    │ │ │ │ -
    virtual Vector unwhiten(const Vector &v) const =0
    Unwhiten an error vector.
    │ │ │ │ -
    virtual double loss(const double squared_distance) const
    loss function, input is Mahalanobis distance
    Definition NoiseModel.h:102
    │ │ │ │ -
    virtual void unwhitenInPlace(Vector &v) const
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:117
    │ │ │ │ -
    virtual Matrix Whiten(const Matrix &H) const =0
    Whiten a matrix.
    │ │ │ │ -
    Base(size_t dim=1)
    primary constructor
    Definition NoiseModel.h:65
    │ │ │ │ -
    Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
    Definition NoiseModel.h:162
    │ │ │ │ -
    virtual Matrix R() const
    Return R itself, but note that Whiten(H) is cheaper than R*H.
    Definition NoiseModel.h:255
    │ │ │ │ -
    Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
    constructor takes square root information matrix
    Definition NoiseModel.h:186
    │ │ │ │ -
    boost::optional< Matrix > sqrt_information_
    Matrix square root of information matrix (R)
    Definition NoiseModel.h:167
    │ │ │ │ -
    A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
    Definition NoiseModel.h:281
    │ │ │ │ -
    Matrix R() const override
    Return R itself, but note that Whiten(H) is cheaper than R*H.
    Definition NoiseModel.h:352
    │ │ │ │ -
    Vector sigmas_
    Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
    Definition NoiseModel.h:289
    │ │ │ │ -
    double sigma(size_t i) const
    Return standard deviations (sqrt of diagonal)
    Definition NoiseModel.h:335
    │ │ │ │ -
    const Vector & invsigmas() const
    Return sqrt precisions.
    Definition NoiseModel.h:340
    │ │ │ │ -
    Vector sigmas() const override
    Calculate standard deviations.
    Definition NoiseModel.h:325
    │ │ │ │ -
    const Vector & precisions() const
    Return precisions.
    Definition NoiseModel.h:346
    │ │ │ │ -
    A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
    Definition NoiseModel.h:381
    │ │ │ │ -
    bool isConstrained() const override
    true if a constrained noise mode, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:409
    │ │ │ │ -
    static shared_ptr All(size_t dim, const Vector &mu)
    Fully constrained variations.
    Definition NoiseModel.h:462
    │ │ │ │ -
    static shared_ptr All(size_t dim, double mu)
    Fully constrained variations with a mu parameter.
    Definition NoiseModel.h:467
    │ │ │ │ -
    static shared_ptr All(size_t dim)
    Fully constrained variations.
    Definition NoiseModel.h:457
    │ │ │ │ -
    const Vector & mu() const
    Access mu as a vector.
    Definition NoiseModel.h:415
    │ │ │ │ -
    Vector mu_
    Penalty function weight - needs to be large enough to dominate soft constraints.
    Definition NoiseModel.h:385
    │ │ │ │ -
    An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
    Definition NoiseModel.h:516
    │ │ │ │ -
    double sigma() const
    Return standard deviation.
    Definition NoiseModel.h:565
    │ │ │ │ -
    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
    │ │ │ │ -
    Isotropic(size_t dim, double sigma)
    protected constructor takes sigma
    Definition NoiseModel.h:521
    │ │ │ │ -
    Unit: i.i.d.
    Definition NoiseModel.h:584
    │ │ │ │ -
    void WhitenInPlace(Eigen::Block< Matrix >) const override
    In-place version.
    Definition NoiseModel.h:610
    │ │ │ │ -
    Vector unwhiten(const Vector &v) const override
    Unwhiten an error vector.
    Definition NoiseModel.h:607
    │ │ │ │ -
    void whitenInPlace(Eigen::Block< Vector > &) const override
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:613
    │ │ │ │ -
    bool isUnit() const override
    true if a unit noise model, saves slow/clumsy dynamic casting
    Definition NoiseModel.h:602
    │ │ │ │ -
    Unit(size_t dim=1)
    constructor for serialization
    Definition NoiseModel.h:590
    │ │ │ │ -
    Vector whiten(const Vector &v) const override
    Whiten an error vector.
    Definition NoiseModel.h:606
    │ │ │ │ -
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ -
    void unwhitenInPlace(Vector &) const override
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:612
    │ │ │ │ -
    void unwhitenInPlace(Eigen::Block< Vector > &) const override
    in-place unwhiten, override if can be done more efficiently
    Definition NoiseModel.h:614
    │ │ │ │ -
    void whitenInPlace(Vector &) const override
    in-place whiten, override if can be done more efficiently
    Definition NoiseModel.h:611
    │ │ │ │ -
    void WhitenInPlace(Matrix &) const override
    In-place version.
    Definition NoiseModel.h:609
    │ │ │ │ -
    Matrix Whiten(const Matrix &H) const override
    Whiten a matrix.
    Definition NoiseModel.h:608
    │ │ │ │ -
    Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
    Definition NoiseModel.h:642
    │ │ │ │ -
    const RobustModel::shared_ptr & robust() const
    Return the contained robust error function.
    Definition NoiseModel.h:669
    │ │ │ │ -
    Robust()
    Default Constructor for serialization.
    Definition NoiseModel.h:656
    │ │ │ │ -
    Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
    Constructor.
    Definition NoiseModel.h:659
    │ │ │ │ -
    double squaredMahalanobisDistance(const Vector &v) const override
    Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
    Definition NoiseModel.h:688
    │ │ │ │ -
    const NoiseModel::shared_ptr noise_
    noise model used
    Definition NoiseModel.h:651
    │ │ │ │ -
    double loss(const double squared_distance) const override
    Compute loss from the m-estimator using the Mahalanobis distance.
    Definition NoiseModel.h:682
    │ │ │ │ -
    Vector unwhiten(const Vector &) const override
    Unwhiten an error vector.
    Definition NoiseModel.h:679
    │ │ │ │ -
    Matrix Whiten(const Matrix &A) const override
    Whiten a matrix.
    Definition NoiseModel.h:677
    │ │ │ │ -
    const RobustModel::shared_ptr robust_
    robust error function used
    Definition NoiseModel.h:650
    │ │ │ │ -
    ~Robust() override
    Destructor.
    Definition NoiseModel.h:663
    │ │ │ │ -
    const NoiseModel::shared_ptr & noise() const
    Return the contained noise model.
    Definition NoiseModel.h:672
    │ │ │ │ -
    Vector whiten(const Vector &v) const override
    Whiten an error vector.
    Definition NoiseModel.h:675
    │ │ │ │ +
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ +
    virtual void deallocate_() const =0
    Deallocate a raw pointer of this value.
    │ │ │ │ +
    virtual Value * clone_() const =0
    Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    Definition Values.h:49
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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...
    │ │ │ │ +
    bool empty() const
    whether the config is empty
    Definition Values.h:184
    │ │ │ │ +
    deref_iterator upper_bound(Key j) const
    Find the lowest-ordered element greater than the specified key.
    Definition Values.h:219
    │ │ │ │ +
    boost::shared_ptr< Values > shared_ptr
    A shared_ptr to this class.
    Definition Values.h:87
    │ │ │ │ +
    void clear()
    Remove all variables from the config.
    Definition Values.h:301
    │ │ │ │ +
    void swap(Values &other)
    Swap the contents of two Values without copying data.
    Definition Values.h:298
    │ │ │ │ +
    void insertDouble(Key j, double c)
    version for double
    Definition Values.h:252
    │ │ │ │ +
    size_t size() const
    The number of variables in this config.
    Definition Values.h:181
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::shared_ptr< const Values > const_shared_ptr
    A const shared_ptr to this class.
    Definition Values.h:90
    │ │ │ │ +
    Values()=default
    Default constructor creates an empty Values class.
    │ │ │ │ +
    deref_iterator lower_bound(Key j) const
    Find the element greater than or equal to the specified key.
    Definition Values.h:216
    │ │ │ │ +
    double atDouble(size_t key) const
    version for double
    Definition Values.h:159
    │ │ │ │ +
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ +
    Value & value
    The value.
    Definition Values.h:95
    │ │ │ │ +
    const Key key
    The key.
    Definition Values.h:94
    │ │ │ │ +
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:101
    │ │ │ │ +
    const Key key
    The key.
    Definition Values.h:102
    │ │ │ │ +
    const Value & value
    The value.
    Definition Values.h:103
    │ │ │ │ +
    Definition Values.h:190
    │ │ │ │ +
    Definition Values.h:453
    │ │ │ │ +
    const Key key_
    The key that already existed.
    Definition Values.h:455
    │ │ │ │ +
    GTSAM_EXPORT const char * what() const noexcept override
    The message to be displayed to the user.
    Definition Values.cpp:280
    │ │ │ │ +
    ValuesKeyAlreadyExists(Key key) noexcept
    Construct with the key-value pair attempted to be added.
    Definition Values.h:462
    │ │ │ │ +
    Key key() const noexcept
    The duplicate key that was attempted to be added.
    Definition Values.h:468
    │ │ │ │ +
    Definition Values.h:475
    │ │ │ │ +
    ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
    Construct with the key that does not exist in the values.
    Definition Values.h:485
    │ │ │ │ +
    const Key key_
    The key that does not exist.
    Definition Values.h:478
    │ │ │ │ +
    Key key() const noexcept
    The key that was attempted to be accessed that does not exist.
    Definition Values.h:491
    │ │ │ │ +
    const char * operation_
    The operation that attempted to access the key.
    Definition Values.h:477
    │ │ │ │ +
    Definition Values.h:498
    │ │ │ │ +
    const std::type_info & storedTypeId() const
    The typeid of the value stores in the Values.
    Definition Values.h:519
    │ │ │ │ +
    Key key() const noexcept
    The key that was attempted to be accessed that does not exist.
    Definition Values.h:516
    │ │ │ │ +
    const Key key_
    The key requested.
    Definition Values.h:500
    │ │ │ │ +
    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
    │ │ │ │ +
    const std::type_info & requestedTypeId() const
    The requested typeid.
    Definition Values.h:522
    │ │ │ │ +
    Definition Values.h:529
    │ │ │ │ +
    Definition Values.h:542
    │ │ │ │ +
    The Factor::error simply extracts the.
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,805 +1,668 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NoiseModel.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 │ │ │ │ │ -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 { │ │ │ │ │ -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 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; │ │ │ │ │ +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#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 │ │ │ │ │ -82 virtual Vector sigmas() const; │ │ │ │ │ +81 // The member to store the values, see just above │ │ │ │ │ +82 KeyValueMap values_; │ │ │ │ │ 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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ +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 │ │ │ │ │ -180 │ │ │ │ │ -181 public: │ │ │ │ │ +_1_8_1 size_t _s_i_z_e() const { return values_.size(); } │ │ │ │ │ 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) {} │ │ │ │ │ +_1_8_4 bool _e_m_p_t_y() const { return values_.empty(); } │ │ │ │ │ +185 │ │ │ │ │ 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; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -_2_5_5 virtual Matrix _R() const { return thisR();} │ │ │ │ │ +255 void update(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ 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 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ +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 │ │ │ │ │ -_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(); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -404 _C_o_n_s_t_r_a_i_n_e_d(const Vector& sigmas = Z_1x1); │ │ │ │ │ +399 template │ │ │ │ │ +400 class Filtered; │ │ │ │ │ +401 │ │ │ │ │ +403 template │ │ │ │ │ +404 class ConstFiltered; │ │ │ │ │ 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; │ │ │ │ │ +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 const Vector& _m_u() const { return mu_; } │ │ │ │ │ -416 │ │ │ │ │ -421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas); │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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) │ │ │ │ │ +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 │ │ │ │ │ -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 {} │ │ │ │ │ +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 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; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._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/a00758.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00776.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/AcceleratedPowerMethod.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    PowerMethod.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    AcceleratedPowerMethod.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Power method for fast eigenvalue and eigenvector computation. │ │ │ │ +

    accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::PowerMethod< Operator >
     Compute maximum Eigenpair with power method. More...
    class  gtsam::AcceleratedPowerMethod< Operator >
     Compute maximum Eigenpair with accelerated power method. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    │ │ │ │ +using gtsam::Sparse = Eigen::SparseMatrix< double >
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Power method for fast eigenvalue and eigenvector computation.

    │ │ │ │ +

    accelerated power method for fast eigenvalue and eigenvector computation

    │ │ │ │
    Date
    Sept 2020
    │ │ │ │
    Author
    Jing Wu
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -PowerMethod.h File Reference │ │ │ │ │ -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 | _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 │ │ │ │ │ -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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSppaarrssee = Eigen::SparseMatrix< double > │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ +accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ Date │ │ │ │ │ Sept 2020 │ │ │ │ │ Author │ │ │ │ │ Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _P_o_w_e_r_M_e_t_h_o_d_._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/a00761_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00149_source.html │ │ │ │┄ Files 25% 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/base/OptionalJacobian.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JacobianFactor.h
    │ │ │ │ +
    OptionalJacobian.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27#include <boost/make_shared.hpp>
    │ │ │ │ -
    28#include <boost/serialization/version.hpp>
    │ │ │ │ -
    29#include <boost/serialization/split_member.hpp>
    │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ -
    32
    │ │ │ │ -
    33 // Forward declarations
    │ │ │ │ -
    34 class HessianFactor;
    │ │ │ │ -
    35 class VariableSlots;
    │ │ │ │ -
    36 class GaussianFactorGraph;
    │ │ │ │ - │ │ │ │ -
    38 class HessianFactor;
    │ │ │ │ -
    39 class VectorValues;
    │ │ │ │ -
    40 class Ordering;
    │ │ │ │ -
    41 class JacobianFactor;
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │
    42
    │ │ │ │ -
    48 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
    │ │ │ │ -
    49 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
    │ │ │ │ +
    43public:
    │ │ │ │ +
    44
    │ │ │ │ +
    47 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
    │ │ │ │ +
    48
    │ │ │ │ +
    49private:
    │ │ │ │
    50
    │ │ │ │ -
    │ │ │ │ -
    90 class GTSAM_EXPORT JacobianFactor : public GaussianFactor
    │ │ │ │ -
    91 {
    │ │ │ │ -
    92 public:
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    71 map_(nullptr) {
    │ │ │ │ +
    72 }
    │ │ │ │ +
    │ │ │ │ +
    73
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    76 map_(nullptr) {
    │ │ │ │ +
    77 usurp(fixed.data());
    │ │ │ │ +
    78 }
    │ │ │ │ +
    │ │ │ │ +
    79
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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>
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    172 if (*this)
    │ │ │ │ +
    173 return OptionalJacobian<Rows, N>(&map_(0,startCol));
    │ │ │ │ +
    174 else
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ -
    225
    │ │ │ │ -
    227 void hessianDiagonalAdd(VectorValues& d) const override;
    │ │ │ │ -
    228
    │ │ │ │ -
    230 void hessianDiagonal(double* d) const override;
    │ │ │ │ +
    199public:
    │ │ │ │ +
    200
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ -
    253 Matrix augmentedJacobianUnweighted() const;
    │ │ │ │ -
    254
    │ │ │ │ -
    256 const VerticalBlockMatrix& matrixObject() const { return Ab_; }
    │ │ │ │ -
    257
    │ │ │ │ - │ │ │ │ -
    260
    │ │ │ │ -
    266 GaussianFactor::shared_ptr negate() const override;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    251 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
    │ │ │ │ +
    252};
    │ │ │ │ +
    │ │ │ │ +
    253
    │ │ │ │ +
    260template<class T, class A>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    264};
    │ │ │ │ +
    │ │ │ │ +
    265
    │ │ │ │ +
    266} // namespace gtsam
    │ │ │ │
    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
    │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A matrix with column blocks of pre-defined sizes.
    │ │ │ │ -
    VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
    │ │ │ │ - │ │ │ │ -
    A factor with a quadratic error function - a Gaussian.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ -
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    Block range(DenseIndex startBlock, DenseIndex endBlock)
    access ranges of blocks at a time
    Definition VerticalBlockMatrix.h:130
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ -
    DenseIndex cols() const
    Column size.
    Definition VerticalBlockMatrix.h:118
    │ │ │ │ -
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ -
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    A combined factor is assembled as one block of rows for each component factor.
    Definition VariableSlots.h:52
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ -
    BVector getb()
    Get a view of the r.h.s.
    Definition JacobianFactor.h:306
    │ │ │ │ -
    const SharedDiagonal & get_model() const
    get a copy of model
    Definition JacobianFactor.h:291
    │ │ │ │ -
    JacobianFactor(const JacobianFactor &jf)
    Copy constructor.
    Definition JacobianFactor.h:114
    │ │ │ │ -
    JacobianFactor This
    Typedef to this class.
    Definition JacobianFactor.h:94
    │ │ │ │ -
    bool isConstrained() const
    is noise model constrained ?
    Definition JacobianFactor.h:269
    │ │ │ │ -
    constABlock getA() const
    Get a view of the A matrix, not weighted by noise.
    Definition JacobianFactor.h:303
    │ │ │ │ -
    GaussianFactor::shared_ptr clone() const override
    Clone this JacobianFactor.
    Definition JacobianFactor.h:189
    │ │ │ │ -
    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
    │ │ │ │ -
    SharedDiagonal & get_model()
    get a copy of model (non-const version)
    Definition JacobianFactor.h:294
    │ │ │ │ -
    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
    │ │ │ │ -
    VerticalBlockMatrix & matrixObject()
    Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
    Definition JacobianFactor.h:259
    │ │ │ │ -
    ABlock getA()
    Get a view of the A matrix.
    Definition JacobianFactor.h:312
    │ │ │ │ -
    GaussianFactor Base
    Typedef to base class.
    Definition JacobianFactor.h:95
    │ │ │ │ -
    ~JacobianFactor() override
    Virtual destructor.
    Definition JacobianFactor.h:186
    │ │ │ │ -
    const VerticalBlockMatrix & matrixObject() const
    Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
    Definition JacobianFactor.h:256
    │ │ │ │ -
    size_t rows() const
    return the number of rows in the corresponding linear system
    Definition JacobianFactor.h:283
    │ │ │ │ -
    size_t cols() const
    return the number of columns in the corresponding linear system
    Definition JacobianFactor.h:288
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ -
    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
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ -
    is the normalization constant.
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
    Constructor compatible with old-style derivatives.
    Definition OptionalJacobian.h:127
    │ │ │ │ +
    OptionalJacobian(boost::none_t)
    Constructor with boost::none just makes empty.
    Definition OptionalJacobian.h:122
    │ │ │ │ +
    OptionalJacobian(Eigen::MatrixXd &dynamic)
    Constructor that will resize a dynamic matrix (unless already correct)
    Definition OptionalJacobian.h:88
    │ │ │ │ +
    Eigen::Map< Jacobian > & operator*()
    De-reference, like boost optional.
    Definition OptionalJacobian.h:148
    │ │ │ │ +
    OptionalJacobian()
    Default constructor acts like boost::none.
    Definition OptionalJacobian.h:70
    │ │ │ │ +
    OptionalJacobian(Jacobian &fixed)
    Constructor that will usurp data of a fixed-size matrix.
    Definition OptionalJacobian.h:75
    │ │ │ │ +
    OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
    Constructor from an Eigen::Ref value.
    Definition OptionalJacobian.h:106
    │ │ │ │ +
    OptionalJacobian(Eigen::MatrixXd *dynamic)
    Constructor that will resize a dynamic matrix (unless already correct)
    Definition OptionalJacobian.h:95
    │ │ │ │ +
    Eigen::Map< Jacobian > * operator->()
    operator->()
    Definition OptionalJacobian.h:153
    │ │ │ │ +
    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
    │ │ │ │ +
    OptionalJacobian(Jacobian *fixedPtr)
    Constructor that will usurp data of a fixed-size matrix, pointer version.
    Definition OptionalJacobian.h:81
    │ │ │ │ +
    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
    │ │ │ │ +
    OptionalJacobian()
    View on constructor argument, if given.
    Definition OptionalJacobian.h:202
    │ │ │ │ +
    OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
    Constructor compatible with old-style derivatives.
    Definition OptionalJacobian.h:220
    │ │ │ │ +
    Eigen::MatrixXd Jacobian
    Jacobian size type.
    Definition OptionalJacobian.h:193
    │ │ │ │ +
    Jacobian & operator*()
    De-reference, like boost optional.
    Definition OptionalJacobian.h:233
    │ │ │ │ +
    OptionalJacobian(boost::none_t)
    Constructor with boost::none just makes empty.
    Definition OptionalJacobian.h:215
    │ │ │ │ +
    OptionalJacobian(Jacobian *pointer)
    Construct from pointer to dynamic matrix.
    Definition OptionalJacobian.h:207
    │ │ │ │ +
    OptionalJacobian(Jacobian &dynamic)
    Construct from refrence to dynamic matrix.
    Definition OptionalJacobian.h:210
    │ │ │ │ +
    Jacobian * operator->()
    TODO: operator->()
    Definition OptionalJacobian.h:238
    │ │ │ │ +
    : meta-function to generate Jacobian
    Definition OptionalJacobian.h:250
    │ │ │ │ +
    : meta-function to generate JacobianTA optional reference Used mainly by Expressions
    Definition OptionalJacobian.h:261
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,502 +1,321 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactor.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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +_4_7 typedef Eigen::Matrix _J_a_c_o_b_i_a_n; │ │ │ │ │ +48 │ │ │ │ │ +49private: │ │ │ │ │ 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: │ │ │ │ │ +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 │ │ │ │ │ +_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_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); │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -_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_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 │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _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/a00767_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00512_source.html │ │ │ │┄ Files 8% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    28
    │ │ │ │ -
    29#include <Eigen/Sparse>
    │ │ │ │ -
    30
    │ │ │ │ -
    31namespace gtsam {
    │ │ │ │ -
    32
    │ │ │ │ -
    35typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
    │ │ │ │ -
    36
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ + │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    A Bayes net of Gaussian Conditionals indexed by discrete keys.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen
    Eigen-format sparse matrix.
    Definition SparseEigen.h:35
    │ │ │ │ -
    SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
    Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
    Definition SparseEigen.h:38
    │ │ │ │ +
    A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
    Definition HybridBayesNet.h:35
    │ │ │ │ +
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ +
    Definition HybridSmoother.h:25
    │ │ │ │ +
    GaussianMixture::shared_ptr gaussianMixture(size_t index) const
    Get the Gaussian Mixture from the Bayes Net posterior at index.
    Definition HybridSmoother.cpp:131
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    const HybridBayesNet & hybridBayesNet() const
    Return the Bayes Net posterior.
    Definition HybridSmoother.cpp:137
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    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/a00773_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00956_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/navigation/TangentPreintegration.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianBayesTree.h
    │ │ │ │ +
    TangentPreintegration.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
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │
    21
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27namespace gtsam {
    │ │ │ │ -
    28
    │ │ │ │ -
    29 // Forward declarations
    │ │ │ │ - │ │ │ │ -
    31 class VectorValues;
    │ │ │ │ -
    32
    │ │ │ │ -
    33 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    35 class GTSAM_EXPORT GaussianBayesTreeClique :
    │ │ │ │ -
    36 public BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>
    │ │ │ │ -
    37 {
    │ │ │ │ -
    38 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    42 typedef boost::weak_ptr<This> weak_ptr;
    │ │ │ │ - │ │ │ │ -
    44 virtual ~GaussianBayesTreeClique() {}
    │ │ │ │ -
    45 GaussianBayesTreeClique(const boost::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
    │ │ │ │ -
    46 };
    │ │ │ │ +
    22namespace gtsam {
    │ │ │ │ +
    23
    │ │ │ │ +
    │ │ │ │ +
    28class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
    │ │ │ │ +
    29 protected:
    │ │ │ │ +
    30
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    38
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    41 resetIntegration();
    │ │ │ │ +
    42 }
    │ │ │ │
    │ │ │ │ +
    43
    │ │ │ │ +
    44public:
    │ │ │ │
    47
    │ │ │ │ -
    48 /* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    50 class GTSAM_EXPORT GaussianBayesTree :
    │ │ │ │ -
    51 public BayesTree<GaussianBayesTreeClique>
    │ │ │ │ -
    52 {
    │ │ │ │ -
    53 private:
    │ │ │ │ - │ │ │ │ +
    53 TangentPreintegration(const boost::shared_ptr<Params>& p,
    │ │ │ │ + │ │ │ │
    55
    │ │ │ │ -
    56 public:
    │ │ │ │ -
    57 typedef GaussianBayesTree This;
    │ │ │ │ -
    58 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    58 }
    │ │ │ │ +
    │ │ │ │
    59
    │ │ │ │ - │ │ │ │ -
    62
    │ │ │ │ -
    64 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ -
    65
    │ │ │ │ -
    67 VectorValues optimize() const;
    │ │ │ │ +
    61
    │ │ │ │ +
    65 void resetIntegration() override;
    │ │ │ │ +
    66
    │ │ │ │
    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 };
    │ │ │ │ +
    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;
    │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    131
    │ │ │ │ -
    133 template<>
    │ │ │ │ -
    │ │ │ │ -
    134 struct traits<GaussianBayesTree> : public Testable<GaussianBayesTree> {
    │ │ │ │ -
    135 };
    │ │ │ │ -
    │ │ │ │ -
    136
    │ │ │ │ -
    137} //\ namespace gtsam
    │ │ │ │ -
    Bayes Tree is a tree of cliques of a Bayes Chain.
    │ │ │ │ -
    Base class for cliques of a BayesTree.
    │ │ │ │ -
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    143};
    │ │ │ │ +
    │ │ │ │ +
    144
    │ │ │ │ +
    145}
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    This is the base class for BayesTree cliques.
    Definition BayesTreeCliqueBase.h:50
    │ │ │ │ -
    A clique in a GaussianBayesTree.
    Definition GaussianBayesTree.h:37
    │ │ │ │ -
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ -
    GaussianBayesTree()
    Default constructor, creates an empty Bayes tree.
    Definition GaussianBayesTree.h:61
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    is the normalization constant.
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │ +
    Integrate on the 9D tangent space of the NavState manifold.
    Definition TangentPreintegration.h:28
    │ │ │ │ +
    Matrix93 preintegrated_H_biasOmega_
    Jacobian of preintegrated_ w.r.t. angular rate bias.
    Definition TangentPreintegration.h:37
    │ │ │ │ +
    ~TangentPreintegration() override
    Virtual destructor.
    Definition TangentPreintegration.h:57
    │ │ │ │ +
    Vector9 preintegrated_
    Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
    Definition TangentPreintegration.h:35
    │ │ │ │ +
    TangentPreintegration()
    Default constructor for serialization.
    Definition TangentPreintegration.h:40
    │ │ │ │ +
    Matrix93 preintegrated_H_biasAcc_
    Jacobian of preintegrated_ w.r.t. acceleration bias.
    Definition TangentPreintegration.h:36
    │ │ │ │ +
    virtual boost::shared_ptr< TangentPreintegration > clone() const
    Dummy clone for MATLAB.
    Definition TangentPreintegration.h:123
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,146 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianBayesTree.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 │ │ │ │ │ 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_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 │ │ │ │ │ -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 }; │ │ │ │ │ +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 } │ │ │ │ │ +43 │ │ │ │ │ +44public: │ │ │ │ │ 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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -_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; │ │ │ │ │ +61 │ │ │ │ │ +65 void resetIntegration() override; │ │ │ │ │ +66 │ │ │ │ │ 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; │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00929.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/navigation/ImuBias.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    GaussianDensity.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    ImuBias.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    A Gaussian Density. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::GaussianDensity
     A GaussianDensity is a GaussianConditional without parents. 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

    │ │ │ │ -

    A Gaussian Density.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    Jan 21, 2012
    │ │ │ │ +
    Date
    Feb 2, 2012
    │ │ │ │ +
    Author
    Vadim Indelman, Stephen Williams
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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_:_:_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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -A Gaussian Density. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Jan 21, 2012 │ │ │ │ │ + Feb 2, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Vadim Indelman, Stephen Williams │ │ │ │ │ * _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 │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _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/a00785_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01382_source.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/symbolic/SymbolicFactor-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianDensity.h
    │ │ │ │ +
    SymbolicFactor-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
    │ │ │ │ -
    19// \callgraph
    │ │ │ │ -
    20#pragma once
    │ │ │ │ -
    21
    │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <gtsam/inference/Key.h>
    │ │ │ │ +
    24#include <gtsam/base/timing.h>
    │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ -
    32 class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
    │ │ │ │ -
    33
    │ │ │ │ -
    34 public:
    │ │ │ │ -
    35
    │ │ │ │ -
    36 typedef boost::shared_ptr<GaussianDensity> shared_ptr;
    │ │ │ │ -
    37
    │ │ │ │ +
    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> >
    │ │ │ │
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    41 }
    │ │ │ │ -
    │ │ │ │ +
    39 EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
    │ │ │ │ +
    40 {
    │ │ │ │ +
    41 gttic(EliminateSymbolic);
    │ │ │ │
    42
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ +
    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 utilities.
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 mean(const CONTAINER &points)
    mean
    Definition Point3.h:68
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    size_t nrParents() const
    return the number of parents
    Definition Conditional.h:113
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    A GaussianDensity is a GaussianConditional without parents.
    Definition GaussianDensity.h:32
    │ │ │ │ -
    GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal &noiseModel=SharedDiagonal())
    constructor using d, R
    Definition GaussianDensity.h:51
    │ │ │ │ -
    GaussianDensity()
    default constructor needed for serialization
    Definition GaussianDensity.h:39
    │ │ │ │ -
    GaussianDensity(const GaussianConditional &conditional)
    Copy constructor from GaussianConditional.
    Definition GaussianDensity.h:44
    │ │ │ │ + │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    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
    │ │ │ │ +
    static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
    Constructor from a collection of keys.
    Definition SymbolicFactor.h:121
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,110 +1,120 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianDensity.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// \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 { │ │ │ │ │ +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> │ │ │ │ │ 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 } │ │ │ │ │ +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 _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. │ │ │ │ │ +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 │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._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 │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00953_source.html │ │ │ │┄ Files 11% similar despite different names │ │ │ │ @@ -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/navigation/ImuBias.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    IterativeSolver.h
    │ │ │ │ +
    ImuBias.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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    78
    │ │ │ │ -
    79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s);
    │ │ │ │ -
    80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v);
    │ │ │ │ -
    81};
    │ │ │ │ -
    │ │ │ │ -
    82
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    87public:
    │ │ │ │ -
    88 typedef boost::shared_ptr<IterativeSolver> shared_ptr;
    │ │ │ │ - │ │ │ │ -
    90 }
    │ │ │ │ -
    91 virtual ~IterativeSolver() {
    │ │ │ │ -
    92 }
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ +
    62
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    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_;
    │ │ │ │ -
    136
    │ │ │ │ -
    137 void initialize(const GaussianFactorGraph &fg);
    │ │ │ │ -
    138
    │ │ │ │ -
    139public:
    │ │ │ │ -
    140
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    143 numCols_(0) {
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
    │ │ │ │ +
    129 }
    │ │ │ │ +
    │ │ │ │ +
    130
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    166 VectorValues x0() const;
    │ │ │ │
    167
    │ │ │ │ -
    169 Vector x0vector() const;
    │ │ │ │ -
    170
    │ │ │ │ -
    171};
    │ │ │ │ -
    │ │ │ │ -
    172
    │ │ │ │ -
    173} // \ namespace gtsam
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    Special class for optional Jacobian arguments.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │ -
    Base class for Iterative Solvers like SubgraphSolver.
    Definition IterativeSolver.h:86
    │ │ │ │ -
    Handy data structure for iterative solvers key to (index, dimension, start)
    Definition IterativeSolver.h:114
    │ │ │ │ -
    Handy data structure for iterative solvers.
    Definition IterativeSolver.h:126
    │ │ │ │ -
    const Ordering & ordering() const
    Return the ordering.
    Definition IterativeSolver.h:158
    │ │ │ │ -
    size_t numCols() const
    Return the total number of columns (scalar variables = sum of dimensions)
    Definition IterativeSolver.h:153
    │ │ │ │ -
    KeyInfo()
    Default Constructor.
    Definition IterativeSolver.h:142
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    VectorSpace provides both Testable and VectorSpaceTraits.
    Definition VectorSpace.h:207
    │ │ │ │ +
    Definition ImuBias.h:30
    │ │ │ │ +
    const Vector3 & gyroscope() const
    get gyroscope bias
    Definition ImuBias.h:69
    │ │ │ │ +
    static ConstantBias Identity()
    identity for group operation
    Definition ImuBias.h:112
    │ │ │ │ +
    bool equals(const ConstantBias &expected, double tol=1e-5) const
    equality up to tolerance
    Definition ImuBias.h:102
    │ │ │ │ +
    ConstantBias operator-(const ConstantBias &b) const
    subtraction
    Definition ImuBias.h:132
    │ │ │ │ +
    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
    │ │ │ │ +
    ConstantBias operator-() const
    inverse
    Definition ImuBias.h:117
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector6 vector() const
    return the accelerometer and gyro biases in a single vector
    Definition ImuBias.h:57
    │ │ │ │ +
    ConstantBias operator+(const Vector6 &v) const
    addition of vector on right
    Definition ImuBias.h:122
    │ │ │ │ +
    ConstantBias operator+(const ConstantBias &b) const
    addition
    Definition ImuBias.h:127
    │ │ │ │ +
    const Vector3 & accelerometer() const
    get accelerometer bias
    Definition ImuBias.h:64
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,219 +1,245 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -IterativeSolver.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 │ │ │ │ │ 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_/_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 { │ │ │ │ │ +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 { │ │ │ │ │ +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 │ │ │ │ │ -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 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 │ │ │ │ │ -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() { │ │ │ │ │ -90 } │ │ │ │ │ -91 virtual _~_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r() { │ │ │ │ │ -92 } │ │ │ │ │ +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 } │ │ │ │ │ +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 } │ │ │ │ │ +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 │ │ │ │ │ -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_; │ │ │ │ │ -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) { │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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 │ │ │ │ │ +_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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _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/a00812.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00884.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/KalmanFilter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
    │ │ │ │ -
    AcceleratedPowerMethod.h File Reference
    │ │ │ │ +Macros
    │ │ │ │ +
    KalmanFilter.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ +

    Simple linear Kalman filter. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::AcceleratedPowerMethod< Operator >
     Compute maximum Eigenpair with accelerated power method. More...
    class  gtsam::KalmanFilter
     Kalman Filter class. More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    │ │ │ │ -Typedefs

    │ │ │ │ -using gtsam::Sparse = Eigen::SparseMatrix< double >
     

    │ │ │ │ +Macros

    │ │ │ │ +#define KALMANFILTER_DEFAULT_FACTORIZATION   QR
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    accelerated power method for fast eigenvalue and eigenvector computation

    │ │ │ │ -
    Date
    Sept 2020
    │ │ │ │ -
    Author
    Jing Wu
    │ │ │ │ +

    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,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 | _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 | _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_:_:_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_:_:_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. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSppaarrssee = Eigen::SparseMatrix< double > │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  KKAALLMMAANNFFIILLTTEERR__DDEEFFAAUULLTT__FFAACCTTOORRIIZZAATTIIOONN   QR │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ +Simple linear Kalman filter. │ │ │ │ │ +Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ Date │ │ │ │ │ - Sept 2020 │ │ │ │ │ + Sep 3, 2011 │ │ │ │ │ Author │ │ │ │ │ - Jing Wu │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00812_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00923_source.html │ │ │ │┄ Files 14% similar despite different names │ │ │ │ @@ -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/navigation/PreintegrationParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    AcceleratedPowerMethod.h
    │ │ │ │ +
    PreintegrationParams.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
    │ │ │ │ +
    17#pragma once
    │ │ │ │ +
    18
    │ │ │ │ + │ │ │ │ +
    20#include <boost/make_shared.hpp>
    │ │ │ │
    21
    │ │ │ │ - │ │ │ │ +
    22namespace gtsam {
    │ │ │ │
    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:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    32 Vector3 n_gravity;
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    37 accelerometerCovariance(I_3x3),
    │ │ │ │ +
    38 integrationCovariance(I_3x3),
    │ │ │ │ +
    39 use2ndOrderCoriolis(false),
    │ │ │ │ +
    40 n_gravity(0, 0, -1) {}
    │ │ │ │ +
    │ │ │ │ +
    41
    │ │ │ │ +
    │ │ │ │ +
    44 PreintegrationParams(const Vector3& n_gravity)
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    73protected:
    │ │ │ │
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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
    │ │ │ │ -
    Power method for fast eigenvalue and eigenvector computation.
    │ │ │ │ +
    87#ifdef GTSAM_USE_QUATERNIONS
    │ │ │ │ +
    88 // Align if we are using Quaternions
    │ │ │ │ +
    89public:
    │ │ │ │ + │ │ │ │ +
    91#endif
    │ │ │ │ +
    92};
    │ │ │ │ +
    │ │ │ │ +
    93
    │ │ │ │ +
    94} // namespace gtsam
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Compute maximum Eigenpair with accelerated power method.
    Definition AcceleratedPowerMethod.h:51
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector acceleratedPowerIteration() const
    Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
    Definition AcceleratedPowerMethod.h:92
    │ │ │ │ -
    bool compute(size_t maxIterations, double tol)
    Start the accelerated iteration, after performing the accelerated iteration, calculate the ritz error...
    Definition AcceleratedPowerMethod.h:156
    │ │ │ │ -
    Compute maximum Eigenpair with power method.
    Definition PowerMethod.h:57
    │ │ │ │ -
    const Operator & A_
    Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
    Definition PowerMethod.h:63
    │ │ │ │ -
    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
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegratedRotation.h:31
    │ │ │ │ +
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegrationParams.h:26
    │ │ │ │ +
    Matrix3 accelerometerCovariance
    Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/s²/√Hz.
    Definition PreintegrationParams.h:29
    │ │ │ │ +
    bool use2ndOrderCoriolis
    Whether to use second order Coriolis integration.
    Definition PreintegrationParams.h:31
    │ │ │ │ +
    PreintegrationParams()
    Default constructor for serialization only.
    Definition PreintegrationParams.h:35
    │ │ │ │ +
    Vector3 n_gravity
    Gravity vector in nav frame.
    Definition PreintegrationParams.h:32
    │ │ │ │ +
    PreintegrationParams(const Vector3 &n_gravity)
    The Params constructor insists on getting the navigation frame gravity vector For convenience,...
    Definition PreintegrationParams.h:44
    │ │ │ │ +
    Matrix3 integrationCovariance
    continuous-time "Covariance" describing integration uncertainty
    Definition PreintegrationParams.h:30
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,151 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AcceleratedPowerMethod.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-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 │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ 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_); │ │ │ │ │ -70 │ │ │ │ │ -71 // initialize beta_ │ │ │ │ │ -72 beta_ = initialBeta; │ │ │ │ │ -73 } │ │ │ │ │ +_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 } │ │ │ │ │ +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 │ │ │ │ │ +73protected: │ │ │ │ │ 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; │ │ │ │ │ +_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 │ │ │ │ │ -_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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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/a00821_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00806_source.html │ │ │ │┄ Files 0% 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
    │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │
    23
    │ │ │ │
    24#include <boost/optional.hpp>
    │ │ │ │
    25#include <boost/shared_ptr.hpp>
    │ │ │ │
    26
    │ │ │ │
    27namespace gtsam
    │ │ │ │
    28{
    │ │ │ │ @@ -251,16 +251,16 @@ │ │ │ │
    152 treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor);
    │ │ │ │
    153 return preVisitor.collectedResult;
    │ │ │ │
    154 }
    │ │ │ │
    155 }
    │ │ │ │
    156 }
    │ │ │ │
    157}
    │ │ │ │ │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ +
    Conditional Gaussian Base class.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │
    An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
    Definition types.h:192
    │ │ │ │ @@ -281,13 +281,13 @@ │ │ │ │
    Values::const_iterator const_iterator
    Const iterator over vector values.
    Definition VectorValues.h:82
    │ │ │ │
    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/a00824_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00650_source.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/inference/Conditional-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JacobianFactor-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
    │ │ │ │ +
    18// \callgraph
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ + │ │ │ │
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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 // Assign RHS vector
    │ │ │ │ -
    101 getb() = b;
    │ │ │ │ -
    102
    │ │ │ │ -
    103 // Assign noise model
    │ │ │ │ -
    104 model_ = noiseModel;
    │ │ │ │ -
    105 }
    │ │ │ │ -
    │ │ │ │ -
    106
    │ │ │ │ -
    107} // gtsam
    │ │ │ │ -
    108
    │ │ │ │ -
    Exceptions that may be thrown by linear solver components.
    │ │ │ │ +
    23#include <cmath>
    │ │ │ │ +
    24#include <iostream>
    │ │ │ │ +
    25
    │ │ │ │ +
    26namespace gtsam {
    │ │ │ │ +
    27
    │ │ │ │ +
    28/* ************************************************************************* */
    │ │ │ │ +
    29template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    42 double tol) const {
    │ │ │ │ +
    43 return nrFrontals_ == c.nrFrontals_;
    │ │ │ │ +
    44}
    │ │ │ │ +
    │ │ │ │ +
    45
    │ │ │ │ +
    46/* ************************************************************************* */
    │ │ │ │ +
    47template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    49 const HybridValues& c) const {
    │ │ │ │ +
    50 throw std::runtime_error("Conditional::logProbability is not implemented");
    │ │ │ │ +
    51}
    │ │ │ │ +
    │ │ │ │ +
    52
    │ │ │ │ +
    53/* ************************************************************************* */
    │ │ │ │ +
    54template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    56 const HybridValues& c) const {
    │ │ │ │ +
    57 throw std::runtime_error("Conditional::evaluate is not implemented");
    │ │ │ │ +
    58}
    │ │ │ │ +
    │ │ │ │ +
    59
    │ │ │ │ +
    60/* ************************************************************************* */
    │ │ │ │ +
    61template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    63 const {
    │ │ │ │ +
    64 throw std::runtime_error(
    │ │ │ │ +
    65 "Conditional::logNormalizationConstant is not implemented");
    │ │ │ │ +
    66}
    │ │ │ │ +
    │ │ │ │ +
    67
    │ │ │ │ +
    68/* ************************************************************************* */
    │ │ │ │ +
    69template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    71 return std::exp(logNormalizationConstant());
    │ │ │ │ +
    72}
    │ │ │ │ +
    │ │ │ │ +
    73
    │ │ │ │ +
    74/* ************************************************************************* */
    │ │ │ │ +
    75template <class FACTOR, class DERIVEDCONDITIONAL>
    │ │ │ │ +
    76template <class VALUES>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    98} // namespace gtsam
    │ │ │ │ +
    │ │ │ │ +
    Base class for conditional densities.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
    Definition VerticalBlockMatrix.h:43
    │ │ │ │ -
    DenseIndex rows() const
    Row size.
    Definition VerticalBlockMatrix.h:115
    │ │ │ │ -
    DenseIndex cols() const
    Column size.
    Definition VerticalBlockMatrix.h:118
    │ │ │ │ -
    DenseIndex nBlocks() const
    Block count.
    Definition VerticalBlockMatrix.h:121
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ -
    An abstract virtual base class for JacobianFactor and HessianFactor.
    Definition GaussianFactor.h:39
    │ │ │ │ -
    const constBVector getb() const
    Get a view of the r.h.s.
    Definition JacobianFactor.h:297
    │ │ │ │ -
    JacobianFactor()
    default constructor for I/O
    Definition JacobianFactor.cpp:54
    │ │ │ │ -
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ -
    An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
    Definition linearExceptions.h:106
    │ │ │ │ -
    An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
    Definition linearExceptions.h:124
    │ │ │ │ +
    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
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
    Check invariants of this conditional, given the values x.
    Definition Conditional-inst.h:77
    │ │ │ │ +
    size_t nrFrontals_
    The first nrFrontal variables are frontal and the rest are parents.
    Definition Conditional.h:67
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    bool equals(const This &c, double tol=1e-9) const
    check equality
    Definition Conditional-inst.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    double normalizationConstant() const
    Non-virtual, exponentiate logNormalizationConstant.
    Definition Conditional-inst.h:70
    │ │ │ │ +
    void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print with optional formatter
    Definition Conditional-inst.h:30
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,183 +1,167 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactor-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 │ │ │ │ │ +18// \callgraph │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -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 // 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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorr--iinnll..hh │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * 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/a00830_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01247_source.html │ │ │ │┄ Files 27% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    VectorValues.h
    │ │ │ │ +
    dataset.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/Vector.h>
    │ │ │ │ -
    23#include <gtsam/base/ConcurrentMap.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    26
    │ │ │ │ -
    27#include <boost/shared_ptr.hpp>
    │ │ │ │ -
    28
    │ │ │ │ -
    29
    │ │ │ │ -
    30#include <map>
    │ │ │ │ -
    31#include <string>
    │ │ │ │ -
    32#include <iosfwd>
    │ │ │ │ -
    33
    │ │ │ │ -
    34namespace gtsam {
    │ │ │ │ -
    35
    │ │ │ │ -
    │ │ │ │ -
    74 class GTSAM_EXPORT VectorValues {
    │ │ │ │ -
    75 protected:
    │ │ │ │ -
    76 typedef VectorValues This;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    79
    │ │ │ │ -
    80 public:
    │ │ │ │ -
    81 typedef Values::iterator iterator;
    │ │ │ │ -
    82 typedef Values::const_iterator const_iterator;
    │ │ │ │ -
    83 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    86 typedef std::map<Key, size_t> Dims;
    │ │ │ │ -
    87
    │ │ │ │ -
    90
    │ │ │ │ - │ │ │ │ -
    93
    │ │ │ │ -
    │ │ │ │ -
    95 VectorValues(std::initializer_list<std::pair<Key, Vector>> init)
    │ │ │ │ -
    96 : values_(init.begin(), init.end()) {}
    │ │ │ │ -
    │ │ │ │ -
    97
    │ │ │ │ -
    100 VectorValues(const VectorValues& first, const VectorValues& second);
    │ │ │ │ -
    101
    │ │ │ │ -
    103 template<class CONTAINER>
    │ │ │ │ -
    104 explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
    │ │ │ │ -
    105
    │ │ │ │ -
    107 VectorValues(const VectorValues& c) : values_(c.values_) {}
    │ │ │ │ -
    108
    │ │ │ │ -
    110 template<typename ITERATOR>
    │ │ │ │ -
    111 VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
    │ │ │ │ -
    112
    │ │ │ │ -
    114 VectorValues(const Vector& c, const Dims& dims);
    │ │ │ │ +
    21#pragma once
    │ │ │ │ +
    22
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    25#include <gtsam/sfm/SfmData.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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    72};
    │ │ │ │ +
    │ │ │ │ +
    73
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │
    115
    │ │ │ │ -
    117 VectorValues(const Vector& c, const Scatter& scatter);
    │ │ │ │ -
    118
    │ │ │ │ -
    120 static VectorValues Zero(const VectorValues& other);
    │ │ │ │ -
    121
    │ │ │ │ -
    125
    │ │ │ │ -
    127 size_t size() const { return values_.size(); }
    │ │ │ │ -
    128
    │ │ │ │ -
    130 size_t dim(Key j) const { return at(j).rows(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    133 bool exists(Key j) const { return find(j) != end(); }
    │ │ │ │ -
    134
    │ │ │ │ -
    │ │ │ │ -
    139 Vector& at(Key j) {
    │ │ │ │ -
    140 iterator item = find(j);
    │ │ │ │ -
    141 if (item == end())
    │ │ │ │ -
    142 throw std::out_of_range(
    │ │ │ │ -
    143 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
    │ │ │ │ -
    144 else
    │ │ │ │ -
    145 return item->second;
    │ │ │ │ -
    146 }
    │ │ │ │ -
    │ │ │ │ -
    147
    │ │ │ │ -
    │ │ │ │ -
    152 const Vector& at(Key j) const {
    │ │ │ │ -
    153 const_iterator item = find(j);
    │ │ │ │ -
    154 if (item == end())
    │ │ │ │ -
    155 throw std::out_of_range(
    │ │ │ │ -
    156 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
    │ │ │ │ -
    157 else
    │ │ │ │ -
    158 return item->second;
    │ │ │ │ -
    159 }
    │ │ │ │ -
    │ │ │ │ -
    160
    │ │ │ │ -
    163 Vector& operator[](Key j) { return at(j); }
    │ │ │ │ -
    164
    │ │ │ │ -
    167 const Vector& operator[](Key j) const { return at(j); }
    │ │ │ │ -
    168
    │ │ │ │ -
    172 VectorValues& update(const VectorValues& values);
    │ │ │ │ -
    173
    │ │ │ │ -
    178 iterator insert(const std::pair<Key, Vector>& key_value);
    │ │ │ │ -
    179
    │ │ │ │ -
    184 template<class... Args>
    │ │ │ │ -
    │ │ │ │ -
    185 inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
    │ │ │ │ -
    186#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
    │ │ │ │ -
    187 return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
    │ │ │ │ -
    188#else
    │ │ │ │ -
    189 return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
    │ │ │ │ -
    190#endif
    │ │ │ │ -
    191 }
    │ │ │ │ -
    │ │ │ │ -
    192
    │ │ │ │ -
    │ │ │ │ -
    197 iterator insert(Key j, const Vector& value) {
    │ │ │ │ -
    198 return insert(std::make_pair(j, value));
    │ │ │ │ -
    199 }
    │ │ │ │ -
    │ │ │ │ -
    200
    │ │ │ │ -
    203 VectorValues& insert(const VectorValues& values);
    │ │ │ │ -
    204
    │ │ │ │ -
    │ │ │ │ -
    209 inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
    │ │ │ │ -
    210#ifdef TBB_GREATER_EQUAL_2020
    │ │ │ │ -
    211 return values_.emplace(j, value);
    │ │ │ │ -
    212#else
    │ │ │ │ -
    213 return values_.insert(std::make_pair(j, value));
    │ │ │ │ -
    214#endif
    │ │ │ │ -
    215 }
    │ │ │ │ -
    │ │ │ │ -
    216
    │ │ │ │ -
    │ │ │ │ -
    219 void insert_or_assign(Key j, const Vector& value) {
    │ │ │ │ -
    220 if (!tryInsert(j, value).second) {
    │ │ │ │ -
    221 (*this)[j] = value;
    │ │ │ │ -
    222 }
    │ │ │ │ -
    223 }
    │ │ │ │ -
    │ │ │ │ +
    137GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
    │ │ │ │ +
    138 const std::string& tag);
    │ │ │ │ +
    139
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    226 void erase(Key var) {
    │ │ │ │ -
    227 if (values_.unsafe_erase(var) == 0)
    │ │ │ │ -
    228 throw std::invalid_argument("Requested variable '" +
    │ │ │ │ -
    229 DefaultKeyFormatter(var) +
    │ │ │ │ -
    230 "', is not in this VectorValues.");
    │ │ │ │ -
    231 }
    │ │ │ │ -
    │ │ │ │ -
    232
    │ │ │ │ -
    234 void setZero();
    │ │ │ │ -
    235
    │ │ │ │ -
    236 iterator begin() { return values_.begin(); }
    │ │ │ │ -
    │ │ │ │ -
    237 const_iterator begin() const { return values_.begin(); }
    │ │ │ │ -
    │ │ │ │ -
    238 iterator end() { return values_.end(); }
    │ │ │ │ -
    │ │ │ │ -
    239 const_iterator end() const { return values_.end(); }
    │ │ │ │ +
    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
    │ │ │ │ -
    245 iterator find(Key j) { return values_.find(j); }
    │ │ │ │ -
    246
    │ │ │ │ -
    251 const_iterator find(Key j) const { return values_.find(j); }
    │ │ │ │ -
    252
    │ │ │ │ -
    254 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
    │ │ │ │ -
    255
    │ │ │ │ -
    257 void print(const std::string& str = "VectorValues",
    │ │ │ │ -
    258 const KeyFormatter& formatter = DefaultKeyFormatter) const;
    │ │ │ │ -
    259
    │ │ │ │ -
    261 bool equals(const VectorValues& x, double tol = 1e-9) const;
    │ │ │ │ -
    262
    │ │ │ │ -
    266
    │ │ │ │ -
    268 Vector vector() const;
    │ │ │ │ -
    269
    │ │ │ │ -
    271 template <typename CONTAINER>
    │ │ │ │ -
    │ │ │ │ -
    272 Vector vector(const CONTAINER& keys) const {
    │ │ │ │ -
    273 DenseIndex totalDim = 0;
    │ │ │ │ - │ │ │ │ -
    275 items.reserve(keys.end() - keys.begin());
    │ │ │ │ -
    276 for (Key key : keys) {
    │ │ │ │ -
    277 const Vector* v = &at(key);
    │ │ │ │ -
    278 totalDim += v->size();
    │ │ │ │ -
    279 items.push_back(v);
    │ │ │ │ -
    280 }
    │ │ │ │ -
    281
    │ │ │ │ -
    282 Vector result(totalDim);
    │ │ │ │ -
    283 DenseIndex pos = 0;
    │ │ │ │ -
    284 for (const Vector* v : items) {
    │ │ │ │ -
    285 result.segment(pos, v->size()) = *v;
    │ │ │ │ -
    286 pos += v->size();
    │ │ │ │ -
    287 }
    │ │ │ │ -
    288
    │ │ │ │ -
    289 return result;
    │ │ │ │ -
    290 }
    │ │ │ │ -
    │ │ │ │ -
    291
    │ │ │ │ -
    293 Vector vector(const Dims& dims) const;
    │ │ │ │ -
    294
    │ │ │ │ -
    296 void swap(VectorValues& other);
    │ │ │ │ -
    297
    │ │ │ │ -
    299 bool hasSameStructure(const VectorValues other) const;
    │ │ │ │ -
    300
    │ │ │ │ -
    304
    │ │ │ │ -
    308 double dot(const VectorValues& v) const;
    │ │ │ │ -
    309
    │ │ │ │ -
    311 double norm() const;
    │ │ │ │ -
    312
    │ │ │ │ -
    314 double squaredNorm() const;
    │ │ │ │ -
    315
    │ │ │ │ -
    318 VectorValues operator+(const VectorValues& c) const;
    │ │ │ │ -
    319
    │ │ │ │ -
    322 VectorValues add(const VectorValues& c) const;
    │ │ │ │ -
    323
    │ │ │ │ -
    326 VectorValues& operator+=(const VectorValues& c);
    │ │ │ │ -
    327
    │ │ │ │ -
    330 VectorValues& addInPlace(const VectorValues& c);
    │ │ │ │ -
    331
    │ │ │ │ -
    333 VectorValues& addInPlace_(const VectorValues& c);
    │ │ │ │ -
    334
    │ │ │ │ -
    337 VectorValues operator-(const VectorValues& c) const;
    │ │ │ │ -
    338
    │ │ │ │ -
    341 VectorValues subtract(const VectorValues& c) const;
    │ │ │ │ -
    342
    │ │ │ │ -
    344 friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);
    │ │ │ │ -
    345
    │ │ │ │ -
    347 VectorValues scale(const double a) const;
    │ │ │ │ -
    348
    │ │ │ │ -
    350 VectorValues& operator*=(double alpha);
    │ │ │ │ -
    351
    │ │ │ │ -
    353 VectorValues& scaleInPlace(double alpha);
    │ │ │ │ -
    354
    │ │ │ │ -
    356
    │ │ │ │ -
    359
    │ │ │ │ -
    365 std::string html(
    │ │ │ │ -
    366 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │ -
    367
    │ │ │ │ -
    369
    │ │ │ │ -
    370 private:
    │ │ │ │ -
    372 friend class boost::serialization::access;
    │ │ │ │ -
    373 template<class ARCHIVE>
    │ │ │ │ -
    374 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    375 ar & BOOST_SERIALIZATION_NVP(values_);
    │ │ │ │ -
    376 }
    │ │ │ │ -
    377 }; // VectorValues definition
    │ │ │ │ -
    378
    │ │ │ │ -
    380 template<>
    │ │ │ │ -
    │ │ │ │ -
    381 struct traits<VectorValues> : public Testable<VectorValues> {
    │ │ │ │ -
    382 };
    │ │ │ │ -
    │ │ │ │ -
    383
    │ │ │ │ -
    384} // \namespace gtsam
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Variable ordering for the elimination algorithm.
    │ │ │ │ -
    Maps global variable indices to slot indices.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ -
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ +
    Calibration used by Bundler.
    │ │ │ │ +
    Base class for all pinhole cameras.
    │ │ │ │ +
    2D Pose
    │ │ │ │ +
    3D Pose
    │ │ │ │ + │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ +
    Data structure for dealing with Structure from Motion data.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
    Free version of html.
    Definition DiscreteValues.cpp:134
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    Errors operator+(const Errors &a, const Errors &b)
    Addition.
    Definition Errors.cpp:60
    │ │ │ │ -
    Point2 operator*(double s, const Point2 &p)
    multiply with scalar
    Definition Point2.h:47
    │ │ │ │ -
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Errors operator-(const Errors &a, const Errors &b)
    Subtraction.
    Definition Errors.cpp:75
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition ConcurrentMap.h:68
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    value_type KeyValuePair
    Typedef to pair<Key, Vector>
    Definition VectorValues.h:85
    │ │ │ │ -
    const_iterator end() const
    Iterator over variables.
    Definition VectorValues.h:239
    │ │ │ │ -
    Values::value_type value_type
    Typedef to pair<Key, Vector>
    Definition VectorValues.h:84
    │ │ │ │ -
    Values::const_iterator const_iterator
    Const iterator over vector values.
    Definition VectorValues.h:82
    │ │ │ │ -
    iterator end()
    Iterator over variables.
    Definition VectorValues.h:238
    │ │ │ │ -
    Values::iterator iterator
    Iterator over vector values.
    Definition VectorValues.h:81
    │ │ │ │ -
    iterator insert(Key j, const Vector &value)
    Insert a vector value with key j.
    Definition VectorValues.h:197
    │ │ │ │ -
    VectorValues(const CONTAINER &c)
    Create from another container holding pair<Key,Vector>.
    Definition VectorValues.h:104
    │ │ │ │ -
    Values values_
    Vectors making up this VectorValues.
    Definition VectorValues.h:78
    │ │ │ │ -
    std::map< Key, size_t > Dims
    Keyed vector dimensions.
    Definition VectorValues.h:86
    │ │ │ │ -
    ConcurrentMap< Key, Vector > Values
    Collection of Vectors making up a VectorValues.
    Definition VectorValues.h:77
    │ │ │ │ -
    Vector & operator[](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:163
    │ │ │ │ -
    std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
    Emplace a vector value with key j.
    Definition VectorValues.h:185
    │ │ │ │ -
    iterator find(Key j)
    Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
    Definition VectorValues.h:245
    │ │ │ │ -
    size_t dim(Key j) const
    Return the dimension of variable j.
    Definition VectorValues.h:130
    │ │ │ │ -
    size_t size() const
    Number of variables stored.
    Definition VectorValues.h:127
    │ │ │ │ -
    const Vector & operator[](Key j) const
    Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
    Definition VectorValues.h:167
    │ │ │ │ -
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │ -
    VectorValues()
    Default constructor creates an empty VectorValues.
    Definition VectorValues.h:92
    │ │ │ │ -
    const_iterator find(Key j) const
    Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
    Definition VectorValues.h:251
    │ │ │ │ -
    VectorValues(ITERATOR first, ITERATOR last)
    Create from a pair of iterators over pair<Key,Vector>.
    Definition VectorValues.h:111
    │ │ │ │ -
    const Vector & at(Key j) const
    Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
    Definition VectorValues.h:152
    │ │ │ │ -
    Vector vector(const CONTAINER &keys) const
    Access a vector that is a subset of relevant keys.
    Definition VectorValues.h:272
    │ │ │ │ -
    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
    │ │ │ │ -
    const_iterator begin() const
    Iterator over variables.
    Definition VectorValues.h:237
    │ │ │ │ -
    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
    │ │ │ │ -
    void erase(Key var)
    Erase the vector with the given key, or throw std::out_of_range if it does not exist.
    Definition VectorValues.h:226
    │ │ │ │ -
    VectorValues(std::initializer_list< std::pair< Key, Vector > > init)
    Construct from initializer list.
    Definition VectorValues.h:95
    │ │ │ │ -
    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
    │ │ │ │ -
    VectorValues(const VectorValues &c)
    Implicit copy constructor to specialize the explicit constructor from any container.
    Definition VectorValues.h:107
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition VectorValues.h:83
    │ │ │ │ -
    iterator begin()
    Iterator over variables.
    Definition VectorValues.h:236
    │ │ │ │ -
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ +
    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_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.
    │ │ │ │ +
    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
    │ │ │ │ +
    std::pair< size_t, Pose2 > IndexedPose
    Return type for auxiliary functions.
    Definition dataset.h:112
    │ │ │ │ +
    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_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.
    │ │ │ │ +
    KernelFunctionType
    Robust kernel type to wrap around quadratic noise model.
    Definition dataset.h:75
    │ │ │ │ +
    NoiseFormat
    Indicates how noise parameters are stored in file.
    Definition dataset.h:66
    │ │ │ │ +
    @ NoiseFormatGRAPH
    default: toro-style order, but covariance matrix !
    Definition dataset.h:69
    │ │ │ │ +
    @ NoiseFormatAUTO
    Try to guess covariance matrix layout.
    Definition dataset.h:71
    │ │ │ │ +
    @ NoiseFormatTORO
    Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
    Definition dataset.h:68
    │ │ │ │ +
    @ NoiseFormatCOV
    Covariance matrix C11, C12, C13, C22, C23, C33.
    Definition dataset.h:70
    │ │ │ │ +
    @ NoiseFormatG2O
    Information matrix I11, I12, I13, I22, I23, I33.
    Definition dataset.h:67
    │ │ │ │ +
    boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
    Parse G2O landmark vertex "id x y".
    Definition dataset.cpp:187
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
    Parse TORO/G2O edge "id1 id2 x y yaw".
    Definition dataset.cpp:293
    │ │ │ │ +
    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
    │ │ │ │ +
    boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
    Parse TORO/G2O vertex "id x y yaw".
    Definition dataset.cpp:167
    │ │ │ │ +
    GraphAndValues load3D(const string &filename)
    Load TORO 3D Graph.
    Definition dataset.cpp:918
    │ │ │ │ +
    void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
    save 2d graph
    Definition dataset.cpp:584
    │ │ │ │ +
    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
    │ │ │ │ +
    string findExampleDataFile(const string &name)
    Find the full path to an example dataset distributed with gtsam.
    Definition dataset.cpp:62
    │ │ │ │ +
    A 2D pose (Point2,Rot2)
    Definition Pose2.h:36
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,468 +1,295 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VectorValues.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_/_l_i_n_e_a_r_/_S_c_a_t_t_e_r_._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_/_V_e_c_t_o_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33 │ │ │ │ │ -34namespace _g_t_s_a_m { │ │ │ │ │ -35 │ │ │ │ │ -_7_4 class GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s { │ │ │ │ │ -75 protected: │ │ │ │ │ -76 typedef _V_e_c_t_o_r_V_a_l_u_e_s _T_h_i_s; │ │ │ │ │ -_7_7 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_e_y_,_ _V_e_c_t_o_r_> _V_a_l_u_e_s; │ │ │ │ │ -_7_8 _V_a_l_u_e_s _v_a_l_u_e_s__; │ │ │ │ │ -79 │ │ │ │ │ -80 public: │ │ │ │ │ -_8_1 typedef Values::iterator _i_t_e_r_a_t_o_r; │ │ │ │ │ -_8_2 typedef Values::const_iterator _c_o_n_s_t___i_t_e_r_a_t_o_r; │ │ │ │ │ -_8_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_8_4 typedef _V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e _v_a_l_u_e___t_y_p_e; │ │ │ │ │ -_8_5 typedef _v_a_l_u_e___t_y_p_e _K_e_y_V_a_l_u_e_P_a_i_r; │ │ │ │ │ -_8_6 typedef std::map _D_i_m_s; │ │ │ │ │ -87 │ │ │ │ │ -90 │ │ │ │ │ -_9_2 _V_e_c_t_o_r_V_a_l_u_e_s() {} │ │ │ │ │ -93 │ │ │ │ │ -_9_5 _V_e_c_t_o_r_V_a_l_u_e_s(std::initializer_list> init) │ │ │ │ │ -96 : values_(init.begin(), init.end()) {} │ │ │ │ │ -97 │ │ │ │ │ -100 _V_e_c_t_o_r_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& first, const _V_e_c_t_o_r_V_a_l_u_e_s& second); │ │ │ │ │ -101 │ │ │ │ │ -103 template │ │ │ │ │ -_1_0_4 explicit _V_e_c_t_o_r_V_a_l_u_e_s(const CONTAINER& c) : values_(c.begin(), c.end()) {} │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 _V_e_c_t_o_r_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& c) : values_(c.values_) {} │ │ │ │ │ -108 │ │ │ │ │ -110 template │ │ │ │ │ -_1_1_1 _V_e_c_t_o_r_V_a_l_u_e_s(ITERATOR first, ITERATOR last) : values_(first, last) {} │ │ │ │ │ -112 │ │ │ │ │ -114 _V_e_c_t_o_r_V_a_l_u_e_s(const Vector& c, const Dims& dims); │ │ │ │ │ +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 { │ │ │ │ │ +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_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 │ │ │ │ │ -117 _V_e_c_t_o_r_V_a_l_u_e_s(const Vector& c, const _S_c_a_t_t_e_r& scatter); │ │ │ │ │ -118 │ │ │ │ │ -120 static _V_e_c_t_o_r_V_a_l_u_e_s Zero(const _V_e_c_t_o_r_V_a_l_u_e_s& other); │ │ │ │ │ -121 │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 size_t _s_i_z_e() const { return values_.size(); } │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 size_t _d_i_m(_K_e_y j) const { return at(j).rows(); } │ │ │ │ │ +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 │ │ │ │ │ -_1_3_3 bool _e_x_i_s_t_s(_K_e_y j) const { return find(j) != end(); } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_9 Vector& _a_t(_K_e_y j) { │ │ │ │ │ -140 _i_t_e_r_a_t_o_r item = find(j); │ │ │ │ │ -141 if (item == end()) │ │ │ │ │ -142 throw std::out_of_range( │ │ │ │ │ -143 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this │ │ │ │ │ -VectorValues."); │ │ │ │ │ -144 else │ │ │ │ │ -145 return item->second; │ │ │ │ │ -146 } │ │ │ │ │ -147 │ │ │ │ │ -_1_5_2 const Vector& _a_t(_K_e_y j) const { │ │ │ │ │ -153 _c_o_n_s_t___i_t_e_r_a_t_o_r item = find(j); │ │ │ │ │ -154 if (item == end()) │ │ │ │ │ -155 throw std::out_of_range( │ │ │ │ │ -156 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this │ │ │ │ │ -VectorValues."); │ │ │ │ │ -157 else │ │ │ │ │ -158 return item->second; │ │ │ │ │ -159 } │ │ │ │ │ -160 │ │ │ │ │ -_1_6_3 Vector& _o_p_e_r_a_t_o_r_[_](_K_e_y j) { return at(j); } │ │ │ │ │ -164 │ │ │ │ │ -_1_6_7 const Vector& _o_p_e_r_a_t_o_r_[_](_K_e_y j) const { return at(j); } │ │ │ │ │ -168 │ │ │ │ │ -172 _V_e_c_t_o_r_V_a_l_u_e_s& update(const _V_e_c_t_o_r_V_a_l_u_e_s& values); │ │ │ │ │ -173 │ │ │ │ │ -178 iterator insert(const std::pair& key_value); │ │ │ │ │ -179 │ │ │ │ │ -184 template │ │ │ │ │ -_1_8_5 inline std::pair _e_m_p_l_a_c_e(_K_e_y j, Args&&... │ │ │ │ │ -args) { │ │ │ │ │ -186#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020) │ │ │ │ │ -187 return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), │ │ │ │ │ -std::forward_as_tuple(args...)); │ │ │ │ │ -188#else │ │ │ │ │ -189 return values_.insert(std::make_pair(j, Vector(std::forward │ │ │ │ │ -(args)...))); │ │ │ │ │ -190#endif │ │ │ │ │ -191 } │ │ │ │ │ -192 │ │ │ │ │ -_1_9_7 _i_t_e_r_a_t_o_r _i_n_s_e_r_t(_K_e_y j, const Vector& value) { │ │ │ │ │ -198 return insert(std::make_pair(j, value)); │ │ │ │ │ -199 } │ │ │ │ │ -200 │ │ │ │ │ -203 _V_e_c_t_o_r_V_a_l_u_e_s& insert(const _V_e_c_t_o_r_V_a_l_u_e_s& values); │ │ │ │ │ -204 │ │ │ │ │ -_2_0_9 inline std::pair _t_r_y_I_n_s_e_r_t(_K_e_y j, const Vector& value) { │ │ │ │ │ -210#ifdef TBB_GREATER_EQUAL_2020 │ │ │ │ │ -211 return values_.emplace(j, value); │ │ │ │ │ -212#else │ │ │ │ │ -213 return values_.insert(std::make_pair(j, value)); │ │ │ │ │ -214#endif │ │ │ │ │ -215 } │ │ │ │ │ -216 │ │ │ │ │ -_2_1_9 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const Vector& value) { │ │ │ │ │ -220 if (!tryInsert(j, value).second) { │ │ │ │ │ -221 (*this)[j] = value; │ │ │ │ │ -222 } │ │ │ │ │ -223 } │ │ │ │ │ +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 │ │ │ │ │ -_2_2_6 void _e_r_a_s_e(_K_e_y var) { │ │ │ │ │ -227 if (values_.unsafe_erase(var) == 0) │ │ │ │ │ -228 throw std::invalid_argument("Requested variable '" + │ │ │ │ │ -229 DefaultKeyFormatter(var) + │ │ │ │ │ -230 "', is not in this VectorValues."); │ │ │ │ │ -231 } │ │ │ │ │ -232 │ │ │ │ │ -234 void setZero(); │ │ │ │ │ -235 │ │ │ │ │ -_2_3_6 _i_t_e_r_a_t_o_r _b_e_g_i_n() { return values_.begin(); } │ │ │ │ │ -_2_3_7 _c_o_n_s_t___i_t_e_r_a_t_o_r _b_e_g_i_n() const { return values_.begin(); } │ │ │ │ │ -_2_3_8 _i_t_e_r_a_t_o_r _e_n_d() { return values_.end(); } │ │ │ │ │ -_2_3_9 _c_o_n_s_t___i_t_e_r_a_t_o_r _e_n_d() const { return values_.end(); } │ │ │ │ │ +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 │ │ │ │ │ -_2_4_5 _i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) { return values_.find(j); } │ │ │ │ │ -246 │ │ │ │ │ -_2_5_1 _c_o_n_s_t___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) const { return values_.find(j); } │ │ │ │ │ -252 │ │ │ │ │ -254 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s&); │ │ │ │ │ -255 │ │ │ │ │ -257 void _p_r_i_n_t(const std::string& str = "VectorValues", │ │ │ │ │ -258 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ -259 │ │ │ │ │ -261 bool _e_q_u_a_l_s(const _V_e_c_t_o_r_V_a_l_u_e_s& x, double tol = 1e-9) const; │ │ │ │ │ -262 │ │ │ │ │ -266 │ │ │ │ │ -268 Vector vector() const; │ │ │ │ │ -269 │ │ │ │ │ -271 template │ │ │ │ │ -_2_7_2 Vector _v_e_c_t_o_r(const CONTAINER& keys) const { │ │ │ │ │ -273 _D_e_n_s_e_I_n_d_e_x totalDim = 0; │ │ │ │ │ -274 _F_a_s_t_V_e_c_t_o_r_<_c_o_n_s_t_ _V_e_c_t_o_r_*_> items; │ │ │ │ │ -275 items.reserve(keys.end() - keys.begin()); │ │ │ │ │ -276 for (_K_e_y key : keys) { │ │ │ │ │ -277 const Vector* v = &at(key); │ │ │ │ │ -278 totalDim += v->size(); │ │ │ │ │ -279 items.push_back(v); │ │ │ │ │ -280 } │ │ │ │ │ -281 │ │ │ │ │ -282 Vector result(totalDim); │ │ │ │ │ -283 _D_e_n_s_e_I_n_d_e_x pos = 0; │ │ │ │ │ -284 for (const Vector* v : items) { │ │ │ │ │ -285 result.segment(pos, v->size()) = *v; │ │ │ │ │ -286 pos += v->size(); │ │ │ │ │ -287 } │ │ │ │ │ -288 │ │ │ │ │ -289 return result; │ │ │ │ │ -290 } │ │ │ │ │ -291 │ │ │ │ │ -293 Vector vector(const Dims& dims) const; │ │ │ │ │ -294 │ │ │ │ │ -296 void swap(_V_e_c_t_o_r_V_a_l_u_e_s& other); │ │ │ │ │ -297 │ │ │ │ │ -299 bool hasSameStructure(const _V_e_c_t_o_r_V_a_l_u_e_s other) const; │ │ │ │ │ -300 │ │ │ │ │ -304 │ │ │ │ │ -308 double _d_o_t(const _V_e_c_t_o_r_V_a_l_u_e_s& v) const; │ │ │ │ │ -309 │ │ │ │ │ -311 double norm() const; │ │ │ │ │ -312 │ │ │ │ │ -314 double squaredNorm() const; │ │ │ │ │ -315 │ │ │ │ │ -318 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_+(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -319 │ │ │ │ │ -322 _V_e_c_t_o_r_V_a_l_u_e_s add(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -323 │ │ │ │ │ -326 _V_e_c_t_o_r_V_a_l_u_e_s& operator+=(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ -327 │ │ │ │ │ -330 _V_e_c_t_o_r_V_a_l_u_e_s& addInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ -331 │ │ │ │ │ -333 _V_e_c_t_o_r_V_a_l_u_e_s& addInPlace_(const _V_e_c_t_o_r_V_a_l_u_e_s& c); │ │ │ │ │ -334 │ │ │ │ │ -337 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_-(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -338 │ │ │ │ │ -341 _V_e_c_t_o_r_V_a_l_u_e_s subtract(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -342 │ │ │ │ │ -344 friend GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _o_p_e_r_a_t_o_r_*(const double a, const │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s &v); │ │ │ │ │ -345 │ │ │ │ │ -347 _V_e_c_t_o_r_V_a_l_u_e_s scale(const double a) const; │ │ │ │ │ -348 │ │ │ │ │ -350 _V_e_c_t_o_r_V_a_l_u_e_s& operator*=(double alpha); │ │ │ │ │ -351 │ │ │ │ │ -353 _V_e_c_t_o_r_V_a_l_u_e_s& scaleInPlace(double alpha); │ │ │ │ │ -354 │ │ │ │ │ -356 │ │ │ │ │ -359 │ │ │ │ │ -365 std::string _h_t_m_l( │ │ │ │ │ -366 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -367 │ │ │ │ │ -369 │ │ │ │ │ -370 private: │ │ │ │ │ -_3_7_2 friend class boost::serialization::access; │ │ │ │ │ -373 template │ │ │ │ │ -374 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -375 ar & BOOST_SERIALIZATION_NVP(values_); │ │ │ │ │ -376 } │ │ │ │ │ -377 }; // VectorValues definition │ │ │ │ │ -378 │ │ │ │ │ -380 template<> │ │ │ │ │ -_3_8_1 struct _t_r_a_i_t_s<_V_e_c_t_o_r_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -382 }; │ │ │ │ │ -383 │ │ │ │ │ -384} // \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. │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_S_c_a_t_t_e_r_._h │ │ │ │ │ -Maps global variable indices to slot indices. │ │ │ │ │ -_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 │ │ │ │ │ +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. │ │ │ │ │ +_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_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D 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 │ │ │ │ │ _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_:_:_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_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_:_:_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_:_:_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_:_:_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_:_:_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_:_:_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_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -value_type KeyValuePair │ │ │ │ │ -Typedef to pair │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_n_d │ │ │ │ │ -const_iterator end() const │ │ │ │ │ -Iterator over variables. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:239 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e │ │ │ │ │ -Values::value_type value_type │ │ │ │ │ -Typedef to pair │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -Values::const_iterator const_iterator │ │ │ │ │ -Const iterator over vector values. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_n_d │ │ │ │ │ -iterator end() │ │ │ │ │ -Iterator over variables. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:238 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ -Values::iterator iterator │ │ │ │ │ -Iterator over vector values. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:81 │ │ │ │ │ -_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(Key j, const Vector &value) │ │ │ │ │ -Insert a vector value with key j. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:197 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues(const CONTAINER &c) │ │ │ │ │ -Create from another container holding pair. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e_s__ │ │ │ │ │ -Values values_ │ │ │ │ │ -Vectors making up this VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_D_i_m_s │ │ │ │ │ -std::map< Key, size_t > Dims │ │ │ │ │ -Keyed vector dimensions. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ -ConcurrentMap< Key, Vector > Values │ │ │ │ │ -Collection of Vectors making up a VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -Vector & operator[](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:163 │ │ │ │ │ -_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_:_:_f_i_n_d │ │ │ │ │ -iterator find(Key j) │ │ │ │ │ -Return the iterator corresponding to the requested key, or end() if no variable │ │ │ │ │ -is present with this ... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:245 │ │ │ │ │ -_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -Number of variables stored. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -const Vector & operator[](Key j) const │ │ │ │ │ -Access the vector value with key j (const version), throws std::out_of_range if │ │ │ │ │ -j does not exist,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:167 │ │ │ │ │ -_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues() │ │ │ │ │ -Default constructor creates an empty VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_f_i_n_d │ │ │ │ │ -const_iterator find(Key j) const │ │ │ │ │ -Return the iterator corresponding to the requested key, or end() if no variable │ │ │ │ │ -is present with this ... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues(ITERATOR first, ITERATOR last) │ │ │ │ │ -Create from a pair of iterators over pair. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const Vector & at(Key j) const │ │ │ │ │ -Access the vector value with key j (const version), throws std::out_of_range if │ │ │ │ │ -j does not exist,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector vector(const CONTAINER &keys) const │ │ │ │ │ -Access a vector that is a subset of relevant keys. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:272 │ │ │ │ │ -_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_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator over variables. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:237 │ │ │ │ │ -_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_:_:_e_r_a_s_e │ │ │ │ │ -void erase(Key var) │ │ │ │ │ -Erase the vector with the given key, or throw std::out_of_range if it does not │ │ │ │ │ -exist. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:226 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues(std::initializer_list< std::pair< Key, Vector > > init) │ │ │ │ │ -Construct from initializer list. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:95 │ │ │ │ │ -_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_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues(const VectorValues &c) │ │ │ │ │ -Implicit copy constructor to specialize the explicit constructor from any │ │ │ │ │ -container. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_b_e_g_i_n │ │ │ │ │ -iterator begin() │ │ │ │ │ -Iterator over variables. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:236 │ │ │ │ │ -_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _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/a00842.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00452.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/geometry/Similarity3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    SubgraphBuilder.h File Reference
    │ │ │ │ +
    Similarity3.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

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

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::Subgraph
    class  gtsam::Similarity3
     3D similarity transform More...
     
    struct  gtsam::Subgraph::Edge
    struct  gtsam::Similarity3::ChartAtOrigin
     Chart at the origin. More...
     
    struct  gtsam::SubgraphBuilderParameters
    struct  gtsam::traits< Similarity3 >
     
    class  gtsam::SubgraphBuilder
    struct  gtsam::traits< const Similarity3 >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ 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, GaussianFactorGraphgtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
     Split the graph into a subgraph and the remaining edges.
     
    │ │ │ │ +template<>
    Matrix gtsam::wedge< Similarity3 > (const Vector &xi)
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Date
    Dec 31, 2009
    │ │ │ │ -
    Author
    Frank Dellaert, Yong-Dian Jian
    │ │ │ │ +

    Implementation of Similarity3 transform.

    │ │ │ │ +
    Author
    Paul Drews
    │ │ │ │ +
    │ │ │ │ +John Lambert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,43 +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 | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SubgraphBuilder.h File Reference │ │ │ │ │ +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_u_b_g_r_a_p_h │ │ │ │ │ + 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_u_b_g_r_a_p_h_:_:_E_d_g_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_:_:_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 │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_i_m_i_l_a_r_i_t_y_3_ _> │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ 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. │ │ │ │ │ +template<> │ │ │ │ │ + Matrix  ggttssaamm::::wweeddggee<< SSiimmiillaarriittyy33 >> (const Vector &xi) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +Implementation of Similarity3 transform. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Paul Drews │ │ │ │ │ + John Lambert │ │ │ │ │ * _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_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a00848_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00323_source.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/geometry/SO3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianBayesNet.h
    │ │ │ │ +
    SO3.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    27
    │ │ │ │ -
    28#include <utility>
    │ │ │ │ -
    29namespace gtsam {
    │ │ │ │ -
    30
    │ │ │ │ -
    │ │ │ │ -
    35 class GTSAM_EXPORT GaussianBayesNet: public BayesNet<GaussianConditional>
    │ │ │ │ -
    36 {
    │ │ │ │ -
    37 public:
    │ │ │ │ +
    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.
    │ │ │ │
    38
    │ │ │ │ - │ │ │ │ -
    40 typedef GaussianBayesNet This;
    │ │ │ │ - │ │ │ │ -
    42 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    43 typedef boost::shared_ptr<ConditionalType> sharedConditional;
    │ │ │ │ -
    44
    │ │ │ │ -
    47
    │ │ │ │ - │ │ │ │ +
    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);
    │ │ │ │
    50
    │ │ │ │ -
    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>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    66 : Base(graph) {}
    │ │ │ │ -
    │ │ │ │ -
    67
    │ │ │ │ -
    72 template <class DERIVEDCONDITIONAL>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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).
    │ │ │ │
    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
    │ │ │ │ -
    Factor Graph Base Class.
    │ │ │ │ -
    Bayes network.
    │ │ │ │ -
    Conditional Gaussian Base class.
    │ │ │ │ -
    Included from all GTSAM files.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    N*N matrix representation of SO(N).
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    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
    │ │ │ │ +
    std::string serialize(const T &input)
    serializes to a string
    Definition serialization.h:113
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A BayesNet is a tree of conditionals, stored in elimination order.
    Definition BayesNet.h:35
    │ │ │ │ -
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
    Definition GaussianBayesNet.h:36
    │ │ │ │ -
    double operator()(const VectorValues &x) const
    Evaluate probability density, sugar.
    Definition GaussianBayesNet.h:114
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
    print graph
    Definition GaussianBayesNet.h:89
    │ │ │ │ -
    GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > > conditionals)
    Constructor that takes an initializer list of shared pointers.
    Definition GaussianBayesNet.h:73
    │ │ │ │ -
    GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition GaussianBayesNet.h:65
    │ │ │ │ -
    virtual ~GaussianBayesNet()=default
    Destructor.
    │ │ │ │ -
    GaussianBayesNet(const CONTAINER &conditionals)
    Construct from container of factors (shared_ptr or plain objects)
    Definition GaussianBayesNet.h:58
    │ │ │ │ -
    GaussianBayesNet()
    Construct empty bayes net.
    Definition GaussianBayesNet.h:49
    │ │ │ │ -
    GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
    Construct from iterator over conditionals.
    Definition GaussianBayesNet.h:53
    │ │ │ │ -
    A GaussianConditional functions as the node in a Bayes network.
    Definition GaussianConditional.h:43
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    static SO< N > Retract(const TangentVector &v)
    Retract at origin: possible in Lie group because it has an identity.
    Definition Lie.h:111
    │ │ │ │ +
    Both LieGroupTraits and Testable.
    Definition Lie.h:229
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Functor implementing Exponential map.
    Definition SO3.h:134
    │ │ │ │ +
    Functor that implements Exponential map and its derivatives.
    Definition SO3.h:155
    │ │ │ │ +
    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_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
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
    Return vectorized rotation matrix in column order.
    Definition SOn-inl.h:88
    │ │ │ │ +
    static SO ChordalMean(const std::vector< SO > &rotations)
    Named constructor that finds chordal mean , currently only defined for SO3.
    │ │ │ │ +
    static TangentVector Vee(const MatrixNN &X)
    Inverse of Hat. See note about xi element order in Hat.
    Definition SOn-inl.h:35
    │ │ │ │ +
    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
    │ │ │ │ +
    static SO AxisAngle(const Vector3 &axis, double theta)
    Constructor from axis and angle. Only defined for SO3.
    │ │ │ │ +
    MatrixDD AdjointMap() const
    Adjoint map.
    Definition SO4.cpp:159
    │ │ │ │ +
    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
    │ │ │ │ +
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ +
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │ +
    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,228 +1,266 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianBayesNet.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// \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_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: │ │ │ │ │ +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. │ │ │ │ │ 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() {} │ │ │ │ │ +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); │ │ │ │ │ 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; │ │ │ │ │ +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). │ │ │ │ │ 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. │ │ │ │ │ +_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). │ │ │ │ │ _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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a00854.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/slam/EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    Scatter.cpp File Reference
    │ │ │ │ +
    EssentialMatrixConstraint.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    June 2015
    │ │ │ │ +Pablo Alcantarilla
    │ │ │ │ +
    Date
    Jan 5, 2014
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Scatter.cpp File Reference │ │ │ │ │ +EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Pablo Alcantarilla │ │ │ │ │ Date │ │ │ │ │ - June 2015 │ │ │ │ │ + Jan 5, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _S_c_a_t_t_e_r_._c_p_p │ │ │ │ │ + * _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/a00878_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00740_source.html │ │ │ │┄ Files 7% similar despite different names │ │ │ │ @@ -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/GaussianEliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussianJunctionTree.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
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    27 // Forward declarations
    │ │ │ │ -
    28 class GaussianEliminationTree;
    │ │ │ │ -
    29
    │ │ │ │ -
    │ │ │ │ -
    38 class GTSAM_EXPORT GaussianJunctionTree :
    │ │ │ │ -
    39 public JunctionTree<GaussianBayesTree, GaussianFactorGraph> {
    │ │ │ │ -
    40 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    43 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    44
    │ │ │ │ -
    53 GaussianJunctionTree(const GaussianEliminationTree& eliminationTree);
    │ │ │ │ -
    54 };
    │ │ │ │ -
    │ │ │ │ -
    55
    │ │ │ │ -
    56}
    │ │ │ │ -
    The junction tree.
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    │ │ │ │ +
    27 class GTSAM_EXPORT GaussianEliminationTree :
    │ │ │ │ +
    28 public EliminationTree<GaussianBayesNet, GaussianFactorGraph>
    │ │ │ │ +
    29 {
    │ │ │ │ +
    30 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    33 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    34
    │ │ │ │ + │ │ │ │ +
    44 const VariableIndex& structure, const Ordering& order);
    │ │ │ │ +
    45
    │ │ │ │ + │ │ │ │ +
    52 const Ordering& order);
    │ │ │ │ +
    53
    │ │ │ │ +
    55 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ +
    56
    │ │ │ │ +
    57 private:
    │ │ │ │ +
    58
    │ │ │ │ +
    59 friend class ::EliminationTreeTester;
    │ │ │ │ +
    60
    │ │ │ │ +
    61 };
    │ │ │ │ +
    │ │ │ │ +
    62
    │ │ │ │ +
    63}
    │ │ │ │ + │ │ │ │ +
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ +
    Chordal Bayes Net, the result of eliminating a factor graph.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    Definition GaussianEliminationTree.h:29
    │ │ │ │ -
    A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
    Definition GaussianJunctionTree.h:39
    │ │ │ │ -
    GaussianJunctionTree This
    This class.
    Definition GaussianJunctionTree.h:42
    │ │ │ │ -
    JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base
    Base class.
    Definition GaussianJunctionTree.h:41
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition GaussianJunctionTree.h:43
    │ │ │ │ +
    GaussianEliminationTree This
    This class.
    Definition GaussianEliminationTree.h:32
    │ │ │ │ +
    EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base
    Base class.
    Definition GaussianEliminationTree.h:31
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition GaussianEliminationTree.h:33
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianJunctionTree.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 │ │ │ │ │ @@ -16,62 +16,77 @@ │ │ │ │ │ 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_/_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 │ │ │ │ │ -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. │ │ │ │ │ +_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: │ │ │ │ │ +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_:_:_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_:_:_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_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 GaussianJunctionTree.h:43 │ │ │ │ │ +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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00881_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01226_source.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/sfm/TranslationRecovery.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    iterative.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ -
    21#include <gtsam/base/Matrix.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 }
    │ │ │ │ +
    19#include <gtsam/geometry/Unit3.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.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    52 public:
    │ │ │ │ +
    53 using KeyPair = std::pair<Key, Key>;
    │ │ │ │ +
    54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
    │ │ │ │
    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 }
    │ │ │ │ +
    56 private:
    │ │ │ │ +
    57 // Translation directions between camera pairs.
    │ │ │ │ +
    58 TranslationEdges relativeTranslations_;
    │ │ │ │ +
    59
    │ │ │ │ +
    60 // Parameters.
    │ │ │ │ + │ │ │ │ +
    62
    │ │ │ │ +
    63 public:
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    70 : lmParams_(lmParams) {}
    │ │ │ │
    │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ + │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ + │ │ │ │ +
    85 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
    │ │ │ │
    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
    │ │ │ │ - │ │ │ │ -
    141 const GaussianFactorGraph& fg,
    │ │ │ │ -
    142 const VectorValues& x,
    │ │ │ │ -
    143 const ConjugateGradientParameters & parameters);
    │ │ │ │ -
    144
    │ │ │ │ -
    145
    │ │ │ │ -
    146} // namespace gtsam
    │ │ │ │ -
    147
    │ │ │ │ - │ │ │ │ -
    149
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ -
    Iterative methods, template implementation.
    │ │ │ │ -
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ +
    102 void addPrior(
    │ │ │ │ +
    103 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
    │ │ │ │ +
    104 const double scale,
    │ │ │ │ +
    105 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
    │ │ │ │ + │ │ │ │ +
    107 const SharedNoiseModel &priorNoiseModel =
    │ │ │ │ +
    108 noiseModel::Isotropic::Sigma(3, 0.01)) const;
    │ │ │ │ +
    109
    │ │ │ │ + │ │ │ │ +
    122 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
    │ │ │ │ +
    123 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
    │ │ │ │ +
    124 std::mt19937 *rng, const Values &initialValues = Values()) const;
    │ │ │ │ +
    125
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ +
    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
    │ │ │ │ +
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
    Method of conjugate gradients (CG), System version.
    Definition iterative.cpp:45
    │ │ │ │ -
    parameters for the conjugate gradient method
    Definition ConjugateGradientSolver.h:29
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrice...
    Definition iterative.h:44
    │ │ │ │ -
    Vector operator*(const Vector &x) const
    Apply operator A.
    Definition iterative.h:78
    │ │ │ │ -
    const Matrix & A() const
    Access A matrix.
    Definition iterative.h:57
    │ │ │ │ -
    void multiplyInPlace(const Vector &x, Vector &e) const
    Apply operator A in place.
    Definition iterative.h:83
    │ │ │ │ -
    const Vector & b() const
    Access b vector.
    Definition iterative.h:60
    │ │ │ │ -
    Vector gradient(const Vector &x) const
    gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_)
    Definition iterative.h:73
    │ │ │ │ -
    void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const
    x += alpha* A'*e
    Definition iterative.h:88
    │ │ │ │ -
    Vector operator^(const Vector &e) const
    Apply operator A'*e.
    Definition iterative.h:63
    │ │ │ │ -
    parameters for iterative linear solvers
    Definition IterativeSolver.h:44
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    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
    │ │ │ │ +
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    Definition BinaryMeasurement.h:36
    │ │ │ │ +
    Definition TranslationRecovery.h:51
    │ │ │ │ +
    NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
    Build the factor graph to do the optimization.
    Definition TranslationRecovery.cpp:99
    │ │ │ │ +
    static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
    Simulate translation direction measurements.
    Definition TranslationRecovery.cpp:216
    │ │ │ │ +
    TranslationRecovery()=default
    Default constructor.
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    TranslationRecovery(const LevenbergMarquardtParams &lmParams)
    Construct a new Translation Recovery object.
    Definition TranslationRecovery.h:69
    │ │ │ │ +
    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,183 +1,177 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -iterative.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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 } │ │ │ │ │ -66 │ │ │ │ │ -70 void _p_r_i_n_t (const std::string& s = "System") const; │ │ │ │ │ +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_3 Vector _g_r_a_d_i_e_n_t(const Vector& x) const { │ │ │ │ │ -74 return A() ^ (A() * x - b()); │ │ │ │ │ -75 } │ │ │ │ │ +_7_5 _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y() = default; │ │ │ │ │ 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 } │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ -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 │ │ │ │ │ -_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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._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/a00890_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00734_source.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    15 * @date Jan 31, 2012
    │ │ │ │
    16 * @author Yong-Dian Jian
    │ │ │ │
    17 * @author Sungtae An
    │ │ │ │
    18 */
    │ │ │ │
    19
    │ │ │ │
    20#pragma once
    │ │ │ │
    21
    │ │ │ │ - │ │ │ │ + │ │ │ │
    23#include <string>
    │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │
    27class GaussianFactorGraph;
    │ │ │ │
    28class KeyInfo;
    │ │ │ │
    29class Preconditioner;
    │ │ │ │ @@ -218,15 +218,15 @@ │ │ │ │
    117 const std::map<Key, size_t> & dimensions);
    │ │ │ │
    118
    │ │ │ │
    120VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
    │ │ │ │
    121
    │ │ │ │
    123
    │ │ │ │
    124}
    │ │ │ │
    125
    │ │ │ │ -
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │ +
    Implementation of Conjugate Gradient solver for a linear system.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
    Create VectorValues from a Vector.
    Definition PCGSolver.cpp:151
    │ │ │ │
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    void axpy(double alpha, const Errors &x, Errors &y)
    BLAS level 2 style AXPY, y := alpha*x + y
    Definition Errors.cpp:111
    │ │ │ │
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00893_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00515_source.html │ │ │ │┄ Files 9% 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/hybrid/HybridValues.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    RegularHessianFactor.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23#include <vector>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <gtsam/inference/Key.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │
    26
    │ │ │ │ -
    27template<size_t D>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30public:
    │ │ │ │ -
    31
    │ │ │ │ -
    32 typedef Eigen::Matrix<double, D, 1> VectorD;
    │ │ │ │ -
    33 typedef Eigen::Matrix<double, D, D> MatrixD;
    │ │ │ │ -
    34
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ -
    │ │ │ │ -
    53
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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 }
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    58 : continuous_(cv), discrete_(dv){};
    │ │ │ │ +
    │ │ │ │ +
    59
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    62 const Values& v)
    │ │ │ │ +
    63 : continuous_(cv), discrete_(dv), nonlinear_(v){};
    │ │ │ │
    │ │ │ │
    64
    │ │ │ │ -
    67 template<typename KEYS>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    71 checkInvariants();
    │ │ │ │ -
    72 }
    │ │ │ │ -
    │ │ │ │ -
    73
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    76 : HessianFactor(jf) {}
    │ │ │ │ +
    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
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    80 const Scatter& scatter)
    │ │ │ │ -
    81 : HessianFactor(factors, scatter) {
    │ │ │ │ -
    82 checkInvariants();
    │ │ │ │ -
    83 }
    │ │ │ │ -
    │ │ │ │ -
    84
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    87 : HessianFactor(factors) {
    │ │ │ │ -
    88 checkInvariants();
    │ │ │ │ -
    89 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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
    │ │ │ │ -
    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 {
    │ │ │ │ - │ │ │ │ -
    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();
    │ │ │ │ +
    101 bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
    │ │ │ │ +
    102
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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; };
    │ │ │ │
    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 }
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    138 continuous_.insert(values);
    │ │ │ │ +
    139 return *this;
    │ │ │ │ +
    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 }
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    145 discrete_.insert(values);
    │ │ │ │ +
    146 return *this;
    │ │ │ │
    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 }
    │ │ │ │ +
    │ │ │ │ +
    151 HybridValues& insert(const Values& values) {
    │ │ │ │ +
    152 nonlinear_.insert(values);
    │ │ │ │ +
    153 return *this;
    │ │ │ │ +
    154 }
    │ │ │ │ +
    │ │ │ │ +
    155
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    182 continuous_.update(values);
    │ │ │ │ +
    183 return *this;
    │ │ │ │ +
    184 }
    │ │ │ │ +
    │ │ │ │ +
    185
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    191 discrete_.update(values);
    │ │ │ │ +
    192 return *this;
    │ │ │ │
    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};
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    200 continuous_.update(values.continuous());
    │ │ │ │ +
    201 discrete_.update(values.discrete());
    │ │ │ │ +
    202 return *this;
    │ │ │ │ +
    203 }
    │ │ │ │ +
    │ │ │ │ +
    204
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    207 VectorValues measurements;
    │ │ │ │ +
    208 for (const auto& key : keys) {
    │ │ │ │ +
    209 measurements.insert(key, continuous_.at(key));
    │ │ │ │ +
    210 }
    │ │ │ │ +
    211 return measurements;
    │ │ │ │ +
    212 }
    │ │ │ │
    │ │ │ │ -
    215
    │ │ │ │ -
    216}
    │ │ │ │ +
    213
    │ │ │ │
    217
    │ │ │ │ -
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ -
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ +
    specialized key for discrete variables
    │ │ │ │ + │ │ │ │ +
    An assignment from labels to a discrete value index (size_t)
    │ │ │ │ + │ │ │ │ +
    Factor Graph Values.
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ -
    ptrdiff_t DenseIndex
    The index type for Eigen objects.
    Definition types.h:106
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    This class stores a dense matrix and allows it to be accessed as a collection of blocks.
    Definition SymmetricBlockMatrix.h:52
    │ │ │ │ -
    Vector diagonal(DenseIndex J) const
    Get the diagonal of the J'th diagonal block.
    Definition SymmetricBlockMatrix.h:145
    │ │ │ │ -
    DenseIndex cols() const
    Column size.
    Definition SymmetricBlockMatrix.h:117
    │ │ │ │ -
    constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
    Get block above the diagonal (I, J).
    Definition SymmetricBlockMatrix.h:150
    │ │ │ │ -
    Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
    Return the J'th diagonal block as a self adjoint view.
    Definition SymmetricBlockMatrix.h:135
    │ │ │ │ -
    DenseIndex nBlocks() const
    Block count.
    Definition SymmetricBlockMatrix.h:120
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ -
    size_t size() const
    Definition Factor.h:157
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ -
    Matrix augmentedInformation() const override
    Return the augmented information matrix represented by this GaussianFactor.
    Definition HessianFactor.cpp:282
    │ │ │ │ -
    SymmetricBlockMatrix info_
    The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
    Definition HessianFactor.h:104
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition HessianFactor.cpp:391
    │ │ │ │ -
    Definition RegularHessianFactor.h:28
    │ │ │ │ -
    RegularHessianFactor(const GaussianFactorGraph &factors)
    Construct from a GaussianFactorGraph.
    Definition RegularHessianFactor.h:86
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
    y += alpha * A'*A*x
    Definition RegularHessianFactor.h:112
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
    y += alpha * A'*A*x
    Definition RegularHessianFactor.h:118
    │ │ │ │ -
    void hessianDiagonal(double *d) const override
    Return the diagonal of the Hessian for this factor (raw memory version)
    Definition RegularHessianFactor.h:185
    │ │ │ │ -
    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
    │ │ │ │ -
    void gradientAtZero(double *d) const override
    Add gradient at zero to d TODO: is it really the goal to add ??
    Definition RegularHessianFactor.h:196
    │ │ │ │ -
    RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
    Construct from a GaussianFactorGraph.
    Definition RegularHessianFactor.h:79
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    RegularHessianFactor(const RegularJacobianFactor< D > &jf)
    Construct from RegularJacobianFactor.
    Definition RegularHessianFactor.h:75
    │ │ │ │ -
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ -
    Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
    Definition Scatter.h:49
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ +
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const
    Output as a html table.
    Definition DiscreteValues.cpp:104
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print required by Testable.
    Definition DiscreteValues.cpp:31
    │ │ │ │ +
    bool equals(const DiscreteValues &x, double tol=1e-9) const
    equals required by Testable for unit testing.
    Definition DiscreteValues.cpp:40
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    void print(const std::string &s="HybridValues", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print required by Testable for unit testing
    Definition HybridValues.h:70
    │ │ │ │ +
    void insert(Key j, const Vector &value)
    Insert a vector value with key j.
    Definition HybridValues.h:117
    │ │ │ │ +
    HybridValues & insert(const DiscreteValues &values)
    Insert all discrete values from values.
    Definition HybridValues.h:144
    │ │ │ │ +
    HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v)
    Construct from all values types.
    Definition HybridValues.h:61
    │ │ │ │ +
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output as a html table.
    Definition HybridValues.h:224
    │ │ │ │ +
    VectorValues continuousSubset(const KeyVector &keys) const
    Extract continuous values with given keys.
    Definition HybridValues.h:206
    │ │ │ │ +
    void insert_or_assign(Key j, const Vector &value)
    insert_or_assign() , similar to Values.h
    Definition HybridValues.h:126
    │ │ │ │ +
    HybridValues & insert(const Values &values)
    Insert all values from values.
    Definition HybridValues.h:151
    │ │ │ │ +
    bool equals(const HybridValues &other, double tol=1e-9) const
    equals required by Testable for unit testing
    Definition HybridValues.h:79
    │ │ │ │ +
    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
    │ │ │ │ +
    const DiscreteValues & discrete() const
    Return the discrete values.
    Definition HybridValues.h:92
    │ │ │ │ +
    bool existsDiscrete(Key j)
    Check whether a variable with key j exists in DiscreteValues.
    Definition HybridValues.h:101
    │ │ │ │ +
    const VectorValues & continuous() const
    Return the multi-dimensional vector values.
    Definition HybridValues.h:89
    │ │ │ │ +
    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
    │ │ │ │ +
    bool exists(Key j)
    Check whether a variable with key j exists.
    Definition HybridValues.h:109
    │ │ │ │ +
    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
    │ │ │ │ +
    HybridValues & insert(const HybridValues &values)
    Insert all values from values.
    Definition HybridValues.h:158
    │ │ │ │ +
    HybridValues & insert(const VectorValues &values)
    Insert all continuous values from values.
    Definition HybridValues.h:137
    │ │ │ │ +
    void insert(Key j, size_t value)
    Insert a discrete value with key j.
    Definition HybridValues.h:123
    │ │ │ │ +
    const Values & nonlinear() const
    Return the nonlinear values.
    Definition HybridValues.h:95
    │ │ │ │ +
    bool existsNonlinear(Key j)
    Check whether a variable with key j exists in values.
    Definition HybridValues.h:104
    │ │ │ │ +
    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
    │ │ │ │ +
    HybridValues()=default
    Default constructor creates an empty HybridValues.
    │ │ │ │ +
    bool existsVector(Key j)
    Check whether a variable with key j exists in VectorValues.
    Definition HybridValues.h:98
    │ │ │ │ +
    HybridValues(const VectorValues &cv, const DiscreteValues &dv)
    Construct from DiscreteValues and VectorValues.
    Definition HybridValues.h:57
    │ │ │ │ +
    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
    │ │ │ │ +
    void insert_or_assign(Key j, size_t value)
    insert_or_assign() , similar to Values.h
    Definition HybridValues.h:131
    │ │ │ │
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    iterator insert(const std::pair< Key, Vector > &key_value)
    Insert a vector value with key j.
    Definition VectorValues.cpp:91
    │ │ │ │ +
    void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
    print required by Testable for unit testing
    Definition VectorValues.cpp:158
    │ │ │ │ +
    bool equals(const VectorValues &x, double tol=1e-9) const
    equals required by Testable for unit testing
    Definition VectorValues.cpp:166
    │ │ │ │ +
    bool exists(Key j) const
    Check whether a variable with key j exists.
    Definition VectorValues.h:133
    │ │ │ │ +
    std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    Output as a html table.
    Definition VectorValues.cpp:387
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    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
    │ │ │ │ +
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,356 +1,392 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularHessianFactor.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_/_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 { │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -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) {} │ │ │ │ │ +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 _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 } │ │ │ │ │ +_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 │ │ │ │ │ -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 } │ │ │ │ │ +_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 │ │ │ │ │ -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(); │ │ │ │ │ +_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; }; │ │ │ │ │ 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); │ │ │ │ │ +_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 │ │ │ │ │ -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 } │ │ │ │ │ +_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_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 } │ │ │ │ │ +_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_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());; │ │ │ │ │ +_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 } │ │ │ │ │ -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} │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ +_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_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00914.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00356.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/geometry/Similarity3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    AttitudeFactor.cpp File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Similarity3.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Implementation file for Attitude factor. │ │ │ │ +

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

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

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

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

    Detailed Description

    │ │ │ │ -

    Implementation file for Attitude factor.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    January 28, 2014
    │ │ │ │ +

    Implementation of Similarity3 transform.

    │ │ │ │ +
    Author
    Paul Drews
    │ │ │ │ +
    │ │ │ │ +John Lambert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,24 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_i_m_i_l_a_r_i_t_y_3 &p) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation file for Attitude factor. │ │ │ │ │ +Implementation of Similarity3 transform. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 28, 2014 │ │ │ │ │ + Paul Drews │ │ │ │ │ + John Lambert │ │ │ │ │ * _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 │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a00920.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01307.html │ │ │ │┄ Files 5% 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/slam/SmartProjectionFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ScenarioRunner.h File Reference
    │ │ │ │ +
    SmartProjectionFactor.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Simple class to test navigation scenarios. │ │ │ │ +

    Smart factor on cameras (pose + calibration) │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::ScenarioRunner
    class  gtsam::SmartProjectionFactor< CAMERA >
     SmartProjectionFactor: triangulates point and keeps an estimate of it around. More...
     
    class  gtsam::CombinedScenarioRunner
    struct  gtsam::traits< SmartProjectionFactor< CAMERA > >
     traits More...
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -

    Simple class to test navigation scenarios.

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

    Smart factor on cameras (pose + calibration)

    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Zsolt Kira
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +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 │ │ │ │ │ -ScenarioRunner.h File Reference │ │ │ │ │ -Simple class to test navigation scenarios. _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_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ + 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_:_:_C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +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 ********** │ │ │ │ │ -Simple class to test navigation scenarios. │ │ │ │ │ +Smart factor on cameras (pose + calibration) │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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/a01022_source.html │ │ │ │┄ Files 10% 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/nonlinear/DoglegOptimizerImpl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ScenarioRunner.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
    │ │ │ │ -
    18#pragma once
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    17#pragma once
    │ │ │ │ +
    18
    │ │ │ │ +
    19#include <iomanip>
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ + │ │ │ │
    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:
    │ │ │ │ - │ │ │ │ -
    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); }
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    133 double T, const Bias& estimatedBias = Bias(),
    │ │ │ │ -
    134 bool corrupted = false) const;
    │ │ │ │ +
    │ │ │ │ +
    32struct GTSAM_EXPORT DoglegOptimizerImpl {
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ +
    34 struct GTSAM_EXPORT IterationResult {
    │ │ │ │ +
    35 double delta;
    │ │ │ │ +
    36 VectorValues dx_d;
    │ │ │ │ +
    37 double f_error;
    │ │ │ │ +
    38 };
    │ │ │ │ +
    │ │ │ │ +
    39
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    sampling from a NoiseModel
    │ │ │ │ -
    Simple class to test navigation scenarios.
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    136/* ************************************************************************* */
    │ │ │ │ +
    137template<class M, class F, class VALUES>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Variable ordering for the elimination algorithm.
    │ │ │ │ +
    Factor Graph Values.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Matrix3 transpose() const
    Return 3*3 transpose (inverse) rotation matrix.
    Definition Rot3M.cpp:144
    │ │ │ │ -
    static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
    A Gaussian noise model created by specifying a covariance matrix.
    Definition NoiseModel.cpp:117
    │ │ │ │ -
    Sampling structure that keeps internal random number generators for diagonal distributions specified ...
    Definition Sampler.h:31
    │ │ │ │ -
    Vector sample() const
    sample from distribution
    Definition Sampler.cpp:59
    │ │ │ │ -
    PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
    Definition CombinedImuFactor.h:129
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    const Vector3 & gyroscope() const
    get gyroscope bias
    Definition ImuBias.h:69
    │ │ │ │ -
    const Vector3 & accelerometer() const
    get accelerometer bias
    Definition ImuBias.h:64
    │ │ │ │ -
    PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
    Definition ImuFactor.h:72
    │ │ │ │ -
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    Simple trajectory simulator.
    Definition Scenario.h:25
    │ │ │ │ -
    virtual Vector3 omega_b(double t) const =0
    angular velocity in body frame
    │ │ │ │ -
    Definition ScenarioRunner.h:40
    │ │ │ │ -
    Definition ScenarioRunner.h:114
    │ │ │ │ +
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ +
    static VectorValues Zero(const VectorValues &other)
    Create a VectorValues with the same structure as other, but filled with zeros.
    Definition VectorValues.cpp:78
    │ │ │ │ +
    This class contains the implementation of the Dogleg algorithm.
    Definition DoglegOptimizerImpl.h:32
    │ │ │ │ +
    TrustRegionAdaptationMode
    Specifies how the trust region is adapted at each Dogleg iteration.
    Definition DoglegOptimizerImpl.h:53
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition DoglegOptimizerImpl.h:34
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,207 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ScenarioRunner.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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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 // 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); │ │ │ │ │ -} │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ +_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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │ _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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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/a00941.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │┄ Files 11% similar despite different names │ │ │ │ @@ -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/linear/GaussianFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    AHRSFactor.h File Reference
    │ │ │ │ +
    GaussianFactor.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    A factor with a quadratic error function - a Gaussian. │ │ │ │ +More...

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

    │ │ │ │ -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

    │ │ │ │ -
    Author
    Krunal Chande
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    Date
    July 2014
    │ │ │ │ +

    A factor with a quadratic error function - a Gaussian.

    │ │ │ │ +

    GaussianFactor

    Author
    Fan Jiang
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -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 │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +GaussianFactor │ │ │ │ │ Author │ │ │ │ │ - Krunal Chande │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - July 2014 │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00941_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00917_source.html │ │ │ │┄ Files 4% 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/navigation/PreintegratedRotation.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    AHRSFactor.h
    │ │ │ │ +
    PreintegratedRotation.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 */
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ + │ │ │ │ +
    25#include <gtsam/base/Matrix.h>
    │ │ │ │
    26
    │ │ │ │
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    35
    │ │ │ │ -
    36 protected:
    │ │ │ │ +
    │ │ │ │ +
    31struct GTSAM_EXPORT PreintegratedRotationParams {
    │ │ │ │ + │ │ │ │ +
    35 boost::optional<Vector3> omegaCoriolis;
    │ │ │ │ +
    36 boost::optional<Pose3> body_P_sensor;
    │ │ │ │
    37
    │ │ │ │ -
    38 Vector3 biasHat_;
    │ │ │ │ - │ │ │ │ -
    40
    │ │ │ │ -
    41 friend class AHRSFactor;
    │ │ │ │ -
    42
    │ │ │ │ -
    43 public:
    │ │ │ │ -
    44
    │ │ │ │ - │ │ │ │ -
    47
    │ │ │ │ -
    │ │ │ │ -
    52 PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
    │ │ │ │ -
    53 const Vector3& biasHat) :
    │ │ │ │ -
    54 PreintegratedRotation(p), biasHat_(biasHat) {
    │ │ │ │ -
    55 resetIntegration();
    │ │ │ │ -
    56 }
    │ │ │ │ -
    │ │ │ │ -
    57
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 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:
    │ │ │ │ + │ │ │ │ +
    81#endif
    │ │ │ │ +
    82};
    │ │ │ │ +
    │ │ │ │ +
    83
    │ │ │ │ +
    │ │ │ │ +
    89class GTSAM_EXPORT PreintegratedRotation {
    │ │ │ │ +
    90 public:
    │ │ │ │ + │ │ │ │ +
    92
    │ │ │ │ +
    93 protected:
    │ │ │ │ +
    95 boost::shared_ptr<Params> p_;
    │ │ │ │ +
    96
    │ │ │ │ +
    97 double deltaTij_;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    100
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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> {
    │ │ │ │ +
    123
    │ │ │ │ +
    125 void resetIntegration();
    │ │ │ │ +
    126
    │ │ │ │ +
    │ │ │ │ +
    128 bool matchesParamsWith(const PreintegratedRotation& other) const {
    │ │ │ │ +
    129 return p_ == other.p_;
    │ │ │ │ +
    130 }
    │ │ │ │ +
    │ │ │ │
    132
    │ │ │ │ -
    133 typedef AHRSFactor This;
    │ │ │ │ - │ │ │ │ -
    135
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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 bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
    │ │ │ │ -
    172
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    190
    │ │ │ │ -
    192 AHRSFactor(Key rot_i, Key rot_j, Key bias,
    │ │ │ │ - │ │ │ │ -
    194 const Vector3& omegaCoriolis,
    │ │ │ │ -
    195 const boost::optional<Pose3>& body_P_sensor = boost::none);
    │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    194#endif
    │ │ │ │ +
    195};
    │ │ │ │ +
    │ │ │ │
    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
    │ │ │ │ -
    3D Pose
    │ │ │ │ - │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    197template <>
    │ │ │ │ +
    198struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
    │ │ │ │ +
    199
    │ │ │ │ +
    200}
    │ │ │ │ +
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ +
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    3D Pose
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
    Definition AHRSFactor.h:34
    │ │ │ │ -
    Vector3 biasHat_
    Angular rate bias values used during preintegration.
    Definition AHRSFactor.h:38
    │ │ │ │ -
    PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
    Definition AHRSFactor.h:112
    │ │ │ │ -
    PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
    Default constructor, initialize with no measurements.
    Definition AHRSFactor.h:52
    │ │ │ │ -
    Matrix3 preintMeasCov_
    Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
    Definition AHRSFactor.h:39
    │ │ │ │ -
    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
    │ │ │ │ -
    PreintegratedAhrsMeasurements()
    Default constructor, only for serialization and wrappers.
    Definition AHRSFactor.h:46
    │ │ │ │ -
    Definition AHRSFactor.h:131
    │ │ │ │ -
    const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
    Access the preintegrated measurements.
    Definition AHRSFactor.h:174
    │ │ │ │ -
    boost::shared_ptr< AHRSFactor > shared_ptr
    Shorthand for a smart pointer to a factor.
    Definition AHRSFactor.h:147
    │ │ │ │
    Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
    Definition PreintegratedRotation.h:31
    │ │ │ │ +
    boost::optional< Pose3 > body_P_sensor
    The pose of the sensor in the body frame.
    Definition PreintegratedRotation.h:36
    │ │ │ │ +
    boost::optional< Vector3 > omegaCoriolis
    Coriolis constant.
    Definition PreintegratedRotation.h:35
    │ │ │ │ +
    Matrix3 gyroscopeCovariance
    Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
    Definition PreintegratedRotation.h:34
    │ │ │ │
    PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
    Definition PreintegratedRotation.h:89
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    Matrix3 delRdelBiasOmega_
    Jacobian of preintegrated rotation w.r.t. angular rate bias.
    Definition PreintegratedRotation.h:99
    │ │ │ │ +
    boost::shared_ptr< Params > p_
    Parameters.
    Definition PreintegratedRotation.h:95
    │ │ │ │ +
    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
    │ │ │ │ +
    PreintegratedRotation(const boost::shared_ptr< Params > &p)
    Default constructor, resets integration to zero.
    Definition PreintegratedRotation.h:109
    │ │ │ │ +
    double deltaTij_
    Time interval from i to j.
    Definition PreintegratedRotation.h:97
    │ │ │ │ +
    bool matchesParamsWith(const PreintegratedRotation &other) const
    check parameters equality: checks whether shared pointer points to same Params object.
    Definition PreintegratedRotation.h:128
    │ │ │ │ +
    PreintegratedRotation()
    Default constructor for serialization.
    Definition PreintegratedRotation.h:102
    │ │ │ │ +
    Rot3 deltaRij_
    Preintegrated relative orientation (in frame i)
    Definition PreintegratedRotation.h:98
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,267 +1,253 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AHRSFactor.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 │ │ │ │ │ -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> │ │ │ │ │ +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> │ │ │ │ │ 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: │ │ │ │ │ +_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 │ │ │ │ │ -_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 │ │ │ │ │ -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 } │ │ │ │ │ +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 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 } │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ -{ │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 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); │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_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_:_:_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_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_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_:_:_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_:_:_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 │ │ │ │ │ - * _A_H_R_S_F_a_c_t_o_r_._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/a00944.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │┄ Files 2% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │
    ImuBias.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -126,13 +126,13 @@ │ │ │ │
    Date
    Feb 2, 2012
    │ │ │ │
    Author
    Vadim Indelman, Stephen Williams
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00959_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00326_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/geometry/Cal3Fisheye.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NavState.h
    │ │ │ │ +
    Cal3Fisheye.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
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/base/Vector.h>
    │ │ │ │ -
    23#include <gtsam/base/Manifold.h>
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/geometry/Cal3.h>
    │ │ │ │ + │ │ │ │
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ +
    25#include <boost/shared_ptr.hpp>
    │ │ │ │
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    75 static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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 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
    │ │ │ │ -
    163 Vector9 localCoordinates(const NavState& g, //
    │ │ │ │ -
    164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
    │ │ │ │ -
    165 boost::none) const;
    │ │ │ │ +
    │ │ │ │ +
    163 inline Cal3Fisheye retract(const Vector& d) const {
    │ │ │ │ +
    164 return Cal3Fisheye(vector() + d);
    │ │ │ │ +
    165 }
    │ │ │ │ +
    │ │ │ │
    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};
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    3D Pose
    │ │ │ │ +
    208} // namespace gtsam
    │ │ │ │ +
    Common code for all Calibration models.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector3 Velocity3
    Velocity is currently typedef'd to Vector3.
    Definition NavState.h:28
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    gtsam::Quaternion toQuaternion() const
    Compute the quaternion representation of this rotation.
    Definition Rot3M.cpp:233
    │ │ │ │ -
    Matrix3 matrix() const
    return 3*3 rotation matrix
    Definition Rot3M.cpp:219
    │ │ │ │ -
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ -
    NavState()
    Default constructor.
    Definition NavState.h:55
    │ │ │ │ -
    NavState(const Matrix3 &R, const Vector6 &tv)
    Construct from SO(3) and R^6.
    Definition NavState.h:67
    │ │ │ │ -
    NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
    Construct from attitude, position, velocity.
    Definition NavState.h:59
    │ │ │ │ -
    Matrix3 R() const
    Return rotation matrix. Induces computation in quaternion mode.
    Definition NavState.h:95
    │ │ │ │ -
    NavState(const Pose3 &pose, const Velocity3 &v)
    Construct from pose and velocity.
    Definition NavState.h:63
    │ │ │ │ -
    const Vector3 & v() const
    Return velocity as Vector3. Computation-free.
    Definition NavState.h:107
    │ │ │ │ -
    Vector3 t() const
    Return position as Vector3.
    Definition NavState.h:103
    │ │ │ │ -
    Quaternion quaternion() const
    Return quaternion. Induces computation in matrix mode.
    Definition NavState.h:99
    │ │ │ │ +
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ +
    Calibration of a fisheye camera.
    Definition Cal3Fisheye.h:51
    │ │ │ │ +
    double k4() const
    Second tangential distortion coefficient.
    Definition Cal3Fisheye.h:105
    │ │ │ │ +
    double k1() const
    First distortion coefficient.
    Definition Cal3Fisheye.h:96
    │ │ │ │ +
    double k3() const
    First tangential distortion coefficient.
    Definition Cal3Fisheye.h:102
    │ │ │ │ +
    Vector localCoordinates(const Cal3Fisheye &T2) const
    Given a different calibration, calculate update to obtain it.
    Definition Cal3Fisheye.h:168
    │ │ │ │ +
    static size_t Dim()
    Return dimensions of calibration manifold object.
    Definition Cal3Fisheye.h:160
    │ │ │ │ +
    double k2() const
    Second distortion coefficient.
    Definition Cal3Fisheye.h:99
    │ │ │ │ +
    size_t dim() const override
    Return dimensions of calibration manifold object.
    Definition Cal3Fisheye.h:157
    │ │ │ │ +
    Vector9 vector() const
    Return all parameters as a vector.
    Definition Cal3Fisheye.cpp:28
    │ │ │ │ +
    virtual boost::shared_ptr< Cal3Fisheye > clone() const
    Definition Cal3Fisheye.h:177
    │ │ │ │ +
    Vector4 k() const
    return distortion parameter vector
    Definition Cal3Fisheye.h:108
    │ │ │ │ +
    Cal3Fisheye()=default
    Default Constructor with only unit focal length.
    │ │ │ │ +
    Cal3Fisheye retract(const Vector &d) const
    Given delta vector, update calibration.
    Definition Cal3Fisheye.h:163
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,253 +1,224 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NavState.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_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> │ │ │ │ │ +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 │ │ │ │ │ -_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); │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -163 Vector9 localCoordinates(const NavState& g, // │ │ │ │ │ -164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = │ │ │ │ │ -165 boost::none) const; │ │ │ │ │ +_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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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}; │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ +_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_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 │ │ │ │ │ -_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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _N_a_v_S_t_a_t_e_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a00974.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00947.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/CombinedImuFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    AttitudeFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
    │ │ │ │ +
    CombinedImuFactor.h File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Header file for Attitude factor. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

    │ │ │ │

    │ │ │ │ Classes

    class  gtsam::imuBias::ConstantBias
     
    struct  gtsam::traits< imuBias::ConstantBias >
     
    │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

    │ │ │ │ Classes

    class  gtsam::AttitudeFactor
     Base class for prior on attitude Example: 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::Rot3AttitudeFactor
     Version of AttitudeFactor for Rot3. 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...
     
    struct  gtsam::traits< Rot3AttitudeFactor >
     traits More...
    struct  gtsam::traits< PreintegrationCombinedParams >
     
    class  gtsam::Pose3AttitudeFactor
     Version of AttitudeFactor for Pose3. More...
    struct  gtsam::traits< PreintegratedCombinedMeasurements >
     
    struct  gtsam::traits< Pose3AttitudeFactor >
     traits More...
    struct  gtsam::traits< CombinedImuFactor >
     
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

    │ │ │ │ Namespaces

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

    │ │ │ │ +Typedefs

    │ │ │ │ +typedef ManifoldPreintegration gtsam::PreintegrationType
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Header file for Attitude factor.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    January 28, 2014
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │ +
    │ │ │ │ +David Jensen
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Varun Agrawal
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +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 │ │ │ │ │ -AttitudeFactor.h File Reference │ │ │ │ │ -Header 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 | _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_:_:_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_._._. │ │ │ │ │ +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_:_:_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_<_ _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_<_ _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _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_:_:_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_._._. │ │ │ │ │ +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 ********** │ │ │ │ │ -Header file for Attitude factor. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 28, 2014 │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a00974_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01331_source.html │ │ │ │┄ Files 19% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    AttitudeFactor.h
    │ │ │ │ +
    SmartFactorParams.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    22#include <gtsam/geometry/Unit3.h>
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │
    23
    │ │ │ │
    24namespace gtsam {
    │ │ │ │
    25
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    35
    │ │ │ │ -
    36protected:
    │ │ │ │ -
    37
    │ │ │ │ -
    38 Unit3 nZ_, bRef_;
    │ │ │ │ -
    39
    │ │ │ │ -
    40public:
    │ │ │ │ -
    41
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    44 }
    │ │ │ │ -
    │ │ │ │ -
    45
    │ │ │ │ -
    │ │ │ │ -
    51 AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ -
    52 nZ_(nZ), bRef_(bRef) {
    │ │ │ │ -
    53 }
    │ │ │ │ -
    │ │ │ │ -
    54
    │ │ │ │ -
    56 Vector attitudeError(const Rot3& p,
    │ │ │ │ -
    57 OptionalJacobian<2,3> H = boost::none) const;
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
    │ │ │ │ +
    32};
    │ │ │ │ +
    │ │ │ │ +
    33
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
    │ │ │ │ +
    37};
    │ │ │ │ +
    │ │ │ │ +
    38
    │ │ │ │ +
    39/*
    │ │ │ │ +
    40 * Parameters for the smart (stereo) projection factors
    │ │ │ │ +
    41 */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    43
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    46
    │ │ │ │ +
    49 TriangulationParameters triangulation;
    │ │ │ │ + │ │ │ │ +
    52
    │ │ │ │ + │ │ │ │ + │ │ │ │
    58
    │ │ │ │ -
    59 const Unit3& nZ() const {
    │ │ │ │ -
    60 return nZ_;
    │ │ │ │ -
    61 }
    │ │ │ │ -
    62 const Unit3& bRef() const {
    │ │ │ │ -
    63 return bRef_;
    │ │ │ │ -
    64 }
    │ │ │ │ -
    65
    │ │ │ │ - │ │ │ │ -
    68 template<class ARCHIVE>
    │ │ │ │ -
    69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    70 ar & boost::serialization::make_nvp("nZ_", nZ_);
    │ │ │ │ -
    71 ar & boost::serialization::make_nvp("bRef_", bRef_);
    │ │ │ │ -
    72 }
    │ │ │ │ -
    73};
    │ │ │ │ -
    │ │ │ │ -
    74
    │ │ │ │ -
    │ │ │ │ -
    79class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
    │ │ │ │ -
    80
    │ │ │ │ - │ │ │ │ -
    82
    │ │ │ │ -
    83public:
    │ │ │ │ -
    84
    │ │ │ │ -
    86 typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
    │ │ │ │ -
    87
    │ │ │ │ - │ │ │ │ -
    90
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    93 }
    │ │ │ │ -
    │ │ │ │ -
    94
    │ │ │ │ -
    95 ~Rot3AttitudeFactor() override {
    │ │ │ │ -
    96 }
    │ │ │ │ -
    97
    │ │ │ │ -
    │ │ │ │ -
    105 Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
    │ │ │ │ -
    106 const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ -
    107 Base(model, key), AttitudeFactor(nZ, bRef) {
    │ │ │ │ +
    59 // Constructor
    │ │ │ │ + │ │ │ │ +
    61 DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
    │ │ │ │ +
    62 bool verboseCheirality = false, double retriangulationTh = 1e-5) :
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    111 gtsam::NonlinearFactor::shared_ptr clone() const override {
    │ │ │ │ -
    112 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
    │ │ │ │ -
    113 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
    │ │ │ │ +
    109 void setEnableEPI(bool enableEPI) {
    │ │ │ │ +
    110 triangulation.enableEPI = enableEPI;
    │ │ │ │ +
    111 }
    │ │ │ │ +
    112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
    │ │ │ │ +
    113 triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
    │ │ │ │
    114 }
    │ │ │ │ -
    │ │ │ │ -
    115
    │ │ │ │ -
    117 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
    │ │ │ │ -
    118 DefaultKeyFormatter) const override;
    │ │ │ │ -
    119
    │ │ │ │ -
    121 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
    │ │ │ │ -
    122
    │ │ │ │ -
    │ │ │ │ -
    124 Vector evaluateError(const Rot3& nRb, //
    │ │ │ │ -
    125 boost::optional<Matrix&> H = boost::none) const override {
    │ │ │ │ -
    126 return attitudeError(nRb, H);
    │ │ │ │ -
    127 }
    │ │ │ │ -
    │ │ │ │ -
    128
    │ │ │ │ -
    129private:
    │ │ │ │ -
    130
    │ │ │ │ -
    132 friend class boost::serialization::access;
    │ │ │ │ -
    133 template<class ARCHIVE>
    │ │ │ │ -
    134 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    135 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    144};
    │ │ │ │ -
    │ │ │ │ -
    145
    │ │ │ │ -
    147template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
    │ │ │ │ -
    148
    │ │ │ │ -
    │ │ │ │ -
    153class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
    │ │ │ │ -
    154 public AttitudeFactor {
    │ │ │ │ -
    155
    │ │ │ │ - │ │ │ │ -
    157
    │ │ │ │ -
    158public:
    │ │ │ │ -
    159
    │ │ │ │ -
    161 typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
    │ │ │ │ -
    162
    │ │ │ │ - │ │ │ │ -
    165
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    168 }
    │ │ │ │ -
    │ │ │ │ -
    169
    │ │ │ │ -
    170 ~Pose3AttitudeFactor() override {
    │ │ │ │ -
    171 }
    │ │ │ │ -
    172
    │ │ │ │ -
    │ │ │ │ -
    180 Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
    │ │ │ │ -
    181 const Unit3& bRef = Unit3(0, 0, 1)) :
    │ │ │ │ -
    182 Base(model, key), AttitudeFactor(nZ, bRef) {
    │ │ │ │ -
    183 }
    │ │ │ │ -
    │ │ │ │ -
    184
    │ │ │ │ -
    │ │ │ │ -
    186 gtsam::NonlinearFactor::shared_ptr clone() const override {
    │ │ │ │ -
    187 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
    │ │ │ │ -
    188 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
    │ │ │ │ -
    189 }
    │ │ │ │ -
    │ │ │ │ -
    190
    │ │ │ │ -
    192 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
    │ │ │ │ -
    193 DefaultKeyFormatter) const override;
    │ │ │ │ -
    194
    │ │ │ │ -
    196 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
    │ │ │ │ -
    197
    │ │ │ │ -
    │ │ │ │ -
    199 Vector evaluateError(const Pose3& nTb, //
    │ │ │ │ -
    200 boost::optional<Matrix&> H = boost::none) const override {
    │ │ │ │ -
    201 Vector e = attitudeError(nTb.rotation(), H);
    │ │ │ │ -
    202 if (H) {
    │ │ │ │ -
    203 Matrix H23 = *H;
    │ │ │ │ -
    204 *H = Matrix::Zero(2,6);
    │ │ │ │ -
    205 H->block<2,3>(0,0) = H23;
    │ │ │ │ -
    206 }
    │ │ │ │ -
    207 return e;
    │ │ │ │ -
    208 }
    │ │ │ │ -
    │ │ │ │ -
    209
    │ │ │ │ -
    210private:
    │ │ │ │ -
    211
    │ │ │ │ -
    213 friend class boost::serialization::access;
    │ │ │ │ -
    214 template<class ARCHIVE>
    │ │ │ │ -
    215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    │ │ │ │ -
    216 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    225};
    │ │ │ │ -
    │ │ │ │ -
    226
    │ │ │ │ -
    228template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
    │ │ │ │ -
    229
    │ │ │ │ -
    230}
    │ │ │ │ -
    231
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ -
    3D Pose
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
    │ │ │ │ +
    116 triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
    │ │ │ │ +
    117 }
    │ │ │ │ +
    118
    │ │ │ │ +
    119private:
    │ │ │ │ +
    120
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Functions for triangulation.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ -
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    Base class for prior on attitude Example:
    Definition AttitudeFactor.h:34
    │ │ │ │ -
    Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const
    vector of errors
    Definition AttitudeFactor.cpp:26
    │ │ │ │ -
    Unit3 bRef_
    Position measurement in.
    Definition AttitudeFactor.h:38
    │ │ │ │ -
    AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:51
    │ │ │ │ -
    friend class boost::serialization::access
    Serialization function.
    Definition AttitudeFactor.h:67
    │ │ │ │ -
    AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:43
    │ │ │ │ -
    Version of AttitudeFactor for Rot3.
    Definition AttitudeFactor.h:79
    │ │ │ │ -
    Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition AttitudeFactor.h:124
    │ │ │ │ -
    Rot3AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:92
    │ │ │ │ -
    boost::shared_ptr< Rot3AttitudeFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition AttitudeFactor.h:86
    │ │ │ │ -
    Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:105
    │ │ │ │ -
    Rot3AttitudeFactor This
    Typedef to this class.
    Definition AttitudeFactor.h:89
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AttitudeFactor.h:111
    │ │ │ │ -
    Version of AttitudeFactor for Pose3.
    Definition AttitudeFactor.h:154
    │ │ │ │ -
    Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition AttitudeFactor.h:199
    │ │ │ │ -
    boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
    shorthand for a smart pointer to a factor
    Definition AttitudeFactor.h:161
    │ │ │ │ -
    Pose3AttitudeFactor This
    Typedef to this class.
    Definition AttitudeFactor.h:164
    │ │ │ │ -
    Pose3AttitudeFactor()
    default constructor - only use for serialization
    Definition AttitudeFactor.h:167
    │ │ │ │ -
    gtsam::NonlinearFactor::shared_ptr clone() const override
    Definition AttitudeFactor.h:186
    │ │ │ │ -
    Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
    Constructor.
    Definition AttitudeFactor.h:180
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ +
    DegeneracyMode
    How to manage degeneracy.
    Definition SmartFactorParams.h:35
    │ │ │ │ +
    LinearizationMode
    SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStere...
    Definition SmartFactorParams.h:30
    │ │ │ │ +
    Definition triangulation.h:556
    │ │ │ │ +
    double dynamicOutlierRejectionThreshold
    If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
    Definition triangulation.h:573
    │ │ │ │ +
    double rankTolerance
    threshold to decide whether triangulation is result.degenerate
    Definition triangulation.h:558
    │ │ │ │ +
    double landmarkDistanceThreshold
    if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
    Definition triangulation.h:566
    │ │ │ │ +
    bool enableEPI
    if set to true, will refine triangulation using LM
    Definition triangulation.h:560
    │ │ │ │ +
    Definition SmartFactorParams.h:42
    │ │ │ │ +
    bool throwCheirality
    If true, re-throws Cheirality exceptions (default: false)
    Definition SmartFactorParams.h:55
    │ │ │ │ +
    LinearizationMode linearizationMode
    How to linearize the factor.
    Definition SmartFactorParams.h:44
    │ │ │ │ +
    double retriangulationThreshold
    threshold to decide whether to re-triangulate
    Definition SmartFactorParams.h:50
    │ │ │ │ +
    DegeneracyMode degeneracyMode
    How to linearize the factor.
    Definition SmartFactorParams.h:45
    │ │ │ │ +
    bool verboseCheirality
    If true, prints text for Cheirality exceptions (default: false)
    Definition SmartFactorParams.h:56
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition SmartFactorParams.h:122
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,332 +1,188 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AttitudeFactor.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, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 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_/_P_o_s_e_3_._h> │ │ │ │ │ -22#include │ │ │ │ │ +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_4class _A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ -35 │ │ │ │ │ -36protected: │ │ │ │ │ -37 │ │ │ │ │ -_3_8 _U_n_i_t_3 nZ_, _b_R_e_f__; │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -41 │ │ │ │ │ -_4_3 _A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ -44 } │ │ │ │ │ -45 │ │ │ │ │ -_5_1 _A_t_t_i_t_u_d_e_F_a_c_t_o_r(const _U_n_i_t_3& nZ, const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ -52 nZ_(nZ), _b_R_e_f__(bRef) { │ │ │ │ │ -53 } │ │ │ │ │ -54 │ │ │ │ │ -56 Vector _a_t_t_i_t_u_d_e_E_r_r_o_r(const _R_o_t_3& p, │ │ │ │ │ -57 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_3_> H = boost::none) const; │ │ │ │ │ +_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 │ │ │ │ │ -59 const _U_n_i_t_3& nZ() const { │ │ │ │ │ -60 return nZ_; │ │ │ │ │ -61 } │ │ │ │ │ -62 const Unit3& bRef() const { │ │ │ │ │ -63 return _b_R_e_f__; │ │ │ │ │ -64 } │ │ │ │ │ -65 │ │ │ │ │ -_6_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; │ │ │ │ │ -68 template │ │ │ │ │ -69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -70 ar & boost::serialization::make_nvp("nZ_", nZ_); │ │ │ │ │ -71 ar & boost::serialization::make_nvp("bRef_", _b_R_e_f__); │ │ │ │ │ -72 } │ │ │ │ │ -73}; │ │ │ │ │ -74 │ │ │ │ │ -_7_9class GTSAM_EXPORT _R_o_t_3_A_t_t_i_t_u_d_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, public │ │ │ │ │ -_A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ -80 │ │ │ │ │ -81 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; │ │ │ │ │ -82 │ │ │ │ │ -83public: │ │ │ │ │ -84 │ │ │ │ │ -_8_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -87 │ │ │ │ │ -_8_9 typedef _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -90 │ │ │ │ │ -_9_2 _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -95 _~_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() override { │ │ │ │ │ -96 } │ │ │ │ │ -97 │ │ │ │ │ -_1_0_5 _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r(_K_e_y key, const _U_n_i_t_3& nZ, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -106 const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ -107 _B_a_s_e(model, key), _A_t_t_i_t_u_d_e_F_a_c_t_o_r(nZ, bRef) { │ │ │ │ │ +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 │ │ │ │ │ -_1_1_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -112 return boost::static_pointer_cast( │ │ │ │ │ -113 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +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 │ │ │ │ │ -117 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -118 DefaultKeyFormatter) const override; │ │ │ │ │ -119 │ │ │ │ │ -121 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; │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& nRb, // │ │ │ │ │ -125 boost::optional H = boost::none) const override { │ │ │ │ │ -126 return attitudeError(nRb, H); │ │ │ │ │ -127 } │ │ │ │ │ -128 │ │ │ │ │ -129private: │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 friend class boost::serialization::access; │ │ │ │ │ -133 template │ │ │ │ │ -134 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -135 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -136 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -137 boost::serialization::base_object(*this)); │ │ │ │ │ -138 ar & boost::serialization::make_nvp("AttitudeFactor", │ │ │ │ │ -139 boost::serialization::base_object(*this)); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -142public: │ │ │ │ │ -143 _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 │ │ │ │ │ -144}; │ │ │ │ │ -145 │ │ │ │ │ -_1_4_7template<> struct _t_r_a_i_t_s<_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -148 │ │ │ │ │ -_1_5_3class GTSAM_EXPORT _P_o_s_e_3_A_t_t_i_t_u_d_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, │ │ │ │ │ -154 public _A_t_t_i_t_u_d_e_F_a_c_t_o_r { │ │ │ │ │ -155 │ │ │ │ │ -156 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; │ │ │ │ │ -157 │ │ │ │ │ -158public: │ │ │ │ │ -159 │ │ │ │ │ -_1_6_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -162 │ │ │ │ │ -_1_6_4 typedef _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -165 │ │ │ │ │ -_1_6_7 _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() { │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -170 _~_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r() override { │ │ │ │ │ -171 } │ │ │ │ │ -172 │ │ │ │ │ -_1_8_0 _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r(_K_e_y key, const _U_n_i_t_3& nZ, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model, │ │ │ │ │ -181 const _U_n_i_t_3& bRef = _U_n_i_t_3(0, 0, 1)) : │ │ │ │ │ -182 _B_a_s_e(model, key), _A_t_t_i_t_u_d_e_F_a_c_t_o_r(nZ, bRef) { │ │ │ │ │ -183 } │ │ │ │ │ -184 │ │ │ │ │ -_1_8_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -187 return boost::static_pointer_cast( │ │ │ │ │ -188 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -189 } │ │ │ │ │ -190 │ │ │ │ │ -192 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -193 DefaultKeyFormatter) const override; │ │ │ │ │ -194 │ │ │ │ │ -196 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; │ │ │ │ │ -197 │ │ │ │ │ -_1_9_9 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& nTb, // │ │ │ │ │ -200 boost::optional H = boost::none) const override { │ │ │ │ │ -201 Vector e = attitudeError(nTb._r_o_t_a_t_i_o_n(), H); │ │ │ │ │ -202 if (H) { │ │ │ │ │ -203 Matrix H23 = *H; │ │ │ │ │ -204 *H = Matrix::Zero(2,6); │ │ │ │ │ -205 H->block<2,3>(0,0) = H23; │ │ │ │ │ -206 } │ │ │ │ │ -207 return e; │ │ │ │ │ -208 } │ │ │ │ │ -209 │ │ │ │ │ -210private: │ │ │ │ │ -211 │ │ │ │ │ -_2_1_3 friend class boost::serialization::access; │ │ │ │ │ -214 template │ │ │ │ │ -215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -216 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -217 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -218 boost::serialization::base_object(*this)); │ │ │ │ │ -219 ar & boost::serialization::make_nvp("AttitudeFactor", │ │ │ │ │ -220 boost::serialization::base_object(*this)); │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -223public: │ │ │ │ │ -224 _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 │ │ │ │ │ -225}; │ │ │ │ │ -226 │ │ │ │ │ -_2_2_8template<> struct _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> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -229 │ │ │ │ │ -230} │ │ │ │ │ -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 │ │ │ │ │ -#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 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_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_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -Base class for prior on attitude Example: │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_a_t_t_i_t_u_d_e_E_r_r_o_r │ │ │ │ │ -Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) │ │ │ │ │ -const │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.cpp:26 │ │ │ │ │ -_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_b_R_e_f__ │ │ │ │ │ -Unit3 bRef_ │ │ │ │ │ -Position measurement in. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +_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 AttitudeFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -AttitudeFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:43 │ │ │ │ │ -_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 AttitudeFactor for Rot3. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_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 AttitudeFactor.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -Rot3AttitudeFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< Rot3AttitudeFactor > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, │ │ │ │ │ -const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -Rot3AttitudeFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:111 │ │ │ │ │ -_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 AttitudeFactor for Pose3. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< Pose3AttitudeFactor > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -Pose3AttitudeFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -Pose3AttitudeFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, │ │ │ │ │ -const Unit3 &bRef=Unit3(0, 0, 1)) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn AttitudeFactor.h:180 │ │ │ │ │ -_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 │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:122 │ │ │ │ │ * _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 │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _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/a00977_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00068_source.html │ │ │ │┄ Files 16% similar despite different names │ │ │ │ @@ -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/base/FastSet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    TangentPreintegration.h
    │ │ │ │ +
    FastSet.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
    │ │ │ │ - │ │ │ │ -
    21
    │ │ │ │ -
    22namespace gtsam {
    │ │ │ │ -
    23
    │ │ │ │ -
    │ │ │ │ -
    28class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
    │ │ │ │ -
    29 protected:
    │ │ │ │ -
    30
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    41 resetIntegration();
    │ │ │ │ -
    42 }
    │ │ │ │ -
    │ │ │ │ -
    43
    │ │ │ │ -
    44public:
    │ │ │ │ -
    47
    │ │ │ │ -
    53 TangentPreintegration(const boost::shared_ptr<Params>& p,
    │ │ │ │ - │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    58 }
    │ │ │ │ -
    │ │ │ │ -
    59
    │ │ │ │ -
    61
    │ │ │ │ -
    65 void resetIntegration() override;
    │ │ │ │ -
    66
    │ │ │ │ +
    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
    │ │ │ │ -
    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;
    │ │ │ │ -
    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:
    │ │ │ │ - │ │ │ │ -
    143};
    │ │ │ │ -
    │ │ │ │ -
    144
    │ │ │ │ -
    145}
    │ │ │ │ -
    #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
    This marks a GTSAM object to require alignment.
    Definition types.h:308
    │ │ │ │ - │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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:
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition ImuBias.h:30
    │ │ │ │ -
    PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
    Definition PreintegrationBase.h:41
    │ │ │ │ -
    Integrate on the 9D tangent space of the NavState manifold.
    Definition TangentPreintegration.h:28
    │ │ │ │ -
    Matrix93 preintegrated_H_biasOmega_
    Jacobian of preintegrated_ w.r.t. angular rate bias.
    Definition TangentPreintegration.h:37
    │ │ │ │ -
    ~TangentPreintegration() override
    Virtual destructor.
    Definition TangentPreintegration.h:57
    │ │ │ │ -
    Vector9 preintegrated_
    Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
    Definition TangentPreintegration.h:35
    │ │ │ │ -
    TangentPreintegration()
    Default constructor for serialization.
    Definition TangentPreintegration.h:40
    │ │ │ │ -
    Matrix93 preintegrated_H_biasAcc_
    Jacobian of preintegrated_ w.r.t. acceleration bias.
    Definition TangentPreintegration.h:36
    │ │ │ │ -
    virtual boost::shared_ptr< TangentPreintegration > clone() const
    Dummy clone for MATLAB.
    Definition TangentPreintegration.h:123
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
    Definition FastSet.h:50
    │ │ │ │ +
    void merge(const FastSet &other)
    insert another set: handy for MATLAB access
    Definition FastSet.h:119
    │ │ │ │ +
    void print(const std::string &str="") const
    Print to implement Testable: pretty basic.
    Definition FastSet.h:101
    │ │ │ │ +
    FastSet(const INPUTCONTAINER &container)
    Constructor from a iterable container, passes through to base class.
    Definition FastSet.h:65
    │ │ │ │ +
    FastSet(const FastSet< VALUE > &x)
    Copy constructor from another FastSet.
    Definition FastSet.h:70
    │ │ │ │ +
    bool exists(const VALUE &e) const
    Handy 'exists' function.
    Definition FastSet.h:96
    │ │ │ │ +
    bool equals(const FastSet< VALUE > &other, double tol=1e-9) const
    Check for equality within tolerance to implement Testable.
    Definition FastSet.h:107
    │ │ │ │ +
    friend class boost::serialization::access
    Serialization function.
    Definition FastSet.h:125
    │ │ │ │ +
    FastSet(const Base &x)
    Copy constructor from the base set class.
    Definition FastSet.h:75
    │ │ │ │ +
    FastSet()=default
    Default constructor.
    │ │ │ │ +
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,176 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TangentPreintegration.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 │ │ │ │ │ -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__; │ │ │ │ │ +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 │ │ │ │ │ -_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()); │ │ │ │ │ +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 │ │ │ │ │ -_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 │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ -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 │ │ │ │ │ +_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} │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _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/a00980_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01358_source.html │ │ │ │┄ Files 18% similar despite different names │ │ │ │ @@ -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/symbolic/SymbolicConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    MagFactor.h
    │ │ │ │ +
    SymbolicConditional.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
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/geometry/Rot2.h>
    │ │ │ │ -
    23#include <gtsam/geometry/Rot3.h>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ +
    20#include <gtsam/base/Testable.h>
    │ │ │ │ +
    21#include <gtsam/base/types.h>
    │ │ │ │ +
    22#include <gtsam/inference/Conditional-inst.h>
    │ │ │ │ + │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    │ │ │ │ +
    36 class GTSAM_EXPORT SymbolicConditional :
    │ │ │ │ +
    37 public SymbolicFactor,
    │ │ │ │ +
    38 public Conditional<SymbolicFactor, SymbolicConditional> {
    │ │ │ │ +
    39
    │ │ │ │ +
    40 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
    44 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    50
    │ │ │ │ + │ │ │ │ +
    53
    │ │ │ │ + │ │ │ │
    56
    │ │ │ │ -
    │ │ │ │ -
    58 NonlinearFactor::shared_ptr clone() const override {
    │ │ │ │ -
    59 return boost::static_pointer_cast<NonlinearFactor>(
    │ │ │ │ -
    60 NonlinearFactor::shared_ptr(new MagFactor(*this)));
    │ │ │ │ -
    61 }
    │ │ │ │ -
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 {
    │ │ │ │ + │ │ │ │ +
    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 }
    │ │ │ │
    │ │ │ │ -
    121};
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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:
    │ │ │ │ -
    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
    │ │ │ │ -
    3D rotation represented as a rotation matrix or quaternion
    │ │ │ │ -
    2D rotation
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Typedefs for easier changing of types.
    │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
    Definition Rot2.h:36
    │ │ │ │ -
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ -
    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
    │ │ │ │ -
    static Rot3 Yaw(double t)
    Positive yaw is to right (as in aircraft heading). See ypr.
    Definition Rot3.h:174
    │ │ │ │ -
    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
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
    Return unit-norm Point3.
    Definition Unit3.cpp:144
    │ │ │ │ -
    Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
    Definition MagFactor.h:33
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:58
    │ │ │ │ -
    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
    │ │ │ │ -
    Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition MagFactor.h:77
    │ │ │ │ -
    Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
    Definition MagFactor.h:90
    │ │ │ │ -
    MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:99
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:107
    │ │ │ │ -
    Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
    vector of errors
    Definition MagFactor.h:115
    │ │ │ │ -
    Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
    Definition MagFactor.h:128
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:143
    │ │ │ │ -
    MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:136
    │ │ │ │ -
    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
    │ │ │ │ -
    Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
    Definition MagFactor.h:169
    │ │ │ │ -
    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
    │ │ │ │ -
    MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
    Constructor.
    Definition MagFactor.h:177
    │ │ │ │ -
    NonlinearFactor::shared_ptr clone() const override
    Definition MagFactor.h:184
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Key key() const
    Returns a key.
    Definition NonlinearFactor.h:518
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    HybridValues represents a collection of DiscreteValues and VectorValues.
    Definition HybridValues.h:38
    │ │ │ │ +
    Definition Conditional.h:64
    │ │ │ │ +
    KeyVector::const_iterator const_iterator
    Const iterator over keys.
    Definition Factor.h:80
    │ │ │ │ +
    KeyVector::iterator iterator
    Iterator over keys.
    Definition Factor.h:77
    │ │ │ │ +
    SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
    Definition SymbolicConditional.h:38
    │ │ │ │ +
    SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3)
    Three parents.
    Definition SymbolicConditional.h:64
    │ │ │ │ +
    SymbolicConditional()
    Empty Constructor to make serialization possible.
    Definition SymbolicConditional.h:52
    │ │ │ │ +
    static SymbolicConditional FromKeys(const CONTAINER &keys, size_t nrFrontals)
    Named constructor from an arbitrary number of keys and frontals.
    Definition SymbolicConditional.h:88
    │ │ │ │ +
    BaseFactor::const_iterator const_iterator
    iterator to keys
    Definition SymbolicConditional.h:46
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Typedef to the conditional base class.
    Definition SymbolicConditional.h:44
    │ │ │ │ +
    Conditional< BaseFactor, This > BaseConditional
    Typedef to the factor base class.
    Definition SymbolicConditional.h:43
    │ │ │ │ +
    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
    │ │ │ │ +
    static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
    Named constructor from an arbitrary number of keys and frontals.
    Definition SymbolicConditional.h:68
    │ │ │ │ +
    SymbolicConditional(Key j, Key parent)
    Single parent.
    Definition SymbolicConditional.h:58
    │ │ │ │ +
    SymbolicConditional(Key j)
    No parents.
    Definition SymbolicConditional.h:55
    │ │ │ │ +
    SymbolicConditional(Key j, Key parent1, Key parent2)
    Two parents.
    Definition SymbolicConditional.h:61
    │ │ │ │ +
    SymbolicFactor::shared_ptr clone() const
    Copy this object as its actual derived type.
    Definition SymbolicConditional.h:99
    │ │ │ │ +
    BaseFactor::iterator iterator
    Boost shared_ptr to this class.
    Definition SymbolicConditional.h:45
    │ │ │ │ +
    SymbolicFactor BaseFactor
    Typedef to this class.
    Definition SymbolicConditional.h:42
    │ │ │ │ +
    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
    │ │ │ │ +
    SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
    Definition SymbolicFactor.h:39
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Overriding the shared_ptr typedef.
    Definition SymbolicFactor.h:48
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,297 +1,244 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MagFactor.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_/_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> │ │ │ │ │ +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_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 } │ │ │ │ │ +_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 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 } │ │ │ │ │ +_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 │ │ │ │ │ -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 { │ │ │ │ │ +_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 │ │ │ │ │ -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: │ │ │ │ │ +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 _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}; │ │ │ │ │ +_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 │ │ │ │ │ -_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: │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_g_F_a_c_t_o_r_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00371.js │ │ │ │┄ Files 27% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,14 +1,11 @@ │ │ │ │ │ -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] │ │ │ │ │ +var a00371 = [ │ │ │ │ │ + ["gtsam::Range< Point3, Point3 >", "a03152.html", null], │ │ │ │ │ + ["Point3", "a00371.html#aaa8ed89fd60ea4601d9de63c4811525b", null], │ │ │ │ │ + ["cross", "a00371.html#a86bf6ed2ffb0ecc42a7deb8922f4ca4f", null], │ │ │ │ │ + ["distance3", "a00371.html#aa42f7ec660b7353de39f9c86a26711cc", null], │ │ │ │ │ + ["dot", "a00371.html#a7dc820e4e47218768f104b43a184a1bd", null], │ │ │ │ │ + ["mean", "a00371.html#a300205092710091b8745c0a145da20a1", null], │ │ │ │ │ + ["means", "a00371.html#a57f778b84bc6106471006421d289b832", null], │ │ │ │ │ + ["norm3", "a00371.html#ac37b6f807985ffd25217e33f6136fe58", null], │ │ │ │ │ + ["normalize", "a00371.html#abb52bb00c68909fc1147e5d112e8c2ae", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01067_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01058_source.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/ExtendedKalmanFilter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    GaussNewtonOptimizer.h
    │ │ │ │ +
    ExtendedKalmanFilter.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
    │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ +
    19// \callgraph
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │ -
    25class GaussNewtonOptimizer;
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    │ │ │ │ -
    30class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
    │ │ │ │ -
    31public:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    65 GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
    │ │ │ │ -
    66 const Ordering& ordering);
    │ │ │ │ -
    68
    │ │ │ │ -
    71
    │ │ │ │ - │ │ │ │ -
    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};
    │ │ │ │ +
    44template <class VALUE>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ -
    95}
    │ │ │ │ -
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │ +
    99 T update(const NoiseModelFactor& measurementFactor);
    │ │ │ │ +
    100
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    103 return priorFactor_;
    │ │ │ │ +
    104 }
    │ │ │ │ +
    │ │ │ │ +
    105
    │ │ │ │ +
    107};
    │ │ │ │ +
    │ │ │ │ +
    108
    │ │ │ │ +
    109} // namespace
    │ │ │ │ +
    110
    │ │ │ │ + │ │ │ │ +
    Non-linear factor base classes.
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    Class to perform generic Kalman Filtering using nonlinear factor graphs.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams.
    Definition GaussNewtonOptimizer.h:30
    │ │ │ │ -
    This class performs Gauss-Newton nonlinear optimization.
    Definition GaussNewtonOptimizer.h:38
    │ │ │ │ -
    const GaussNewtonParams & params() const
    Read-only access the parameters.
    Definition GaussNewtonOptimizer.h:82
    │ │ │ │ -
    ~GaussNewtonOptimizer() override
    Virtual destructor.
    Definition GaussNewtonOptimizer.h:73
    │ │ │ │ -
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition GaussNewtonOptimizer.h:88
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ +
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition JacobianFactor.h:96
    │ │ │ │ +
    This is a generic Extended Kalman Filter class implemented using nonlinear factors.
    Definition ExtendedKalmanFilter.h:45
    │ │ │ │ +
    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
    │ │ │ │ +
    const JacobianFactor::shared_ptr Density() const
    Return current predictive (if called after predict)/posterior (if called after update)
    Definition ExtendedKalmanFilter.h:102
    │ │ │ │ +
    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
    │ │ │ │ +
    void print(const std::string &s="") const
    print
    Definition ExtendedKalmanFilter.h:78
    │ │ │ │ +
    A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
    Definition NonlinearFactor.h:174
    │ │ │ │ +
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,111 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussNewtonOptimizer.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_/_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 { │ │ │ │ │ +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 │ │ │ │ │ -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}; │ │ │ │ │ -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_; } │ │ │ │ │ +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 │ │ │ │ │ +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 │ │ │ │ │ -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}; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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_._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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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_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 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01076_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01193_source.html │ │ │ │┄ Files 6% 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/sfm/BinaryMeasurement.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    DoglegOptimizer.h
    │ │ │ │ +
    BinaryMeasurement.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ -
    22
    │ │ │ │ -
    23namespace gtsam {
    │ │ │ │ -
    24
    │ │ │ │ -
    25class DoglegOptimizer;
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ -
    32class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams {
    │ │ │ │ -
    33public:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    36 SILENT,
    │ │ │ │ -
    37 VERBOSE
    │ │ │ │ -
    38 };
    │ │ │ │ -
    │ │ │ │ +
    12#pragma once
    │ │ │ │ +
    13
    │ │ │ │ +
    26#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ +
    28#include <gtsam/inference/Key.h>
    │ │ │ │ + │ │ │ │ +
    30
    │ │ │ │ +
    31#include <iostream>
    │ │ │ │ +
    32#include <vector>
    │ │ │ │ +
    33
    │ │ │ │ +
    34namespace gtsam {
    │ │ │ │ +
    35
    │ │ │ │ +
    │ │ │ │ +
    36template <class T> class BinaryMeasurement : public Factor {
    │ │ │ │ +
    37 // Check that T type is testable
    │ │ │ │ +
    38 BOOST_CONCEPT_ASSERT((IsTestable<T>));
    │ │ │ │
    39
    │ │ │ │ -
    40 double deltaInitial;
    │ │ │ │ - │ │ │ │ -
    42
    │ │ │ │ -
    43 DoglegParams() :
    │ │ │ │ -
    44 deltaInitial(1.0), verbosityDL(SILENT) {}
    │ │ │ │ -
    45
    │ │ │ │ -
    46 ~DoglegParams() override {}
    │ │ │ │ +
    40public:
    │ │ │ │ +
    41 // shorthand for a smart pointer to a measurement
    │ │ │ │ +
    42 using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;
    │ │ │ │ +
    43
    │ │ │ │ +
    44private:
    │ │ │ │ +
    45 T measured_;
    │ │ │ │ +
    46 SharedNoiseModel noiseModel_;
    │ │ │ │
    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 {
    │ │ │ │ +
    48 public:
    │ │ │ │ +
    49 BinaryMeasurement(Key key1, Key key2, const T &measured,
    │ │ │ │ +
    50 const SharedNoiseModel &model = nullptr)
    │ │ │ │ +
    51 : Factor(std::vector<Key>({key1, key2})),
    │ │ │ │ +
    52 measured_(measured),
    │ │ │ │ +
    53 noiseModel_(model) {}
    │ │ │ │ +
    54
    │ │ │ │ +
    56 virtual ~BinaryMeasurement() {}
    │ │ │ │ +
    57
    │ │ │ │ +
    60
    │ │ │ │ +
    61 Key key1() const { return keys_[0]; }
    │ │ │ │ +
    62 Key key2() const { return keys_[1]; }
    │ │ │ │ +
    63 const T &measured() const { return measured_; }
    │ │ │ │ +
    64 const SharedNoiseModel &noiseModel() const { return noiseModel_; }
    │ │ │ │ +
    65
    │ │ │ │
    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}
    │ │ │ │ -
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │ +
    │ │ │ │ +
    70 void print(const std::string &s, const KeyFormatter &keyFormatter =
    │ │ │ │ +
    71 DefaultKeyFormatter) const override {
    │ │ │ │ +
    72 std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
    │ │ │ │ +
    73 << keyFormatter(this->key2()) << ")\n";
    │ │ │ │ +
    74 traits<T>::Print(measured_, " measured: ");
    │ │ │ │ +
    75 this->noiseModel_->print(" noise model: ");
    │ │ │ │ +
    76 }
    │ │ │ │ +
    │ │ │ │ +
    77
    │ │ │ │ +
    78 bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
    │ │ │ │ +
    79 const BinaryMeasurement<T> *e =
    │ │ │ │ +
    80 dynamic_cast<const BinaryMeasurement<T> *>(&expected);
    │ │ │ │ +
    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
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    The base class for all factors.
    │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    Definition Ordering.h:34
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    Parameters for Levenberg-Marquardt optimization.
    Definition DoglegOptimizer.h:32
    │ │ │ │ -
    VerbosityDL
    See DoglegParams::dlVerbosity.
    Definition DoglegOptimizer.h:35
    │ │ │ │ -
    VerbosityDL verbosityDL
    The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity.
    Definition DoglegOptimizer.h:41
    │ │ │ │ -
    double deltaInitial
    The initial trust region radius (default: 10.0)
    Definition DoglegOptimizer.h:40
    │ │ │ │ -
    This class performs Dogleg nonlinear optimization.
    Definition DoglegOptimizer.h:68
    │ │ │ │ -
    ~DoglegOptimizer() override
    Virtual destructor.
    Definition DoglegOptimizer.h:106
    │ │ │ │ -
    const NonlinearOptimizerParams & _params() const override
    Access the parameters (base class version)
    Definition DoglegOptimizer.h:124
    │ │ │ │ -
    const DoglegParams & params() const
    Read-only access the parameters.
    Definition DoglegOptimizer.h:115
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A testable concept check that should be placed in applicable unit tests and in generic algorithms.
    Definition Testable.h:58
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Definition Factor.h:68
    │ │ │ │ +
    KeyVector keys_
    The keys involved in this factor.
    Definition Factor.h:85
    │ │ │ │ +
    Factor()
    Default constructor for I/O.
    Definition Factor.h:91
    │ │ │ │ +
    bool equals(const This &other, double tol=1e-9) const
    check equality
    Definition Factor.cpp:42
    │ │ │ │ +
    Definition BinaryMeasurement.h:36
    │ │ │ │ +
    void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition BinaryMeasurement.h:70
    │ │ │ │ +
    virtual ~BinaryMeasurement()
    Destructor.
    Definition BinaryMeasurement.h:56
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,158 +1,138 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DoglegOptimizer.h │ │ │ │ │ +BinaryMeasurement.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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 │ │ │ │ │ -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 │ │ │ │ │ -_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 }; │ │ │ │ │ +12#pragma once │ │ │ │ │ +13 │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._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_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._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 │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33 │ │ │ │ │ +34namespace _g_t_s_a_m { │ │ │ │ │ +35 │ │ │ │ │ +_3_6template class _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t : public _F_a_c_t_o_r { │ │ │ │ │ +37 // Check that T type is testable │ │ │ │ │ +38 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_T_>)); │ │ │ │ │ 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 {} │ │ │ │ │ +40public: │ │ │ │ │ +41 // shorthand for a smart pointer to a measurement │ │ │ │ │ +42 using shared_ptr = typename boost::shared_ptr; │ │ │ │ │ +43 │ │ │ │ │ +44private: │ │ │ │ │ +45 T measured_; │ │ │ │ │ +46 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ 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 { │ │ │ │ │ +48 public: │ │ │ │ │ +49 _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t(_K_e_y key1, _K_e_y key2, const T &measured, │ │ │ │ │ +50 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model = nullptr) │ │ │ │ │ +51 : _F_a_c_t_o_r(std::vector({key1, key2})), │ │ │ │ │ +52 measured_(measured), │ │ │ │ │ +53 noiseModel_(model) {} │ │ │ │ │ +54 │ │ │ │ │ +_5_6 virtual _~_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t() {} │ │ │ │ │ +57 │ │ │ │ │ +60 │ │ │ │ │ +61 _K_e_y key1() const { return _k_e_y_s__[0]; } │ │ │ │ │ +62 _K_e_y key2() const { return _k_e_y_s__[1]; } │ │ │ │ │ +63 const T &measured() const { return measured_; } │ │ │ │ │ +64 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &noiseModel() const { return noiseModel_; } │ │ │ │ │ +65 │ │ │ │ │ 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. │ │ │ │ │ +_7_0 void _p_r_i_n_t(const std::string &s, const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = │ │ │ │ │ +71 DefaultKeyFormatter) const override { │ │ │ │ │ +72 std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," │ │ │ │ │ +73 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ +74 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(measured_, " measured: "); │ │ │ │ │ +75 this->noiseModel_->print(" noise model: "); │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +78 bool _e_q_u_a_l_s(const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t &expected, double tol = 1e-9) const { │ │ │ │ │ +79 const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_T_> *e = │ │ │ │ │ +80 dynamic_cast *>(&expected); │ │ │ │ │ +81 return e != nullptr && _F_a_c_t_o_r_:_:_e_q_u_a_l_s(*e) && │ │ │ │ │ +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. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_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_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_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_:_:_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_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn BinaryMeasurement.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_:_:_~_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +virtual ~BinaryMeasurement() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn BinaryMeasurement.h:56 │ │ │ │ │ * _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 │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _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/a01079.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00275.html │ │ │ │┄ Files 14% similar despite different names │ │ │ │ @@ -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/geometry/Cal3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Namespaces
    │ │ │ │ -
    LevenbergMarquardtParams.cpp File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    Cal3.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │ -

    Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ +

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

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

    │ │ │ │ Namespaces

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

    │ │ │ │ +Functions

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

    Detailed Description

    │ │ │ │ -

    Parameters for Levenberg-Marquardt trust-region scheme.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +

    Common code for all calibration models.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,23 @@ │ │ │ │ │ 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_._._. │ │ │ │ │ +_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 ********** │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +Common code for all calibration models. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 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_P_a_r_a_m_s_._c_p_p │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a01085.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01340.html │ │ │ │┄ Files 8% similar despite different names │ │ │ │ @@ -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/slam/BoundingConstraint.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    LevenbergMarquardtOptimizer.h File Reference
    │ │ │ │ +
    BoundingConstraint.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ +

    Provides partially implemented constraints to implement bounds. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::LevenbergMarquardtOptimizer
     This class performs Levenberg-Marquardt nonlinear optimization. More...
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

    │ │ │ │ -
    Author
    Richard Roberts
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Luca Carlone
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +

    Provides partially implemented constraints to implement bounds.

    │ │ │ │ +
    Author
    Alex Cunningham
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +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 │ │ │ │ │ -LevenbergMarquardtOptimizer.h File Reference │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_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_:_:_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_._._. │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +Provides partially implemented constraints to implement bounds. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _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 │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _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/a01088_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00530_source.html │ │ │ │┄ Files 9% 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/hybrid/HybridNonlinearISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    WhiteNoiseFactor.h
    │ │ │ │ +
    HybridNonlinearISAM.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24#include <cmath>
    │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28 const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
    │ │ │ │ -
    29
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    42
    │ │ │ │ -
    43 private:
    │ │ │ │ +
    18#pragma once
    │ │ │ │ +
    19
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    │ │ │ │ +
    27class GTSAM_EXPORT HybridNonlinearISAM {
    │ │ │ │ +
    28 protected:
    │ │ │ │ + │ │ │ │ +
    31
    │ │ │ │ + │ │ │ │ +
    34
    │ │ │ │ + │ │ │ │ +
    37
    │ │ │ │ + │ │ │ │ +
    40
    │ │ │ │ + │ │ │ │ +
    43 int reorderCounter_;
    │ │ │ │
    44
    │ │ │ │ -
    45 double z_;
    │ │ │ │ -
    46
    │ │ │ │ -
    47 Key meanKey_;
    │ │ │ │ -
    48 Key precisionKey_;
    │ │ │ │ -
    49
    │ │ │ │ -
    50 typedef NonlinearFactor Base;
    │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    48 public:
    │ │ │ │
    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();
    │ │ │ │ - │ │ │ │ -
    85 new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
    │ │ │ │ -
    86 }
    │ │ │ │ -
    │ │ │ │ -
    87
    │ │ │ │ -
    90
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
    │ │ │ │ -
    97 Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
    │ │ │ │ -
    98 }
    │ │ │ │ + │ │ │ │ +
    97 return factors_;
    │ │ │ │ +
    98 }
    │ │ │ │
    │ │ │ │
    99
    │ │ │ │ +
    101 int reorderInterval() const { return reorderInterval_; }
    │ │ │ │ +
    │ │ │ │ +
    102 int reorderCounter() const { return reorderCounter_; }
    │ │ │ │
    103
    │ │ │ │ -
    │ │ │ │ -
    105 ~WhiteNoiseFactor() override {
    │ │ │ │ -
    106 }
    │ │ │ │ -
    │ │ │ │ +
    105 void print(const std::string& s = "",
    │ │ │ │ +
    106 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │
    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 }
    │ │ │ │ -
    │ │ │ │ +
    109 void printStats() const;
    │ │ │ │ +
    110
    │ │ │ │ +
    112 void saveGraph(const std::string& s,
    │ │ │ │ +
    113 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
    │ │ │ │ +
    114
    │ │ │ │
    118
    │ │ │ │ -
    122
    │ │ │ │ -
    │ │ │ │ -
    124 size_t dim() const override {
    │ │ │ │ -
    125 return 2;
    │ │ │ │ -
    126 }
    │ │ │ │ -
    │ │ │ │ +
    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 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
    │ │ │ │ -
    Contains the HessianFactor class, a general quadratic factor.
    │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    129};
    │ │ │ │ +
    130
    │ │ │ │ +
    131} // namespace gtsam
    │ │ │ │ +
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    Nonlinear hybrid factor graph that uses type erasure.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    const double logSqrt2PI
    constant needed below
    Definition WhiteNoiseFactor.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │
    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
    │ │ │ │ -
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    A shared_ptr to this class.
    Definition HessianFactor.h:110
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactor.cpp:37
    │ │ │ │ +
    A map from keys to values.
    Definition DiscreteValues.h:34
    │ │ │ │ +
    void prune(const size_t maxNumberLeaves)
    Prune the underlying Bayes tree.
    Definition HybridBayesTree.cpp:175
    │ │ │ │ +
    Definition HybridGaussianISAM.h:34
    │ │ │ │ +
    Definition HybridNonlinearFactorGraph.h:33
    │ │ │ │ +
    Wrapper class to manage ISAM in a nonlinear context.
    Definition HybridNonlinearISAM.h:27
    │ │ │ │ +
    const HybridNonlinearFactorGraph & getFactorsUnsafe() const
    get underlying nonlinear graph
    Definition HybridNonlinearISAM.h:96
    │ │ │ │ +
    DiscreteValues assignment_
    The discrete assignment.
    Definition HybridNonlinearISAM.h:36
    │ │ │ │ +
    const Values & getLinearizationPoint() const
    Return the current linearization point.
    Definition HybridNonlinearISAM.h:90
    │ │ │ │ +
    HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
    Periodically reorder and relinearize.
    Definition HybridNonlinearISAM.h:59
    │ │ │ │ +
    HybridNonlinearFactorGraph factors_
    The original factors, used when relinearizing.
    Definition HybridNonlinearISAM.h:39
    │ │ │ │ +
    const DiscreteValues & assignment() const
    Return the current discrete assignment.
    Definition HybridNonlinearISAM.h:93
    │ │ │ │ +
    int reorderCounter() const
    TODO: comment.
    Definition HybridNonlinearISAM.h:102
    │ │ │ │ +
    void prune(const size_t maxNumberLeaves)
    Prune the underlying Bayes tree.
    Definition HybridNonlinearISAM.h:87
    │ │ │ │ +
    int reorderInterval_
    The reordering interval and counter.
    Definition HybridNonlinearISAM.h:42
    │ │ │ │ +
    int reorderInterval() const
    get counters
    Definition HybridNonlinearISAM.h:101
    │ │ │ │ +
    Values linPoint_
    The current linearization point.
    Definition HybridNonlinearISAM.h:33
    │ │ │ │ +
    gtsam::HybridGaussianISAM isam_
    The internal iSAM object.
    Definition HybridNonlinearISAM.h:30
    │ │ │ │ +
    HybridGaussianFactorGraph::Eliminate eliminationFunction_
    The elimination function.
    Definition HybridNonlinearISAM.h:46
    │ │ │ │ +
    const HybridGaussianISAM & bayesTree() const
    access the underlying bayes tree
    Definition HybridNonlinearISAM.h:80
    │ │ │ │ +
    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
    │ │ │ │
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    Binary factor to estimate parameters of zero-mean Gaussian white noise.
    Definition WhiteNoiseFactor.h:41
    │ │ │ │ -
    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
    │ │ │ │ -
    ~WhiteNoiseFactor() override
    Destructor.
    Definition WhiteNoiseFactor.h:105
    │ │ │ │ -
    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
    │ │ │ │ -
    void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    Print.
    Definition WhiteNoiseFactor.h:113
    │ │ │ │ -
    WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
    Construct from measurement.
    Definition WhiteNoiseFactor.h:96
    │ │ │ │ -
    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
    │ │ │ │ -
    static double f(double z, double u, double p)
    negative log likelihood as a function of mean and precision
    Definition WhiteNoiseFactor.h:61
    │ │ │ │ -
    double error(const Values &x) const override
    Calculate the error of the factor, typically equal to log-likelihood.
    Definition WhiteNoiseFactor.h:129
    │ │ │ │ -
    size_t dim() const override
    get the dimension of the factor (number of rows on linearization)
    Definition WhiteNoiseFactor.h:124
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,204 +1,191 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -WhiteNoiseFactor.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 │ │ │ │ │ -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 private: │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -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) { │ │ │ │ │ +_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; │ │ │ │ │ +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 │ │ │ │ │ -_1_0_5 _~_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r() override { │ │ │ │ │ -106 } │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 size_t _d_i_m() const override { │ │ │ │ │ -125 return 2; │ │ │ │ │ -126 } │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +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_:_:_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_:_:_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_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01073_source.html │ │ │ │┄ Files 2% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    nonlinearExceptions.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,15 +117,15 @@ │ │ │ │
    10 * -------------------------------------------------------------------------- */
    │ │ │ │
    11
    │ │ │ │
    18#pragma once
    │ │ │ │
    19
    │ │ │ │
    20#include <boost/lexical_cast.hpp>
    │ │ │ │
    21#include <exception>
    │ │ │ │
    22
    │ │ │ │ -
    23#include <gtsam/inference/Key.h>
    │ │ │ │ +
    23#include <gtsam/inference/Key.h>
    │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │
    │ │ │ │
    33 class MarginalizeNonleafException : public std::exception {
    │ │ │ │
    34 Key key_;
    │ │ │ │
    35 KeyFormatter formatter_;
    │ │ │ │ @@ -144,23 +144,23 @@ │ │ │ │
    48ISAM2::update().\n";
    │ │ │ │
    49 return what_.c_str();
    │ │ │ │
    50 }
    │ │ │ │
    51 };
    │ │ │ │
    │ │ │ │
    52
    │ │ │ │
    53}
    │ │ │ │ - │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │
    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
    │ │ │ │
    Thrown when requesting to marginalize out variables from ISAM2 that are not leaves.
    Definition nonlinearExceptions.h:33
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01121_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00677_source.html │ │ │ │┄ Files 21% similar despite different names │ │ │ │ @@ -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/inference/LabeledSymbol.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    LevenbergMarquardtParams.h
    │ │ │ │ +
    LabeledSymbol.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    28class LevenbergMarquardtOptimizer;
    │ │ │ │ -
    29
    │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <functional>
    │ │ │ │ + │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │
    │ │ │ │ - │ │ │ │ -
    36
    │ │ │ │ -
    37public:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    35class GTSAM_EXPORT LabeledSymbol {
    │ │ │ │ +
    36protected:
    │ │ │ │ +
    37 unsigned char c_, label_;
    │ │ │ │ +
    38 std::uint64_t j_;
    │ │ │ │ +
    39
    │ │ │ │ +
    40public:
    │ │ │ │ + │ │ │ │ +
    43
    │ │ │ │ +
    45 LabeledSymbol(const LabeledSymbol& key);
    │ │ │ │
    46
    │ │ │ │ -
    47public:
    │ │ │ │ -
    48
    │ │ │ │ - │ │ │ │ -
    50 double lambdaFactor;
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    55 std::string logFile;
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    58 double minDiagonal;
    │ │ │ │ -
    59 double maxDiagonal;
    │ │ │ │ -
    60
    │ │ │ │ - │ │ │ │ -
    62 : verbosityLM(SILENT),
    │ │ │ │ -
    63 diagonalDamping(false),
    │ │ │ │ -
    64 minDiagonal(1e-6),
    │ │ │ │ -
    65 maxDiagonal(1e32) {
    │ │ │ │ -
    66 SetLegacyDefaults(this);
    │ │ │ │ -
    67 }
    │ │ │ │ +
    48 LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
    │ │ │ │ +
    49
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 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 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 }
    │ │ │ │ +
    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
    │ │ │ │
    │ │ │ │ -
    155
    │ │ │ │ -
    157};
    │ │ │ │ -
    │ │ │ │ -
    158
    │ │ │ │ -
    159}
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Parameters for nonlinear optimization.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    unsigned char mrsymbolChr(Key key)
    Return the character portion of a symbol key.
    Definition LabeledSymbol.h:131
    │ │ │ │ +
    size_t mrsymbolIndex(Key key)
    Return the index portion of a symbol key.
    Definition LabeledSymbol.h:137
    │ │ │ │ +
    unsigned char mrsymbolLabel(Key key)
    Return the label portion of a symbol key.
    Definition LabeledSymbol.h:134
    │ │ │ │
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    This class performs Levenberg-Marquardt nonlinear optimization.
    Definition LevenbergMarquardtOptimizer.h:35
    │ │ │ │ -
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ -
    double lambdaFactor
    The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
    Definition LevenbergMarquardtParams.h:50
    │ │ │ │ -
    double minDiagonal
    when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
    Definition LevenbergMarquardtParams.h:58
    │ │ │ │ -
    double lambdaUpperBound
    The maximum lambda to try before assuming the optimization has failed (default: 1e5)
    Definition LevenbergMarquardtParams.h:51
    │ │ │ │ -
    double lambdaInitial
    The initial Levenberg-Marquardt damping term (default: 1e-5)
    Definition LevenbergMarquardtParams.h:49
    │ │ │ │ -
    double maxDiagonal
    when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
    Definition LevenbergMarquardtParams.h:59
    │ │ │ │ -
    double minModelFidelity
    Lower bound for the modelFidelity to accept the result of an LM iteration.
    Definition LevenbergMarquardtParams.h:54
    │ │ │ │ -
    double lambdaLowerBound
    The minimum lambda used in LM (default: 0)
    Definition LevenbergMarquardtParams.h:52
    │ │ │ │ -
    boost::shared_ptr< NonlinearOptimizerParams > clone() const
    Definition LevenbergMarquardtParams.h:152
    │ │ │ │ -
    bool diagonalDamping
    if true, use diagonal of Hessian
    Definition LevenbergMarquardtParams.h:56
    │ │ │ │ -
    bool useFixedLambdaFactor
    if true applies constant increase (or decrease) to lambda according to lambdaFactor
    Definition LevenbergMarquardtParams.h:57
    │ │ │ │ -
    std::string logFile
    an optional CSV log file, with [iteration, time, error, lambda]
    Definition LevenbergMarquardtParams.h:55
    │ │ │ │ -
    VerbosityLM
    See LevenbergMarquardtParams::verbosityLM.
    Definition LevenbergMarquardtParams.h:39
    │ │ │ │ -
    VerbosityLM verbosityLM
    The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
    Definition LevenbergMarquardtParams.h:53
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ -
    double absoluteErrorTol
    The maximum absolute error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:43
    │ │ │ │ -
    size_t maxIterations
    The maximum iterations to stop iterating (default 100)
    Definition NonlinearOptimizerParams.h:41
    │ │ │ │ -
    double relativeErrorTol
    The maximum relative error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:42
    │ │ │ │ +
    bool operator!=(const Matrix &A, const Matrix &B)
    inequality
    Definition Matrix.h:107
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    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
    │ │ │ │ +
    bool operator==(const Matrix &A, const Matrix &B)
    equality is just equal_with_abs_tol 1e-9
    Definition Matrix.h:100
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Customized version of gtsam::Symbol for multi-robot use.
    Definition LabeledSymbol.h:35
    │ │ │ │ +
    unsigned char chr() const
    Retrieve key character.
    Definition LabeledSymbol.h:70
    │ │ │ │ +
    gtsam::Key key() const
    return the integer version
    Definition LabeledSymbol.h:64
    │ │ │ │ +
    size_t index() const
    Retrieve key index.
    Definition LabeledSymbol.h:73
    │ │ │ │ +
    unsigned char label() const
    Retrieve label character.
    Definition LabeledSymbol.h:67
    │ │ │ │ + │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,247 +1,189 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LevenbergMarquardtParams.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 │ │ │ │ │ -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; │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +_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 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 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 } │ │ │ │ │ +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 │ │ │ │ │ -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);} │ │ │ │ │ +_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 │ │ │ │ │ -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. │ │ │ │ │ +_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_:_:_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_:_:_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 │ │ │ │ │ - * _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_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/a01133.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01109.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/Marginals.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    GaussNewtonOptimizer.cpp File Reference
    │ │ │ │ +
    Marginals.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │
    Author
    Richard Roberts
    │ │ │ │ -
    Date
    Feb 26, 2012
    │ │ │ │ +
    Date
    May 14, 2012
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ +Marginals.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + May 14, 2012 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01139_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01133_source.html │ │ │ │┄ Files 15% similar despite different names │ │ │ │ @@ -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/Values-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2-impl.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    25#pragma once
    │ │ │ │ +
    26
    │ │ │ │ +
    27#include <utility>
    │ │ │ │ +
    28#include <boost/bind/bind.hpp>
    │ │ │ │ + │ │ │ │
    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
    │ │ │ │ -
    37
    │ │ │ │ -
    38#include <algorithm>
    │ │ │ │ -
    39#include <limits>
    │ │ │ │ -
    40#include <string>
    │ │ │ │ -
    41#include <utility>
    │ │ │ │ +
    31namespace gtsam {
    │ │ │ │ +
    32
    │ │ │ │ +
    33
    │ │ │ │ +
    34 /* ************************************************************************* */
    │ │ │ │ +
    35 template<class ValueType>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    37 const Key key;
    │ │ │ │ +
    38 ValueType& value;
    │ │ │ │ +
    39
    │ │ │ │ +
    40 _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
    │ │ │ │ +
    41 };
    │ │ │ │ +
    │ │ │ │
    42
    │ │ │ │ -
    43namespace gtsam {
    │ │ │ │ -
    44
    │ │ │ │ -
    45/* ************************************************************************* */
    │ │ │ │ -
    46// Special BayesTree class that uses ISAM2 cliques - this is the result of
    │ │ │ │ -
    47// reeliminating ISAM2 subtrees.
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    49 public:
    │ │ │ │ -
    50 typedef ISAM2::Base Base;
    │ │ │ │ -
    51 typedef ISAM2BayesTree This;
    │ │ │ │ -
    52 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    53
    │ │ │ │ - │ │ │ │ -
    55};
    │ │ │ │ +
    43 /* ************************************************************************* */
    │ │ │ │ +
    44 template<class ValueType>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    46 const Key key;
    │ │ │ │ +
    47 const ValueType& value;
    │ │ │ │ +
    48
    │ │ │ │ +
    49 _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
    │ │ │ │ +
    50 key(_key), value(_value) {
    │ │ │ │ +
    51 }
    │ │ │ │ + │ │ │ │ +
    53 key(rhs.key), value(rhs.value) {
    │ │ │ │ +
    54 }
    │ │ │ │ +
    55 };
    │ │ │ │
    │ │ │ │
    56
    │ │ │ │ -
    57/* ************************************************************************* */
    │ │ │ │ -
    58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
    │ │ │ │ -
    59// reeliminating ISAM2 subtrees.
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    61 : public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
    │ │ │ │ -
    62 public:
    │ │ │ │ - │ │ │ │ -
    64 typedef ISAM2JunctionTree This;
    │ │ │ │ -
    65 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    66
    │ │ │ │ -
    67 explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
    │ │ │ │ -
    68 : Base(eliminationTree) {}
    │ │ │ │ -
    69};
    │ │ │ │ +
    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>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    71/* ************************************************************************* */
    │ │ │ │ +
    70 // partial specialized version for ValueType == Value
    │ │ │ │ +
    71 template<class CastedKeyValuePairType, class KeyValuePairType>
    │ │ │ │
    │ │ │ │ -
    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};
    │ │ │ │ +
    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/* ************************************************************************* */
    │ │ │ │ -
    │ │ │ │ -
    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) {}
    │ │ │ │ +
    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
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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_; }
    │ │ │ │
    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);
    │ │ │ │ -
    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 }
    │ │ │ │ +
    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 // 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 }
    │ │ │ │ +
    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));
    │ │ │ │
    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.");
    │ │ │ │ -
    268
    │ │ │ │ -
    269 // Check for relinearization
    │ │ │ │ -
    270 if ((deltaVar.array().abs() > threshold.array()).any()) {
    │ │ │ │ -
    271 relinKeys->insert(var);
    │ │ │ │ -
    272 relinearize = true;
    │ │ │ │ -
    273 }
    │ │ │ │ -
    274 }
    │ │ │ │ +
    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
    │ │ │ │
    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 }
    │ │ │ │ +
    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 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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ +
    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>
    │ │ │ │ + │ │ │ │ +
    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) {
    │ │ │ │ + │ │ │ │ +
    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)
    │ │ │ │ + │ │ │ │ +
    371 return h(j, item->second);
    │ │ │ │
    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);
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    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 // 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
    │ │ │ │ -
    Global debugging flags.
    │ │ │ │ -
    The junction tree, template bodies.
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ -
    Class that stores detailed iSAM2 result.
    │ │ │ │ -
    Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
    │ │ │ │ +
    415}
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    KeySet keys() const
    Potentially slow function to return all keys involved, sorted, as a set.
    Definition FactorGraph-inst.h:85
    │ │ │ │ -
    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
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ -
    void remove(size_t i)
    delete factor without re-arranging indexes by inserting a nullptr pointer
    Definition FactorGraph.h:385
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
    print
    Definition BayesTree-inst.h:212
    │ │ │ │ -
    FastVector< sharedClique > Roots
    Root cliques.
    Definition BayesTree.h:95
    │ │ │ │ -
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ -
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ -
    unsigned char chr() const
    Retrieve key character.
    Definition Symbol.h:73
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ -
    void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
    Remove entries corresponding to the specified factors.
    Definition VariableIndex-inl.h:54
    │ │ │ │ -
    bool empty(Key variable) const
    Return true if no factors associated with a variable.
    Definition VariableIndex.h:98
    │ │ │ │ -
    Definition GaussianEliminationTree.h:29
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Definition ISAM2-impl.h:48
    │ │ │ │ -
    Definition ISAM2-impl.h:61
    │ │ │ │ -
    Definition ISAM2-impl.h:72
    │ │ │ │ -
    Definition ISAM2-impl.h:73
    │ │ │ │ -
    Definition ISAM2-impl.h:77
    │ │ │ │ -
    Implementation functions for update method All of the methods below have clear inputs and outputs,...
    Definition ISAM2-impl.h:114
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
    Definition ISAM2.h:45
    │ │ │ │ -
    Base::sharedClique sharedClique
    Shared pointer to a clique.
    Definition ISAM2.h:103
    │ │ │ │ -
    Definition ISAM2Params.h:135
    │ │ │ │ -
    boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
    Either a constant relinearization threshold or a per-variable-type set of thresholds.
    Definition ISAM2Params.h:140
    │ │ │ │ -
    This struct is returned from ISAM2::update() and contains information about the update that is useful...
    Definition ISAM2Result.h:41
    │ │ │ │ -
    This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
    Definition ISAM2UpdateParams.h:32
    │ │ │ │ -
    bool force_relinearize
    Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
    Definition ISAM2UpdateParams.h:54
    │ │ │ │ -
    FactorIndices removeFactorIndices
    Indices of factors to remove from system (default: empty)
    Definition ISAM2UpdateParams.h:36
    │ │ │ │ -
    bool forceFullSolve
    By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
    Definition ISAM2UpdateParams.h:71
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::optional< FastList< Key > > noRelinKeys
    An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
    Definition ISAM2UpdateParams.h:44
    │ │ │ │ -
    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
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
    print
    Definition NonlinearFactorGraph.cpp:55
    │ │ │ │ -
    double error(const Values &values) const
    unnormalized error, in the most common case
    Definition NonlinearFactorGraph.cpp:170
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    Wraps any type T so it can play as a Value.
    Definition GenericValue.h:47
    │ │ │ │ +
    const T & value() const
    Return a constant value.
    Definition GenericValue.h:67
    │ │ │ │ +
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ +
    Definition Values-inl.h:36
    │ │ │ │ +
    const Key key
    The key.
    Definition Values-inl.h:37
    │ │ │ │ +
    ValueType & value
    The value.
    Definition Values-inl.h:38
    │ │ │ │ +
    Definition Values-inl.h:45
    │ │ │ │ +
    const Key key
    The key.
    Definition Values-inl.h:46
    │ │ │ │ +
    const ValueType & value
    The value.
    Definition Values-inl.h:47
    │ │ │ │ +
    Definition Values-inl.h:62
    │ │ │ │ +
    Definition Values-inl.h:291
    │ │ │ │ +
    Definition Values-inl.h:304
    │ │ │ │ +
    void update(Key j, const Value &val)
    single element change of existing element
    Definition Values.cpp:180
    │ │ │ │ +
    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
    │ │ │ │ +
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ +
    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
    │ │ │ │ +
    void insert_or_assign(Key j, const Value &val)
    If key j exists, update value, else perform an insert.
    Definition Values.cpp:203
    │ │ │ │ +
    Values()=default
    Default constructor creates an empty Values class.
    │ │ │ │ +
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:93
    │ │ │ │ +
    A key-value pair, which you get by dereferencing iterators.
    Definition Values.h:101
    │ │ │ │ +
    Definition Values.h:475
    │ │ │ │ +
    Definition Values.h:498
    │ │ │ │ +
    Definition Values.h:542
    │ │ │ │
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ - │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,689 +1,525 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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_. │ │ │ │ │ +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 │ │ │ │ │ -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> │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -43namespace _g_t_s_a_m { │ │ │ │ │ -44 │ │ │ │ │ -45/* ************************************************************************* │ │ │ │ │ +43 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -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}; │ │ │ │ │ +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 │ │ │ │ │ -57/* ************************************************************************* │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -_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}; │ │ │ │ │ +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/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -_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) {} │ │ │ │ │ +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 │ │ │ │ │ -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 } │ │ │ │ │ +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_; } │ │ │ │ │ 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); │ │ │ │ │ -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(); │ │ │ │ │ +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 // 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); │ │ │ │ │ +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; │ │ │ │ │ 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 } │ │ │ │ │ +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 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 } │ │ │ │ │ +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 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; │ │ │ │ │ +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); │ │ │ │ │ 273 } │ │ │ │ │ -274 } │ │ │ │ │ +274#endif │ │ │ │ │ 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 } │ │ │ │ │ +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 } │ │ │ │ │ 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 } │ │ │ │ │ -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); │ │ │ │ │ +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 } 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; │ │ │ │ │ +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 // 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); │ │ │ │ │ +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 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 } │ │ │ │ │ +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 // 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. │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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... │ │ │ │ │ -_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 │ │ │ │ │ + * 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/a01142_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │┄ Files 24% 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/nonlinear/GncParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    ISAM2.h
    │ │ │ │ +
    GncParams.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    29
    │ │ │ │ -
    30#include <vector>
    │ │ │ │ +
    27#pragma once
    │ │ │ │ +
    28
    │ │ │ │ + │ │ │ │ + │ │ │ │
    31
    │ │ │ │
    32namespace gtsam {
    │ │ │ │
    33
    │ │ │ │ -
    │ │ │ │ -
    45class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    │ │ │ │ -
    46 protected:
    │ │ │ │ - │ │ │ │ -
    49
    │ │ │ │ - │ │ │ │ -
    53
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    82
    │ │ │ │ - │ │ │ │ -
    85
    │ │ │ │ - │ │ │ │ +
    34/* ************************************************************************* */
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    37 GM /*Geman McClure*/,
    │ │ │ │ +
    38 TLS /*Truncated least squares*/
    │ │ │ │ +
    39};
    │ │ │ │ +
    │ │ │ │ +
    40
    │ │ │ │ +
    41template<class BaseOptimizerParameters>
    │ │ │ │ +
    │ │ │ │ +
    42class GncParams {
    │ │ │ │ +
    43 public:
    │ │ │ │ +
    45 typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
    │ │ │ │ +
    46
    │ │ │ │ +
    │ │ │ │ +
    48 enum Verbosity {
    │ │ │ │ +
    49 SILENT = 0,
    │ │ │ │ +
    50 SUMMARY,
    │ │ │ │ +
    51 MU,
    │ │ │ │ +
    52 WEIGHTS,
    │ │ │ │ +
    53 VALUES
    │ │ │ │ +
    54 };
    │ │ │ │ +
    │ │ │ │ +
    55
    │ │ │ │ +
    │ │ │ │ +
    57 GncParams(const BaseOptimizerParameters& baseOptimizerParams)
    │ │ │ │ + │ │ │ │ +
    59 }
    │ │ │ │ +
    │ │ │ │ +
    60
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    64 }
    │ │ │ │ +
    │ │ │ │ +
    65
    │ │ │ │ +
    67 BaseOptimizerParameters baseOptimizerParams;
    │ │ │ │ + │ │ │ │ +
    70 size_t maxIterations = 100;
    │ │ │ │ +
    71 double muStep = 1.4;
    │ │ │ │ +
    72 double relativeCostTol = 1e-5;
    │ │ │ │ +
    73 double weightsTol = 1e-4;
    │ │ │ │ + │ │ │ │ +
    75
    │ │ │ │ +
    76 //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    82 IndexVector knownOutliers = IndexVector();
    │ │ │ │ +
    83
    │ │ │ │ +
    │ │ │ │ +
    85 void setLossType(const GncLossType type) {
    │ │ │ │ +
    86 lossType = type;
    │ │ │ │ +
    87 }
    │ │ │ │ +
    │ │ │ │
    88
    │ │ │ │ -
    90 mutable boost::optional<double> doglegDelta_;
    │ │ │ │ -
    91
    │ │ │ │ - │ │ │ │ -
    95
    │ │ │ │ - │ │ │ │ -
    98
    │ │ │ │ -
    99 public:
    │ │ │ │ -
    100 using This = ISAM2;
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │ -
    Parameters for iSAM 2.
    │ │ │ │ -
    Specialized iSAM2 Clique.
    │ │ │ │ -
    Class that stores extra params for ISAM2::update()
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ -
    Class that stores detailed iSAM2 result.
    │ │ │ │ +
    │ │ │ │ +
    90 void setMaxIterations(const size_t maxIter) {
    │ │ │ │ +
    91 std::cout
    │ │ │ │ +
    92 << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
    │ │ │ │ +
    93 << std::endl;
    │ │ │ │ +
    94 maxIterations = maxIter;
    │ │ │ │ +
    95 }
    │ │ │ │ +
    │ │ │ │ +
    96
    │ │ │ │ +
    │ │ │ │ +
    98 void setMuStep(const double step) {
    │ │ │ │ +
    99 muStep = step;
    │ │ │ │ +
    100 }
    │ │ │ │ +
    │ │ │ │ +
    101
    │ │ │ │ +
    │ │ │ │ +
    103 void setRelativeCostTol(double value) {
    │ │ │ │ +
    104 relativeCostTol = value;
    │ │ │ │ +
    105 }
    │ │ │ │ +
    │ │ │ │ +
    106
    │ │ │ │ +
    │ │ │ │ +
    108 void setWeightsTol(double value) {
    │ │ │ │ +
    109 weightsTol = value;
    │ │ │ │ +
    110 }
    │ │ │ │ +
    │ │ │ │ +
    111
    │ │ │ │ +
    │ │ │ │ +
    113 void setVerbosityGNC(const Verbosity value) {
    │ │ │ │ +
    114 verbosity = value;
    │ │ │ │ +
    115 }
    │ │ │ │ +
    │ │ │ │ +
    116
    │ │ │ │ +
    │ │ │ │ +
    123 void setKnownInliers(const IndexVector& knownIn) {
    │ │ │ │ +
    124 for (size_t i = 0; i < knownIn.size(); i++){
    │ │ │ │ +
    125 knownInliers.push_back(knownIn[i]);
    │ │ │ │ +
    126 }
    │ │ │ │ +
    127 std::sort(knownInliers.begin(), knownInliers.end());
    │ │ │ │ +
    128 }
    │ │ │ │ +
    │ │ │ │ +
    129
    │ │ │ │ +
    │ │ │ │ +
    134 void setKnownOutliers(const IndexVector& knownOut) {
    │ │ │ │ +
    135 for (size_t i = 0; i < knownOut.size(); i++){
    │ │ │ │ +
    136 knownOutliers.push_back(knownOut[i]);
    │ │ │ │ +
    137 }
    │ │ │ │ +
    138 std::sort(knownOutliers.begin(), knownOutliers.end());
    │ │ │ │ +
    139 }
    │ │ │ │ +
    │ │ │ │ +
    140
    │ │ │ │ +
    │ │ │ │ +
    142 bool equals(const GncParams& other, double tol = 1e-9) const {
    │ │ │ │ +
    143 return baseOptimizerParams.equals(other.baseOptimizerParams)
    │ │ │ │ +
    144 && lossType == other.lossType && maxIterations == other.maxIterations
    │ │ │ │ +
    145 && std::fabs(muStep - other.muStep) <= tol
    │ │ │ │ +
    146 && verbosity == other.verbosity && knownInliers == other.knownInliers
    │ │ │ │ +
    147 && knownOutliers == other.knownOutliers;
    │ │ │ │ +
    148 }
    │ │ │ │ +
    │ │ │ │ +
    149
    │ │ │ │ +
    │ │ │ │ +
    151 void print(const std::string& str) const {
    │ │ │ │ +
    152 std::cout << str << "\n";
    │ │ │ │ +
    153 switch (lossType) {
    │ │ │ │ +
    154 case GM:
    │ │ │ │ +
    155 std::cout << "lossType: Geman McClure" << "\n";
    │ │ │ │ +
    156 break;
    │ │ │ │ +
    157 case TLS:
    │ │ │ │ +
    158 std::cout << "lossType: Truncated Least-squares" << "\n";
    │ │ │ │ +
    159 break;
    │ │ │ │ +
    160 default:
    │ │ │ │ +
    161 throw std::runtime_error("GncParams::print: unknown loss type.");
    │ │ │ │ +
    162 }
    │ │ │ │ +
    163 std::cout << "maxIterations: " << maxIterations << "\n";
    │ │ │ │ +
    164 std::cout << "muStep: " << muStep << "\n";
    │ │ │ │ +
    165 std::cout << "relativeCostTol: " << relativeCostTol << "\n";
    │ │ │ │ +
    166 std::cout << "weightsTol: " << weightsTol << "\n";
    │ │ │ │ +
    167 std::cout << "verbosity: " << verbosity << "\n";
    │ │ │ │ +
    168 for (size_t i = 0; i < knownInliers.size(); i++)
    │ │ │ │ +
    169 std::cout << "knownInliers: " << knownInliers[i] << "\n";
    │ │ │ │ +
    170 for (size_t i = 0; i < knownOutliers.size(); i++)
    │ │ │ │ +
    171 std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
    │ │ │ │ +
    172 baseOptimizerParams.print("Base optimizer params: ");
    │ │ │ │ +
    173 }
    │ │ │ │ +
    │ │ │ │ +
    174};
    │ │ │ │ +
    │ │ │ │ +
    175
    │ │ │ │ +
    176}
    │ │ │ │ + │ │ │ │ +
    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
    │ │ │ │ +
    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
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< FactorIndex > FactorIndices
    Define collection types:
    Definition Factor.h:34
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
    Definition FastList.h:40
    │ │ │ │ -
    FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
    Definition FastMap.h:38
    │ │ │ │ - │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ -
    Bayes tree.
    Definition BayesTree.h:67
    │ │ │ │ -
    boost::shared_ptr< Clique > sharedClique
    Shared pointer to a clique.
    Definition BayesTree.h:74
    │ │ │ │ -
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
    Definition ISAM2.h:45
    │ │ │ │ -
    KeySet fixedVariables_
    Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
    Definition ISAM2.h:94
    │ │ │ │ -
    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
    │ │ │ │ -
    int update_count_
    Counter incremented every update(), used to determine periodic relinearization.
    Definition ISAM2.h:96
    │ │ │ │ -
    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
    │ │ │ │ -
    virtual ~ISAM2()
    default virtual destructor
    Definition ISAM2.h:114
    │ │ │ │ -
    const KeySet & getFixedVariables() const
    Access the nonlinear variable index.
    Definition ISAM2.h:265
    │ │ │ │ -
    Base::Cliques Cliques
    List of Cliques.
    Definition ISAM2.h:104
    │ │ │ │ -
    void printStats() const
    prints out clique statistics
    Definition ISAM2.h:270
    │ │ │ │ -
    NonlinearFactorGraph nonlinearFactors_
    All original nonlinear factors are stored here to use during relinearization.
    Definition ISAM2.h:81
    │ │ │ │ -
    const VariableIndex & getVariableIndex() const
    Access the nonlinear variable index.
    Definition ISAM2.h:262
    │ │ │ │ -
    Base::sharedClique sharedClique
    Shared pointer to a clique.
    Definition ISAM2.h:103
    │ │ │ │ -
    VectorValues delta_
    The linear delta from the last linear solution, an update to the estimate in theta.
    Definition ISAM2.h:61
    │ │ │ │ -
    VariableIndex variableIndex_
    VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
    Definition ISAM2.h:52
    │ │ │ │ -
    const Values & getLinearizationPoint() const
    Access the current linearization point.
    Definition ISAM2.h:205
    │ │ │ │ -
    Values theta_
    The current linearization point.
    Definition ISAM2.h:48
    │ │ │ │ -
    ISAM2Params params_
    The current parameters.
    Definition ISAM2.h:87
    │ │ │ │ -
    boost::optional< double > doglegDelta_
    The current Dogleg Delta (trust region radius)
    Definition ISAM2.h:90
    │ │ │ │ -
    GaussianFactorGraph linearFactors_
    The current linear factors, which are only updated as needed.
    Definition ISAM2.h:84
    │ │ │ │ -
    const NonlinearFactorGraph & getFactorsUnsafe() const
    Access the set of nonlinear factors.
    Definition ISAM2.h:257
    │ │ │ │ -
    bool valueExists(Key key) const
    Check whether variable with given key exists in linearization point.
    Definition ISAM2.h:208
    │ │ │ │ -
    Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
    Definition ISAM2Clique.h:37
    │ │ │ │ -
    Definition ISAM2Params.h:135
    │ │ │ │ -
    This struct is returned from ISAM2::update() and contains information about the update that is useful...
    Definition ISAM2Result.h:41
    │ │ │ │ -
    A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
    Definition ISAM2Result.h:117
    │ │ │ │ -
    This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
    Definition ISAM2UpdateParams.h:32
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    const ValueType at(Key j) const
    Retrieve a variable by key j.
    Definition Values-inl.h:361
    │ │ │ │ -
    bool exists(Key j) const
    Check if a value exists with key j.
    Definition Values.cpp:94
    │ │ │ │ +
    GncLossType
    Choice of robust loss function for GNC.
    Definition GncParams.h:36
    │ │ │ │ +
    Definition GncParams.h:42
    │ │ │ │ +
    BaseOptimizerParameters baseOptimizerParams
    GNC parameters.
    Definition GncParams.h:67
    │ │ │ │ +
    IndexVector knownInliers
    Slots in the factor graph corresponding to measurements that we know are outliers.
    Definition GncParams.h:80
    │ │ │ │ +
    GncParams()
    Default constructor.
    Definition GncParams.h:62
    │ │ │ │ +
    BaseOptimizerParameters::OptimizerType OptimizerType
    For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
    Definition GncParams.h:45
    │ │ │ │ +
    void setKnownInliers(const IndexVector &knownIn)
    (Optional) Provide a vector of measurements that must be considered inliers.
    Definition GncParams.h:123
    │ │ │ │ +
    void print(const std::string &str) const
    Print.
    Definition GncParams.h:151
    │ │ │ │ +
    FastVector< uint64_t > IndexVector
    Slots in the factor graph corresponding to measurements that we know are inliers.
    Definition GncParams.h:79
    │ │ │ │ +
    GncParams(const BaseOptimizerParameters &baseOptimizerParams)
    Constructor.
    Definition GncParams.h:57
    │ │ │ │ +
    double muStep
    Multiplicative factor to reduce/increase the mu in gnc.
    Definition GncParams.h:71
    │ │ │ │ +
    double weightsTol
    If the weights are within weightsTol from being binary, stop iterating (only for TLS)
    Definition GncParams.h:73
    │ │ │ │ +
    void setKnownOutliers(const IndexVector &knownOut)
    (Optional) Provide a vector of measurements that must be considered outliers.
    Definition GncParams.h:134
    │ │ │ │ +
    Verbosity verbosity
    Verbosity level.
    Definition GncParams.h:74
    │ │ │ │ +
    bool equals(const GncParams &other, double tol=1e-9) const
    Equals.
    Definition GncParams.h:142
    │ │ │ │ +
    void setRelativeCostTol(double value)
    Set the maximum relative difference in mu values to stop iterating.
    Definition GncParams.h:103
    │ │ │ │ +
    GncLossType lossType
    any other specific GNC parameters:
    Definition GncParams.h:69
    │ │ │ │ +
    size_t maxIterations
    Maximum number of iterations.
    Definition GncParams.h:70
    │ │ │ │ +
    void setLossType(const GncLossType type)
    Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
    Definition GncParams.h:85
    │ │ │ │ +
    void setMuStep(const double step)
    Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
    Definition GncParams.h:98
    │ │ │ │ +
    double relativeCostTol
    If relative cost change is below this threshold, stop iterating.
    Definition GncParams.h:72
    │ │ │ │ +
    void setVerbosityGNC(const Verbosity value)
    Set the verbosity level.
    Definition GncParams.h:113
    │ │ │ │ +
    void setMaxIterations(const size_t maxIter)
    Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate...
    Definition GncParams.h:90
    │ │ │ │ +
    Verbosity
    Verbosity levels.
    Definition GncParams.h:48
    │ │ │ │ +
    void setWeightsTol(double value)
    Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
    Definition GncParams.h:108
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,373 +1,265 @@ │ │ │ │ │ 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_. │ │ │ │ │ +GncParams.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 │ │ │ │ │ +27#pragma once │ │ │ │ │ +28 │ │ │ │ │ +29#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> │ │ │ │ │ +30#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> │ │ │ │ │ 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__; │ │ │ │ │ +34/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_3_6enum _G_n_c_L_o_s_s_T_y_p_e { │ │ │ │ │ +37 GM /*Geman McClure*/, │ │ │ │ │ +38 TLS /*Truncated least squares*/ │ │ │ │ │ +39}; │ │ │ │ │ +40 │ │ │ │ │ +41template │ │ │ │ │ +_4_2class _G_n_c_P_a_r_a_m_s { │ │ │ │ │ +43 public: │ │ │ │ │ +_4_5 typedef typename BaseOptimizerParameters::OptimizerType _O_p_t_i_m_i_z_e_r_T_y_p_e; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 enum _V_e_r_b_o_s_i_t_y { │ │ │ │ │ +49 SILENT = 0, │ │ │ │ │ +50 SUMMARY, │ │ │ │ │ +51 MU, │ │ │ │ │ +52 WEIGHTS, │ │ │ │ │ +53 VALUES │ │ │ │ │ +54 }; │ │ │ │ │ +55 │ │ │ │ │ +_5_7 _G_n_c_P_a_r_a_m_s(const BaseOptimizerParameters& _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) │ │ │ │ │ +58 : _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s(_b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) { │ │ │ │ │ +59 } │ │ │ │ │ +60 │ │ │ │ │ +_6_2 _G_n_c_P_a_r_a_m_s() │ │ │ │ │ +63 : _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() { │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 BaseOptimizerParameters _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s; │ │ │ │ │ +_6_9 _G_n_c_L_o_s_s_T_y_p_e _l_o_s_s_T_y_p_e = TLS; │ │ │ │ │ +_7_0 size_t _m_a_x_I_t_e_r_a_t_i_o_n_s = 100; │ │ │ │ │ +_7_1 double _m_u_S_t_e_p = 1.4; │ │ │ │ │ +_7_2 double _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l = 1e-5; │ │ │ │ │ +_7_3 double _w_e_i_g_h_t_s_T_o_l = 1e-4; │ │ │ │ │ +_7_4 _V_e_r_b_o_s_i_t_y _v_e_r_b_o_s_i_t_y = SILENT; │ │ │ │ │ +75 │ │ │ │ │ +76 //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is │ │ │ │ │ +globally enabled. │ │ │ │ │ +_7_8 using _I_n_d_e_x_V_e_c_t_o_r = _F_a_s_t_V_e_c_t_o_r_<_u_i_n_t_6_4___t_>; │ │ │ │ │ +_8_0 _I_n_d_e_x_V_e_c_t_o_r _k_n_o_w_n_I_n_l_i_e_r_s = _I_n_d_e_x_V_e_c_t_o_r(); │ │ │ │ │ +82 _I_n_d_e_x_V_e_c_t_o_r knownOutliers = _I_n_d_e_x_V_e_c_t_o_r(); │ │ │ │ │ +83 │ │ │ │ │ +_8_5 void _s_e_t_L_o_s_s_T_y_p_e(const _G_n_c_L_o_s_s_T_y_p_e type) { │ │ │ │ │ +86 _l_o_s_s_T_y_p_e = type; │ │ │ │ │ +87 } │ │ │ │ │ 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_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. │ │ │ │ │ +_9_0 void _s_e_t_M_a_x_I_t_e_r_a_t_i_o_n_s(const size_t maxIter) { │ │ │ │ │ +91 std::cout │ │ │ │ │ +92 << "setMaxIterations: changing the max nr of iters might lead to less │ │ │ │ │ +accurate solutions and is not recommended! " │ │ │ │ │ +93 << std::endl; │ │ │ │ │ +94 _m_a_x_I_t_e_r_a_t_i_o_n_s = maxIter; │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_9_8 void _s_e_t_M_u_S_t_e_p(const double step) { │ │ │ │ │ +99 _m_u_S_t_e_p = step; │ │ │ │ │ +100 } │ │ │ │ │ +101 │ │ │ │ │ +_1_0_3 void _s_e_t_R_e_l_a_t_i_v_e_C_o_s_t_T_o_l(double value) { │ │ │ │ │ +104 _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l = value; │ │ │ │ │ +105 } │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 void _s_e_t_W_e_i_g_h_t_s_T_o_l(double value) { │ │ │ │ │ +109 _w_e_i_g_h_t_s_T_o_l = value; │ │ │ │ │ +110 } │ │ │ │ │ +111 │ │ │ │ │ +_1_1_3 void _s_e_t_V_e_r_b_o_s_i_t_y_G_N_C(const _V_e_r_b_o_s_i_t_y value) { │ │ │ │ │ +114 _v_e_r_b_o_s_i_t_y = value; │ │ │ │ │ +115 } │ │ │ │ │ +116 │ │ │ │ │ +_1_2_3 void _s_e_t_K_n_o_w_n_I_n_l_i_e_r_s(const _I_n_d_e_x_V_e_c_t_o_r& knownIn) { │ │ │ │ │ +124 for (size_t i = 0; i < knownIn.size(); i++){ │ │ │ │ │ +125 _k_n_o_w_n_I_n_l_i_e_r_s.push_back(knownIn[i]); │ │ │ │ │ +126 } │ │ │ │ │ +127 std::sort(_k_n_o_w_n_I_n_l_i_e_r_s.begin(), _k_n_o_w_n_I_n_l_i_e_r_s.end()); │ │ │ │ │ +128 } │ │ │ │ │ +129 │ │ │ │ │ +_1_3_4 void _s_e_t_K_n_o_w_n_O_u_t_l_i_e_r_s(const _I_n_d_e_x_V_e_c_t_o_r& knownOut) { │ │ │ │ │ +135 for (size_t i = 0; i < knownOut.size(); i++){ │ │ │ │ │ +136 knownOutliers.push_back(knownOut[i]); │ │ │ │ │ +137 } │ │ │ │ │ +138 std::sort(knownOutliers.begin(), knownOutliers.end()); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 bool _e_q_u_a_l_s(const _G_n_c_P_a_r_a_m_s& other, double tol = 1e-9) const { │ │ │ │ │ +143 return _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s.equals(other._b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s) │ │ │ │ │ +144 && _l_o_s_s_T_y_p_e == other._l_o_s_s_T_y_p_e && _m_a_x_I_t_e_r_a_t_i_o_n_s == other._m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +145 && std::fabs(_m_u_S_t_e_p - other._m_u_S_t_e_p) <= tol │ │ │ │ │ +146 && _v_e_r_b_o_s_i_t_y == other._v_e_r_b_o_s_i_t_y && _k_n_o_w_n_I_n_l_i_e_r_s == other._k_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ +147 && knownOutliers == other.knownOutliers; │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 void _p_r_i_n_t(const std::string& str) const { │ │ │ │ │ +152 std::cout << str << "\n"; │ │ │ │ │ +153 switch (_l_o_s_s_T_y_p_e) { │ │ │ │ │ +154 case GM: │ │ │ │ │ +155 std::cout << "lossType: Geman McClure" << "\n"; │ │ │ │ │ +156 break; │ │ │ │ │ +157 case TLS: │ │ │ │ │ +158 std::cout << "lossType: Truncated Least-squares" << "\n"; │ │ │ │ │ +159 break; │ │ │ │ │ +160 default: │ │ │ │ │ +161 throw std::runtime_error("GncParams::print: unknown loss type."); │ │ │ │ │ +162 } │ │ │ │ │ +163 std::cout << "maxIterations: " << _m_a_x_I_t_e_r_a_t_i_o_n_s << "\n"; │ │ │ │ │ +164 std::cout << "muStep: " << _m_u_S_t_e_p << "\n"; │ │ │ │ │ +165 std::cout << "relativeCostTol: " << _r_e_l_a_t_i_v_e_C_o_s_t_T_o_l << "\n"; │ │ │ │ │ +166 std::cout << "weightsTol: " << _w_e_i_g_h_t_s_T_o_l << "\n"; │ │ │ │ │ +167 std::cout << "verbosity: " << _v_e_r_b_o_s_i_t_y << "\n"; │ │ │ │ │ +168 for (size_t i = 0; i < _k_n_o_w_n_I_n_l_i_e_r_s.size(); i++) │ │ │ │ │ +169 std::cout << "knownInliers: " << _k_n_o_w_n_I_n_l_i_e_r_s[i] << "\n"; │ │ │ │ │ +170 for (size_t i = 0; i < knownOutliers.size(); i++) │ │ │ │ │ +171 std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; │ │ │ │ │ +172 _b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s.print("Base optimizer params: "); │ │ │ │ │ +173 } │ │ │ │ │ +174}; │ │ │ │ │ +175 │ │ │ │ │ +176} │ │ │ │ │ +_G_a_u_s_s_N_e_w_t_o_n_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 │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +_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 │ │ │ │ │ -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_:_:_G_n_c_L_o_s_s_T_y_p_e │ │ │ │ │ +GncLossType │ │ │ │ │ +Choice of robust loss function for GNC. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_b_a_s_e_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +BaseOptimizerParameters baseOptimizerParams │ │ │ │ │ +GNC parameters. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_k_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ +IndexVector knownInliers │ │ │ │ │ +Slots in the factor graph corresponding to measurements that we know are │ │ │ │ │ +outliers. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ +GncParams() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_O_p_t_i_m_i_z_e_r_T_y_p_e │ │ │ │ │ +BaseOptimizerParameters::OptimizerType OptimizerType │ │ │ │ │ +For each parameter, specify the corresponding optimizer: e.g., │ │ │ │ │ +GaussNewtonParams -> GaussNewtonOptimi... │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_K_n_o_w_n_I_n_l_i_e_r_s │ │ │ │ │ +void setKnownInliers(const IndexVector &knownIn) │ │ │ │ │ +(Optional) Provide a vector of measurements that must be considered inliers. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str) const │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_I_n_d_e_x_V_e_c_t_o_r │ │ │ │ │ +FastVector< uint64_t > IndexVector │ │ │ │ │ +Slots in the factor graph corresponding to measurements that we know are │ │ │ │ │ +inliers. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_G_n_c_P_a_r_a_m_s │ │ │ │ │ +GncParams(const BaseOptimizerParameters &baseOptimizerParams) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_m_u_S_t_e_p │ │ │ │ │ +double muStep │ │ │ │ │ +Multiplicative factor to reduce/increase the mu in gnc. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_w_e_i_g_h_t_s_T_o_l │ │ │ │ │ +double weightsTol │ │ │ │ │ +If the weights are within weightsTol from being binary, stop iterating (only │ │ │ │ │ +for TLS) │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_K_n_o_w_n_O_u_t_l_i_e_r_s │ │ │ │ │ +void setKnownOutliers(const IndexVector &knownOut) │ │ │ │ │ +(Optional) Provide a vector of measurements that must be considered outliers. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y │ │ │ │ │ +Verbosity verbosity │ │ │ │ │ +Verbosity level. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const GncParams &other, double tol=1e-9) const │ │ │ │ │ +Equals. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_R_e_l_a_t_i_v_e_C_o_s_t_T_o_l │ │ │ │ │ +void setRelativeCostTol(double value) │ │ │ │ │ +Set the maximum relative difference in mu values to stop iterating. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_l_o_s_s_T_y_p_e │ │ │ │ │ +GncLossType lossType │ │ │ │ │ +any other specific GNC parameters: │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +size_t maxIterations │ │ │ │ │ +Maximum number of iterations. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_L_o_s_s_T_y_p_e │ │ │ │ │ +void setLossType(const GncLossType type) │ │ │ │ │ +Set the robust loss function to be used in GNC (chosen among the ones in │ │ │ │ │ +GncLossType). │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_M_u_S_t_e_p │ │ │ │ │ +void setMuStep(const double step) │ │ │ │ │ +Set the graduated non-convexity step: at each GNC iteration, mu is updated as │ │ │ │ │ +mu <- mu * muStep. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_C_o_s_t_T_o_l │ │ │ │ │ +double relativeCostTol │ │ │ │ │ +If relative cost change is below this threshold, stop iterating. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_V_e_r_b_o_s_i_t_y_G_N_C │ │ │ │ │ +void setVerbosityGNC(const Verbosity value) │ │ │ │ │ +Set the verbosity level. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_M_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +void setMaxIterations(const size_t maxIter) │ │ │ │ │ +Set the maximum number of iterations in GNC (changing the max nr of iters might │ │ │ │ │ +lead to less accurate... │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ +Verbosity │ │ │ │ │ +Verbosity levels. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_P_a_r_a_m_s_:_:_s_e_t_W_e_i_g_h_t_s_T_o_l │ │ │ │ │ +void setWeightsTol(double value) │ │ │ │ │ +Set the maximum difference between the weights and their rounding in {0,1} to │ │ │ │ │ +stop iterating. │ │ │ │ │ +DDeeffiinniittiioonn GncParams.h:108 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_._h │ │ │ │ │ + * GGnnccPPaarraammss..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/a01145.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00389.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/geometry/SOn-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces
    │ │ │ │ -
    NonlinearISAM.cpp File Reference
    │ │ │ │ +
    SOn-inl.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ + │ │ │ │ +

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

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Namespaces

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

    Detailed Description

    │ │ │ │ -
    Date
    Jan 19, 2010
    │ │ │ │ -
    Author
    Viorela Ila and Richard Roberts
    │ │ │ │ +

    Template implementations for SO(n)

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    March 2019
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearISAM.cpp File Reference │ │ │ │ │ +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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 19, 2010 │ │ │ │ │ +Template implementations for SO(n) │ │ │ │ │ Author │ │ │ │ │ - Viorela Ila and Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ * _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 │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a01148.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00350.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/geometry/PinholeCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    ExpressionFactorGraph.h File Reference
    │ │ │ │ +
    PinholeCamera.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Factor graph that supports adding ExpressionFactors directly. │ │ │ │ +

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

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::ExpressionFactorGraph
     Factor graph that supports adding ExpressionFactors directly. More...
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Factor graph that supports adding ExpressionFactors directly.

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    Date
    December 2014
    │ │ │ │ +

    Base class for all pinhole cameras.

    │ │ │ │ +
    Author
    Yong-Dian Jian
    │ │ │ │ +
    Date
    Jan 27, 2012
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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 │ │ │ │ │ -ExpressionFactorGraph.h File Reference │ │ │ │ │ -Factor graph that supports adding ExpressionFactors directly. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_:_:_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_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ Date │ │ │ │ │ - December 2014 │ │ │ │ │ + Jan 27, 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_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a01154_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01001_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/ExpressionFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    expressionTesting.h
    │ │ │ │ +
    ExpressionFactorGraph.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/nonlinear/ExpressionFactor.h>
    │ │ │ │ - │ │ │ │ -
    24#include <gtsam/base/Testable.h>
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ +
    21#include <gtsam/nonlinear/ExpressionFactor.h>
    │ │ │ │ + │ │ │ │ +
    23
    │ │ │ │ +
    24namespace gtsam {
    │ │ │ │
    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)); }
    │ │ │ │ -
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    Evaluate derivatives of a nonlinear factor numerically.
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    Factor Graph consisting of non-linear factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ +
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ +
    IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
    Add a factor directly using a shared_ptr.
    Definition FactorGraph.h:186
    │ │ │ │ +
    Factor that supports arbitrary expressions via AD.
    Definition ExpressionFactor.h:44
    │ │ │ │
    Expression class that supports automatic differentiation.
    Definition Expression.h:48
    │ │ │ │ -
    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
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    Factor graph that supports adding ExpressionFactors directly.
    Definition ExpressionFactorGraph.h:29
    │ │ │ │ +
    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
    │ │ │ │ +
    Definition NonlinearFactorGraph.h:55
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,74 +1,76 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -expressionTesting.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#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> │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +_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} │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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_T_e_s_t_i_n_g_._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/a01160.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01097.html │ │ │ │┄ Files 0% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    NonlinearConjugateGradientOptimizer.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │

    Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::NonlinearConjugateGradientOptimizer
     An implementation of the nonlinear CG method using the template below. More...
     
    │ │ │ │ @@ -139,13 +139,13 @@ │ │ │ │
    Author
    Yong-Dian Jian
    │ │ │ │
    Date
    June 11, 2012
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01160_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00428_source.html │ │ │ │┄ Files 25% 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/geometry/Quaternion.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    NonlinearConjugateGradientOptimizer.h
    │ │ │ │ +
    Quaternion.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>
    │ │ │ │ - │ │ │ │ -
    23#include <boost/tuple/tuple.hpp>
    │ │ │ │ -
    24
    │ │ │ │ -
    25namespace gtsam {
    │ │ │ │ -
    26
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ +
    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>
    │ │ │ │ +
    25
    │ │ │ │ +
    26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
    │ │ │ │ +
    27
    │ │ │ │ +
    28namespace gtsam {
    │ │ │ │
    29
    │ │ │ │ -
    30 /* a class for the nonlinearConjugateGradient template */
    │ │ │ │ -
    31 class System {
    │ │ │ │ -
    32 public:
    │ │ │ │ -
    33 typedef Values State;
    │ │ │ │ -
    34 typedef VectorValues Gradient;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    67 const Values& initialValues, const Parameters& params = Parameters());
    │ │ │ │ -
    68
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ -
    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;
    │ │ │ │ +
    30// Define traits
    │ │ │ │ +
    31template<typename _Scalar, int _Options>
    │ │ │ │ +
    │ │ │ │ +
    32struct traits<QUATERNION_TYPE> {
    │ │ │ │ +
    33 typedef QUATERNION_TYPE ManifoldType;
    │ │ │ │ +
    34 typedef QUATERNION_TYPE Q;
    │ │ │ │ +
    35
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ +
    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 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);
    │ │ │ │ +
    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
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    Base class and parameters for nonlinear optimization algorithms.
    │ │ │ │ +
    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
    │ │ │ │ +
    Base class and basic functions for Lie types.
    │ │ │ │ +
    3*3 matrix representation of SO(3)
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    double lineSearch(const S &system, const V currentValues, const W &gradient)
    Implement the golden-section line search algorithm.
    Definition NonlinearConjugateGradientOptimizer.h:88
    │ │ │ │ -
    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
    │ │ │ │ -
    Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
    Optimize for triangulation.
    Definition triangulation.cpp:155
    │ │ │ │ -
    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
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    VectorValues represents a collection of vector-valued variables associated each with a unique integer...
    Definition VectorValues.h:74
    │ │ │ │ -
    An implementation of the nonlinear CG method using the template below.
    Definition NonlinearConjugateGradientOptimizer.h:28
    │ │ │ │ -
    ~NonlinearConjugateGradientOptimizer() override
    Destructor.
    Definition NonlinearConjugateGradientOptimizer.h:70
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
    Definition NonlinearOptimizer.h:75
    │ │ │ │ -
    The common parameters for Nonlinear optimizers.
    Definition NonlinearOptimizerParams.h:34
    │ │ │ │ -
    double absoluteErrorTol
    The maximum absolute error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:43
    │ │ │ │ -
    IterationHook iterationHook
    Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
    Definition NonlinearOptimizerParams.h:94
    │ │ │ │ -
    size_t maxIterations
    The maximum iterations to stop iterating (default 100)
    Definition NonlinearOptimizerParams.h:41
    │ │ │ │ -
    Verbosity verbosity
    The printing verbosity during optimization (default SILENT)
    Definition NonlinearOptimizerParams.h:45
    │ │ │ │ -
    double relativeErrorTol
    The maximum relative error decrease to stop iterating (default 1e-5)
    Definition NonlinearOptimizerParams.h:42
    │ │ │ │ -
    double errorTol
    The maximum total error to stop iterating (default 0.0)
    Definition NonlinearOptimizerParams.h:44
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    Group operator syntax flavors.
    Definition Group.h:37
    │ │ │ │ +
    tag to assert a type is a Lie group
    Definition Lie.h:164
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    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
    │ │ │ │ +
    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
    │ │ │ │ +
    static MatrixDD ExpmapDerivative(const TangentVector &omega)
    Derivative of Expmap, currently only defined for SO3.
    Definition SOn-inl.h:72
    │ │ │ │ +
    static MatrixDD LogmapDerivative(const TangentVector &omega)
    Derivative of Logmap, currently only defined for SO3.
    Definition SOn-inl.h:82
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,298 +1,214 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearConjugateGradientOptimizer.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 │ │ │ │ │ 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 { │ │ │ │ │ -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 { │ │ │ │ │ +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 │ │ │ │ │ +25 │ │ │ │ │ +26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ 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) { │ │ │ │ │ +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 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 // 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; │ │ │ │ │ +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 │ │ │ │ │ +_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 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); │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a01172_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00416_source.html │ │ │ │┄ Files 11% similar despite different names │ │ │ │ @@ -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/geometry/Cal3_S2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    LinearContainerFactor.h
    │ │ │ │ +
    Cal3_S2.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
    │ │ │ │ - │ │ │ │ -
    13
    │ │ │ │ -
    14namespace gtsam {
    │ │ │ │ -
    15
    │ │ │ │ -
    16 // Forward declarations
    │ │ │ │ -
    17 class JacobianFactor;
    │ │ │ │ -
    18 class HessianFactor;
    │ │ │ │ -
    19
    │ │ │ │ -
    │ │ │ │ -
    26class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor {
    │ │ │ │ -
    27protected:
    │ │ │ │ +
    22#pragma once
    │ │ │ │ +
    23
    │ │ │ │ +
    24#include <gtsam/geometry/Cal3.h>
    │ │ │ │ + │ │ │ │ +
    26
    │ │ │ │ +
    27namespace gtsam {
    │ │ │ │
    28
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    38
    │ │ │ │ -
    39public:
    │ │ │ │ +
    │ │ │ │ +
    34class GTSAM_EXPORT Cal3_S2 : public Cal3 {
    │ │ │ │ +
    35 public:
    │ │ │ │ +
    36 enum { dimension = 5 };
    │ │ │ │ +
    37
    │ │ │ │ +
    39 using shared_ptr = boost::shared_ptr<Cal3_S2>;
    │ │ │ │
    40
    │ │ │ │ -
    41 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    42
    │ │ │ │ - │ │ │ │ -
    45
    │ │ │ │ -
    47 LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
    │ │ │ │ -
    48
    │ │ │ │ -
    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
    │ │ │ │ -
    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;
    │ │ │ │ +
    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) {}
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    83 const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
    │ │ │ │ -
    84
    │ │ │ │ -
    101 GaussianFactor::shared_ptr linearize(const Values& c) const override;
    │ │ │ │ +
    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
    │ │ │ │ -
    106 GaussianFactor::shared_ptr negateToGaussian() const;
    │ │ │ │ -
    107
    │ │ │ │ -
    111 NonlinearFactor::shared_ptr negateToNonlinear() const;
    │ │ │ │ +
    │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    147 boost::shared_ptr<JacobianFactor> toJacobian() const;
    │ │ │ │ +
    146template <>
    │ │ │ │ +
    147struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
    │ │ │ │
    148
    │ │ │ │ -
    150 boost::shared_ptr<HessianFactor> toHessian() const;
    │ │ │ │ +
    149template <>
    │ │ │ │ +
    150struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
    │ │ │ │
    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
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    152} // \ namespace gtsam
    │ │ │ │ +
    Common code for all Calibration models.
    │ │ │ │ +
    2D Point
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactor.h:42
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    A Gaussian factor using the canonical parameters (information form)
    Definition HessianFactor.h:101
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    Dummy version of a generic linear factor to be injected into a nonlinear factor graph.
    Definition LinearContainerFactor.h:26
    │ │ │ │ -
    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
    │ │ │ │ -
    LinearContainerFactor()
    Default constructor - necessary for serialization.
    Definition LinearContainerFactor.h:44
    │ │ │ │ -
    const boost::optional< Values > & linearizationPoint() const
    Extract the linearization point used in recalculating error.
    Definition LinearContainerFactor.h:83
    │ │ │ │ -
    bool hasLinearizationPoint() const
    Casting syntactic sugar.
    Definition LinearContainerFactor.h:138
    │ │ │ │ -
    Nonlinear factor base class.
    Definition NonlinearFactor.h:42
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    In Gaussian factors, the error function returns either the negative log-likelihood,...
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    Both ManifoldTraits and Testable.
    Definition Manifold.h:120
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Common base class for all calibration models.
    Definition Cal3.h:69
    │ │ │ │ +
    Vector5 vector() const
    vectorized form (column-wise)
    Definition Cal3.h:160
    │ │ │ │ +
    double fy_
    focal length
    Definition Cal3.h:71
    │ │ │ │ +
    double s_
    skew
    Definition Cal3.h:72
    │ │ │ │ +
    double v0_
    principal point
    Definition Cal3.h:73
    │ │ │ │ +
    The most common 5DOF 3D->2D calibration.
    Definition Cal3_S2.h:34
    │ │ │ │ +
    Cal3_S2()=default
    Create a default calibration that leaves coordinates unchanged.
    │ │ │ │ +
    Cal3_S2(double fx, double fy, double s, double u0, double v0)
    constructor from doubles
    Definition Cal3_S2.h:48
    │ │ │ │ +
    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
    │ │ │ │ +
    static size_t Dim()
    return DOF, dimensionality of tangent space
    Definition Cal3_S2.h:118
    │ │ │ │ +
    Cal3_S2(const Vector5 &d)
    constructor from vector
    Definition Cal3_S2.h:52
    │ │ │ │ +
    Vector5 localCoordinates(const Cal3_S2 &T2) const
    Unretraction for the calibration.
    Definition Cal3_S2.h:126
    │ │ │ │ +
    Cal3_S2(double fov, int w, int h)
    Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect.
    Definition Cal3_S2.h:60
    │ │ │ │ +
    Cal3_S2 retract(const Vector &d) const
    Given 5-dim tangent vector, create new calibration.
    Definition Cal3_S2.h:121
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,185 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LinearContainerFactor.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 │ │ │ │ │ -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_/_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: │ │ │ │ │ +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 │ │ │ │ │ -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: │ │ │ │ │ +_3_4class GTSAM_EXPORT _C_a_l_3___S_2 : public _C_a_l_3 { │ │ │ │ │ +35 public: │ │ │ │ │ +36 enum { dimension = 5 }; │ │ │ │ │ +37 │ │ │ │ │ +39 using shared_ptr = boost::shared_ptr; │ │ │ │ │ 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 │ │ │ │ │ -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 │ │ │ │ │ -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; │ │ │ │ │ +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) {} │ │ │ │ │ +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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -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; │ │ │ │ │ +_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 │ │ │ │ │ -_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; │ │ │ │ │ +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 │ │ │ │ │ -147 boost::shared_ptr toJacobian() const; │ │ │ │ │ +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 │ │ │ │ │ -150 boost::shared_ptr toHessian() const; │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +152} // \ namespace gtsam │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ -_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_:_:_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_:_:_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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _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/a01187.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00782.html │ │ │ │┄ Files 13% similar despite different names │ │ │ │ @@ -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/linear/GaussianBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    TranslationRecovery.h File Reference
    │ │ │ │ +
    GaussianBayesTree.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Recovering translations in an epipolar graph when rotations are given. │ │ │ │ +

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ More...

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::TranslationRecovery
    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

    │ │ │ │ -

    Recovering translations in an epipolar graph when rotations are given.

    │ │ │ │ -
    Author
    Frank Dellaert, Akshay Krishnan
    │ │ │ │ -
    Date
    March 2020
    │ │ │ │ +

    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

    │ │ │ │ +

    GaussianBayesTree

    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── 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_._._. │ │ │ │ │ +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_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ + 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 ********** │ │ │ │ │ -Recovering translations in an epipolar graph when rotations are given. │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Akshay Krishnan │ │ │ │ │ - Date │ │ │ │ │ - March 2020 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01187_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00371_source.html │ │ │ │┄ Files 13% 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/geometry/Point3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    TranslationRecovery.h
    │ │ │ │ +
    Point3.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// \callgraph
    │ │ │ │ +
    21
    │ │ │ │ +
    22#pragma once
    │ │ │ │
    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.
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    52 public:
    │ │ │ │ -
    53 using KeyPair = std::pair<Key, Key>;
    │ │ │ │ -
    54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
    │ │ │ │ +
    24#include <gtsam/config.h>
    │ │ │ │ +
    25#include <gtsam/base/VectorSpace.h>
    │ │ │ │ +
    26#include <gtsam/base/Vector.h>
    │ │ │ │ +
    27#include <gtsam/dllexport.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;
    │ │ │ │ +
    38
    │ │ │ │ +
    39// Convenience typedef
    │ │ │ │ +
    40using Point3Pair = std::pair<Point3, Point3>;
    │ │ │ │ +
    41GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
    │ │ │ │ +
    42
    │ │ │ │ +
    43using Point3Pairs = std::vector<Point3Pair>;
    │ │ │ │ +
    44
    │ │ │ │ +
    46GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
    │ │ │ │ +
    47 OptionalJacobian<1, 3> H1 = boost::none,
    │ │ │ │ +
    48 OptionalJacobian<1, 3> H2 = boost::none);
    │ │ │ │ +
    49
    │ │ │ │ +
    51GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
    │ │ │ │ +
    52
    │ │ │ │ +
    54GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
    │ │ │ │
    55
    │ │ │ │ -
    56 private:
    │ │ │ │ -
    57 // Translation directions between camera pairs.
    │ │ │ │ -
    58 TranslationEdges relativeTranslations_;
    │ │ │ │ -
    59
    │ │ │ │ -
    60 // Parameters.
    │ │ │ │ - │ │ │ │ -
    62
    │ │ │ │ -
    63 public:
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    70 : lmParams_(lmParams) {}
    │ │ │ │ -
    │ │ │ │ -
    71
    │ │ │ │ - │ │ │ │ -
    76
    │ │ │ │ - │ │ │ │ -
    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,
    │ │ │ │ - │ │ │ │ -
    107 const SharedNoiseModel &priorNoiseModel =
    │ │ │ │ -
    108 noiseModel::Isotropic::Sigma(3, 0.01)) const;
    │ │ │ │ -
    109
    │ │ │ │ - │ │ │ │ -
    122 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
    │ │ │ │ -
    123 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
    │ │ │ │ -
    124 std::mt19937 *rng, const Values &initialValues = Values()) const;
    │ │ │ │ -
    125
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    │ │ │ │ -
    A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
    │ │ │ │ -
    Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
    │ │ │ │ +
    57GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
    │ │ │ │ +
    58 OptionalJacobian<3, 3> H_p = boost::none,
    │ │ │ │ +
    59 OptionalJacobian<3, 3> H_q = boost::none);
    │ │ │ │ +
    60
    │ │ │ │ +
    62GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
    │ │ │ │ +
    63 OptionalJacobian<1, 3> H_p = boost::none,
    │ │ │ │ +
    64 OptionalJacobian<1, 3> H_q = boost::none);
    │ │ │ │ +
    65
    │ │ │ │ +
    67template <class CONTAINER>
    │ │ │ │ +
    │ │ │ │ +
    68Point3 mean(const CONTAINER& points) {
    │ │ │ │ +
    69 if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
    │ │ │ │ +
    70 Point3 sum(0, 0, 0);
    │ │ │ │ +
    71 sum = std::accumulate(points.begin(), points.end(), sum);
    │ │ │ │ +
    72 return sum / points.size();
    │ │ │ │ +
    73}
    │ │ │ │ +
    │ │ │ │ +
    74
    │ │ │ │ +
    76GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
    │ │ │ │ +
    77
    │ │ │ │ +
    78template <typename A1, typename A2>
    │ │ │ │ +
    79struct Range;
    │ │ │ │ +
    80
    │ │ │ │ +
    81template <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    83 typedef double result_type;
    │ │ │ │ +
    84 double operator()(const Point3& p, const Point3& q,
    │ │ │ │ +
    85 OptionalJacobian<1, 3> H1 = boost::none,
    │ │ │ │ +
    86 OptionalJacobian<1, 3> H2 = boost::none) {
    │ │ │ │ +
    87 return distance3(p, q, H1, H2);
    │ │ │ │ +
    88 }
    │ │ │ │ +
    89};
    │ │ │ │ +
    │ │ │ │ +
    90
    │ │ │ │ +
    91} // namespace gtsam
    │ │ │ │ +
    92
    │ │ │ │ +
    serialization for Vectors
    │ │ │ │ +
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    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
    │ │ │ │ -
    Parameters for Levenberg-Marquardt optimization.
    Definition LevenbergMarquardtParams.h:35
    │ │ │ │ -
    Definition NonlinearFactorGraph.h:55
    │ │ │ │ -
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ -
    Definition BinaryMeasurement.h:36
    │ │ │ │ -
    Definition TranslationRecovery.h:51
    │ │ │ │ -
    NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
    Build the factor graph to do the optimization.
    Definition TranslationRecovery.cpp:99
    │ │ │ │ -
    static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
    Simulate translation direction measurements.
    Definition TranslationRecovery.cpp:216
    │ │ │ │ -
    TranslationRecovery()=default
    Default constructor.
    │ │ │ │ -
    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
    │ │ │ │ -
    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
    │ │ │ │ -
    TranslationRecovery(const LevenbergMarquardtParams &lmParams)
    Construct a new Translation Recovery object.
    Definition TranslationRecovery.h:69
    │ │ │ │ -
    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
    │ │ │ │ +
    Point3 mean(const CONTAINER &points)
    mean
    Definition Point3.h:68
    │ │ │ │ +
    Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
    cross product
    Definition Point3.cpp:64
    │ │ │ │ +
    Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
    Calculate the two means of a set of Point2 pairs.
    Definition Point2.cpp:116
    │ │ │ │ +
    double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
    distance between two points
    Definition Point3.cpp:27
    │ │ │ │ +
    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
    │ │ │ │ +
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ +
    double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
    Distance of the point from the origin, with Jacobian.
    Definition Point3.cpp:41
    │ │ │ │ +
    double dot(const V1 &a, const V2 &b)
    Dot product.
    Definition Vector.h:195
    │ │ │ │ +
    OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
    Definition OptionalJacobian.h:41
    │ │ │ │ +
    Definition BearingRange.h:40
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,177 +1,140 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TranslationRecovery.h │ │ │ │ │ +Point3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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> │ │ │ │ │ +20// \callgraph │ │ │ │ │ +21 │ │ │ │ │ +22#pragma once │ │ │ │ │ 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>; │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28#include <_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> │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_3_6typedef Vector3 _P_o_i_n_t_3; │ │ │ │ │ +37typedef std::vector > Point3Vector; │ │ │ │ │ +38 │ │ │ │ │ +39// Convenience typedef │ │ │ │ │ +40using Point3Pair = std::pair; │ │ │ │ │ +41GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam:: │ │ │ │ │ +Point3Pair &p); │ │ │ │ │ +42 │ │ │ │ │ +43using Point3Pairs = std::vector; │ │ │ │ │ +44 │ │ │ │ │ +46GTSAM_EXPORT double _d_i_s_t_a_n_c_e_3(const _P_o_i_n_t_3& p1, const _P_o_i_n_t_3& q, │ │ │ │ │ +47 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1 = boost::none, │ │ │ │ │ +48 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2 = boost::none); │ │ │ │ │ +49 │ │ │ │ │ +51GTSAM_EXPORT double _n_o_r_m_3(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); │ │ │ │ │ +52 │ │ │ │ │ +54GTSAM_EXPORT _P_o_i_n_t_3 _n_o_r_m_a_l_i_z_e(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); │ │ │ │ │ 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... │ │ │ │ │ +57GTSAM_EXPORT _P_o_i_n_t_3 _c_r_o_s_s(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ +58 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H_p = boost::none, │ │ │ │ │ +59 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H_q = boost::none); │ │ │ │ │ +60 │ │ │ │ │ +62GTSAM_EXPORT double _d_o_t(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ +63 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H_p = boost::none, │ │ │ │ │ +64 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H_q = boost::none); │ │ │ │ │ +65 │ │ │ │ │ +67template │ │ │ │ │ +_6_8_P_o_i_n_t_3 _m_e_a_n(const CONTAINER& points) { │ │ │ │ │ +69 if (points.size() == 0) throw std::invalid_argument("Point3::mean input │ │ │ │ │ +container is empty"); │ │ │ │ │ +70 _P_o_i_n_t_3 sum(0, 0, 0); │ │ │ │ │ +71 sum = std::accumulate(points.begin(), points.end(), sum); │ │ │ │ │ +72 return sum / points.size(); │ │ │ │ │ +73} │ │ │ │ │ +74 │ │ │ │ │ +76GTSAM_EXPORT Point3Pair _m_e_a_n_s(const std::vector &abPointPairs); │ │ │ │ │ +77 │ │ │ │ │ +78template │ │ │ │ │ +79struct Range; │ │ │ │ │ +80 │ │ │ │ │ +81template <> │ │ │ │ │ +_8_2struct _R_a_n_g_e<_P_o_i_n_t_3, _P_o_i_n_t_3> { │ │ │ │ │ +83 typedef double result_type; │ │ │ │ │ +84 double operator()(const _P_o_i_n_t_3& p, const _P_o_i_n_t_3& q, │ │ │ │ │ +85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1 = boost::none, │ │ │ │ │ +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 │ │ │ │ │ _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_:_:_m_e_a_n │ │ │ │ │ +Point3 mean(const CONTAINER &points) │ │ │ │ │ +mean │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:68 │ │ │ │ │ +_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_:_:_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_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_:_:_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_:_:_n_o_r_m_3 │ │ │ │ │ +double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H) │ │ │ │ │ +Distance of the point from the origin, with Jacobian. │ │ │ │ │ +DDeeffiinniittiioonn Point3.cpp:41 │ │ │ │ │ +_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_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 │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _P_o_i_n_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/a01190_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00500_source.html │ │ │ │┄ Files 4% similar despite different names │ │ │ │ @@ -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/hybrid/HybridNonlinearFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    TranslationFactor.h
    │ │ │ │ +
    HybridNonlinearFactorGraph.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
    │ │ │ │ -
    12#pragma once
    │ │ │ │ -
    13
    │ │ │ │ - │ │ │ │ -
    22#include <gtsam/geometry/Unit3.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    25
    │ │ │ │ -
    26namespace gtsam {
    │ │ │ │ -
    27
    │ │ │ │ -
    │ │ │ │ -
    42class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
    │ │ │ │ -
    43 private:
    │ │ │ │ - │ │ │ │ -
    45 Point3 measured_w_aZb_;
    │ │ │ │ -
    46
    │ │ │ │ -
    47 public:
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    22
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    25class HybridGaussianFactorGraph;
    │ │ │ │ +
    26
    │ │ │ │ +
    │ │ │ │ +
    33class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
    │ │ │ │ +
    34 protected:
    │ │ │ │ +
    35 public:
    │ │ │ │ +
    36 using Base = HybridFactorGraph;
    │ │ │ │ + │ │ │ │ +
    38 using shared_ptr = boost::shared_ptr<This>;
    │ │ │ │ +
    39
    │ │ │ │ + │ │ │ │ +
    41 using Indices = KeyVector;
    │ │ │ │ +
    42
    │ │ │ │ +
    45
    │ │ │ │ + │ │ │ │ +
    47
    │ │ │ │ +
    53 template <class DERIVEDFACTOR>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 <>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    84 : public Testable<HybridNonlinearFactorGraph> {};
    │ │ │ │
    │ │ │ │ +
    85
    │ │ │ │
    86} // namespace gtsam
    │ │ │ │ -
    3D Point
    │ │ │ │ - │ │ │ │ -
    Non-linear factor base classes.
    │ │ │ │ +
    Factor graph with utilities for hybrid factors.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    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
    │ │ │ │ -
    noiseModel::Base::shared_ptr SharedNoiseModel
    Aliases.
    Definition NoiseModel.h:724
    │ │ │ │ -
    Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
    normalize, with optional Jacobian
    Definition Point3.cpp:52
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    Represents a 3D point on a unit sphere.
    Definition Unit3.h:43
    │ │ │ │ -
    A convenient base class for creating your own NoiseModelFactor with n variables.
    Definition NonlinearFactor.h:400
    │ │ │ │ -
    Binary factor for a relative translation direction measurement w_aZb.
    Definition TranslationFactor.h:42
    │ │ │ │ -
    TranslationFactor()
    default constructor
    Definition TranslationFactor.h:49
    │ │ │ │ -
    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
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ +
    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
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    Hybrid Factor Graph Factor graph with utilities for hybrid factors.
    Definition HybridFactorGraph.h:39
    │ │ │ │ +
    Definition HybridNonlinearFactorGraph.h:33
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    shared_ptr to This
    Definition HybridNonlinearFactorGraph.h:38
    │ │ │ │ +
    HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
    Implicit copy/downcast constructor to override explicit template container constructor.
    Definition HybridNonlinearFactorGraph.h:54
    │ │ │ │ +
    A factor graph is a bipartite graph with factor nodes connected to variable nodes.
    Definition FactorGraph.h:97
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,113 +1,109 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TranslationFactor.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-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 │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -_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. │ │ │ │ │ +_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_:_:_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_:_:_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_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 │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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/a00968_source.html │ │ │ │┄ Files 26% 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/navigation/Scenario.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SfmTrack.h
    │ │ │ │ +
    Scenario.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    24
    │ │ │ │ -
    25#include <Eigen/Core>
    │ │ │ │ -
    26#include <string>
    │ │ │ │ -
    27#include <utility>
    │ │ │ │ -
    28#include <vector>
    │ │ │ │ +
    18#pragma once
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    21
    │ │ │ │ +
    22namespace gtsam {
    │ │ │ │ +
    23
    │ │ │ │ +
    │ │ │ │ +
    25class Scenario {
    │ │ │ │ +
    26 public:
    │ │ │ │ +
    28 virtual ~Scenario() {}
    │ │ │ │
    29
    │ │ │ │ -
    30namespace gtsam {
    │ │ │ │ +
    30 // Quantities a Scenario needs to specify:
    │ │ │ │
    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;
    │ │ │ │ +
    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 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 }
    │ │ │ │ +
    47 Vector3 acceleration_b(double t) const {
    │ │ │ │ +
    48 const Rot3 nRb = rotation(t);
    │ │ │ │ +
    49 return nRb.transpose() * acceleration_n(t);
    │ │ │ │ +
    50 }
    │ │ │ │ +
    51};
    │ │ │ │
    │ │ │ │ -
    94
    │ │ │ │ +
    52
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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 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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ -
    │ │ │ │ -
    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};
    │ │ │ │ -
    │ │ │ │ -
    119
    │ │ │ │ -
    120using SfmTrack2dVector = std::vector<SfmTrack2d>;
    │ │ │ │ -
    121
    │ │ │ │ -
    │ │ │ │ -
    126struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ -
    3D Point
    │ │ │ │ -
    2D Point
    │ │ │ │ +
    99 private:
    │ │ │ │ +
    100 const Rot3 nRb_;
    │ │ │ │ +
    101 const Vector3 p0_, v0_, a_n_, omega_b_;
    │ │ │ │ +
    102};
    │ │ │ │ +
    │ │ │ │ +
    103
    │ │ │ │ +
    104} // namespace gtsam
    │ │ │ │ + │ │ │ │ +
    Navigation state composing of attitude, position, and velocity.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::pair< size_t, size_t > SiftIndex
    Sift index for SfmTrack.
    Definition SfmTrack.h:36
    │ │ │ │ -
    void print(const Matrix &A, const string &s, ostream &stream)
    print without optional string, must specify cout yourself
    Definition Matrix.cpp:156
    │ │ │ │ -
    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
    │ │ │ │ -
    std::pair< size_t, Point2 > SfmMeasurement
    A measurement with its camera index.
    Definition SfmTrack.h:33
    │ │ │ │ +
    Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
    cross product
    Definition Point3.cpp:64
    │ │ │ │
    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
    │ │ │ │ -
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ -
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ -
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ -
    Track containing 2D measurements associated with a single 3D point.
    Definition SfmTrack.h:43
    │ │ │ │ -
    void addMeasurement(size_t idx, const gtsam::Point2 &m)
    Add measurement (camera_idx, Point2) to track.
    Definition SfmTrack.h:65
    │ │ │ │ -
    const SfmMeasurement & measurement(size_t idx) const
    Get the measurement (camera index, Point2) at pose index idx
    Definition SfmTrack.h:73
    │ │ │ │ -
    std::vector< SiftIndex > siftIndices
    The feature descriptors (optional)
    Definition SfmTrack.h:48
    │ │ │ │ -
    const SiftIndex & siftIndex(size_t idx) const
    Get the SIFT feature index corresponding to the measurement at idx
    Definition SfmTrack.h:78
    │ │ │ │ -
    bool hasUniqueCameras() const
    Check that no two measurements are from the same camera.
    Definition SfmTrack.h:84
    │ │ │ │ -
    Eigen::MatrixX2d measurementMatrix() const
    Return the measurements as a 2D matrix.
    Definition SfmTrack.h:100
    │ │ │ │ -
    size_t numberMeasurements() const
    Total number of measurements in this track.
    Definition SfmTrack.h:70
    │ │ │ │ -
    std::vector< SfmMeasurement > measurements
    The 2D image projections (id,(u,v))
    Definition SfmTrack.h:45
    │ │ │ │ -
    Eigen::VectorXi indexVector() const
    Return the camera indices of the measurements.
    Definition SfmTrack.h:109
    │ │ │ │ -
    Definition SfmTrack.h:126
    │ │ │ │ -
    Point3 rgb() const
    Get RGB values describing 3d point.
    Definition SfmTrack.h:148
    │ │ │ │ -
    float b
    RGB color of the 3D point.
    Definition SfmTrack.h:128
    │ │ │ │ -
    const Point3 & point3() const
    Get 3D point.
    Definition SfmTrack.h:145
    │ │ │ │ -
    Point3 p
    3D position of the point
    Definition SfmTrack.h:127
    │ │ │ │ +
    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
    │ │ │ │ +
    A 3D pose (R,t) : (Rot3,Point3)
    Definition Pose3.h:37
    │ │ │ │ +
    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
    │ │ │ │ +
    const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
    get rotation
    Definition Pose3.cpp:315
    │ │ │ │ +
    Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
    Definition Rot3.h:58
    │ │ │ │ +
    Matrix3 matrix() const
    return 3*3 rotation matrix
    Definition Rot3M.cpp:219
    │ │ │ │ +
    Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
    Definition NavState.h:34
    │ │ │ │ +
    Simple trajectory simulator.
    Definition Scenario.h:25
    │ │ │ │ +
    virtual Pose3 pose(double t) const =0
    pose at time t
    │ │ │ │ +
    virtual Vector3 acceleration_n(double t) const =0
    acceleration in nav frame
    │ │ │ │ +
    virtual Vector3 velocity_n(double t) const =0
    velocity at time t, in nav frame
    │ │ │ │ +
    virtual ~Scenario()
    virtual destructor
    Definition Scenario.h:28
    │ │ │ │ +
    virtual Vector3 omega_b(double t) const =0
    angular velocity in body frame
    │ │ │ │ +
    Scenario with constant twist 3D trajectory.
    Definition Scenario.h:60
    │ │ │ │ +
    ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
    Construct scenario with constant twist [w,v].
    Definition Scenario.h:63
    │ │ │ │ +
    Vector3 acceleration_n(double t) const override
    acceleration in nav frame
    Definition Scenario.h:74
    │ │ │ │ +
    Vector3 omega_b(double t) const override
    angular velocity in body frame
    Definition Scenario.h:70
    │ │ │ │ +
    Pose3 pose(double t) const override
    pose at time t
    Definition Scenario.h:67
    │ │ │ │ +
    Vector3 velocity_n(double t) const override
    velocity at time t, in nav frame
    Definition Scenario.h:71
    │ │ │ │ +
    Accelerating from an arbitrary initial state, with optional rotation.
    Definition Scenario.h:83
    │ │ │ │ +
    Pose3 pose(double t) const override
    pose at time t
    Definition Scenario.h:92
    │ │ │ │ +
    Vector3 omega_b(double t) const override
    angular velocity in body frame
    Definition Scenario.h:95
    │ │ │ │ +
    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
    │ │ │ │ +
    Vector3 acceleration_n(double t) const override
    acceleration in nav frame
    Definition Scenario.h:97
    │ │ │ │ +
    Vector3 velocity_n(double t) const override
    velocity at time t, in nav frame
    Definition Scenario.h:96
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,248 +1,213 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SfmTrack.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 │ │ │ │ │ -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 │ │ │ │ │ +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 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ +30 // Quantities a Scenario needs to specify: │ │ │ │ │ 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; │ │ │ │ │ +_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 │ │ │ │ │ -_4_8 std::vector _s_i_f_t_I_n_d_i_c_e_s; │ │ │ │ │ -49 │ │ │ │ │ +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 │ │ │ │ │ -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 │ │ │ │ │ +_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 } │ │ │ │ │ +_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 │ │ │ │ │ -_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}; │ │ │ │ │ -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 │ │ │ │ │ +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_:_:_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ + * _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/a01217.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01232.js │ │ │ │┄ Files 25% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,12 +1,10 @@ │ │ │ │ │ -var a01217 = [ │ │ │ │ │ - ["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] │ │ │ │ │ +var a01232 = [ │ │ │ │ │ + ["gtsam2openGL", "a01232.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ + ["gtsam2openGL", "a01232.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ + ["initialCamerasAndPointsEstimate", "a01232.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ + ["initialCamerasEstimate", "a01232.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ + ["openGL2gtsam", "a01232.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ + ["readBal", "a01232.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ + ["writeBAL", "a01232.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ + ["writeBALfromValues", "a01232.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01235.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00671.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/inference/BayesTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Namespaces | │ │ │ │ Functions
    │ │ │ │ -
    FrobeniusFactor.cpp File Reference
    │ │ │ │ +
    BayesTree-inst.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

    Various factors that minimize some Frobenius norm. │ │ │ │ +

    Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ More...

    │ │ │ │ + │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ 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.
     
    │ │ │ │ +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

    │ │ │ │ -

    Various factors that minimize some Frobenius norm.

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

    Bayes Tree is a tree of cliques of a Bayes Chain.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    │ │ │ │ +Michael Kaess
    │ │ │ │ +
    │ │ │ │ +Viorela Ila
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -FrobeniusFactor.cpp File Reference │ │ │ │ │ -Various factors that minimize some Frobenius norm. _M_o_r_e_._._. │ │ │ │ │ +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_. │ │ │ │ │ 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. │ │ │ │ │ +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 ********** │ │ │ │ │ -Various factors that minimize some Frobenius norm. │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ +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 │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _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/a01259.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01406.html │ │ │ │┄ Files 13% similar despite different names │ │ │ │ @@ -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/precompiled_header.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    │ │ │ │ -Namespaces
    │ │ │ │ -
    EssentialMatrixConstraint.cpp File Reference
    │ │ │ │ +
    precompiled_header.cpp File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

    │ │ │ │ -Namespaces

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

    We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ +More...

    │ │ │ │

    Detailed Description

    │ │ │ │ -
    Author
    Frank Dellaert
    │ │ │ │ -
    │ │ │ │ -Pablo Alcantarilla
    │ │ │ │ -
    Date
    Jan 5, 2014
    │ │ │ │ +

    We need exactly one compilation unit that includes the precompiled headers.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │ +
    Date
    November 2018
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,17 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ +precompiled_header.cpp File Reference │ │ │ │ │ +We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Pablo Alcantarilla │ │ │ │ │ Date │ │ │ │ │ - Jan 5, 2014 │ │ │ │ │ + November 2018 │ │ │ │ │ * _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 │ │ │ │ │ + * _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/a01265_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01052_source.html │ │ │ │┄ Files 10% similar despite different names │ │ │ │ @@ -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/nonlinear/GraphvizFormatting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    JacobianFactorQR.h
    │ │ │ │ +
    GraphvizFormatting.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    12
    │ │ │ │ -
    13namespace gtsam {
    │ │ │ │ -
    14
    │ │ │ │ -
    15class GaussianBayesNet;
    │ │ │ │ -
    16
    │ │ │ │ -
    20template<size_t D, size_t ZDim>
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ +Go to the documentation of this file.
    1/* ----------------------------------------------------------------------------
    │ │ │ │ +
    2
    │ │ │ │ +
    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
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │
    22
    │ │ │ │ -
    23 typedef RegularJacobianFactor<D> Base;
    │ │ │ │ -
    24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
    │ │ │ │ -
    25
    │ │ │ │ -
    26public:
    │ │ │ │ +
    23namespace gtsam {
    │ │ │ │ +
    24
    │ │ │ │ +
    25class Values;
    │ │ │ │ +
    26class Value;
    │ │ │ │
    27
    │ │ │ │ -
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ -
    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;
    │ │ │ │ - │ │ │ │ -
    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
    │ │ │ │ -
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    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
    │ │ │ │ +
    │ │ │ │ +
    32struct GTSAM_EXPORT GraphvizFormatting : public DotWriter {
    │ │ │ │ +
    34 enum Axis { X, Y, Z, NEGX, NEGY, NEGZ };
    │ │ │ │ +
    35
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    40 double scale;
    │ │ │ │ + │ │ │ │ +
    43
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    Graphviz formatter.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    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
    │ │ │ │ -
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ -
    Character and index key used to refer to variables.
    Definition Symbol.h:35
    │ │ │ │ -
    A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    Definition GaussianFactorGraph.h:75
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    shared_ptr to this class
    Definition GaussianFactorGraph.h:81
    │ │ │ │ -
    void add(const GaussianFactor &factor)
    Add a factor by value - makes a copy.
    Definition GaussianFactorGraph.h:127
    │ │ │ │ -
    A Gaussian factor in the squared-error form.
    Definition JacobianFactor.h:91
    │ │ │ │ -
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ -
    JacobianFactor for Schur complement that uses Q noise model.
    Definition JacobianFactorQR.h:21
    │ │ │ │ -
    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
    │ │ │ │ - │ │ │ │ + │ │ │ │ +
    This is the base class for any type to be stored in Values.
    Definition Value.h:37
    │ │ │ │ +
    DotWriter is a helper class for writing graphviz .dot files.
    Definition DotWriter.h:35
    │ │ │ │ +
    Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
    Definition GraphvizFormatting.h:32
    │ │ │ │ +
    Axis
    World axes to be assigned to paper axes.
    Definition GraphvizFormatting.h:34
    │ │ │ │ +
    Axis paperVerticalAxis
    The world axis assigned to the vertical paper axis.
    Definition GraphvizFormatting.h:38
    │ │ │ │ +
    GraphvizFormatting()
    Default constructor sets up robot coordinates.
    Definition GraphvizFormatting.h:46
    │ │ │ │ +
    bool mergeSimilarFactors
    Merge multiple factors that have the same connectivity.
    Definition GraphvizFormatting.h:41
    │ │ │ │ +
    double scale
    Scale all positions to reduce / increase density.
    Definition GraphvizFormatting.h:40
    │ │ │ │ +
    Axis paperHorizontalAxis
    The world axis assigned to the horizontal paper axis.
    Definition GraphvizFormatting.h:36
    │ │ │ │ +
    A non-templated config holding any types of Manifold-group elements.
    Definition Values.h:65
    │ │ │ │ +
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,129 +1,110 @@ │ │ │ │ │ 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 */ │ │ │ │ │ +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-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#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 { │ │ │ │ │ +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> │ │ │ │ │ 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: │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25class _V_a_l_u_e_s; │ │ │ │ │ +26class Value; │ │ │ │ │ 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 │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_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 │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrQQRR..hh │ │ │ │ │ + * _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 │ │ │ │ │ * Generated on Tue Jan 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 │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00149.html │ │ │ │┄ Files 19% similar despite different names │ │ │ │ @@ -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/base/OptionalJacobian.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ Classes | │ │ │ │ Namespaces
    │ │ │ │ -
    SmartFactorBase.h File Reference
    │ │ │ │ +
    OptionalJacobian.h File Reference
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ -

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

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

    │ │ │ │ │ │ │ │ -

    Go to the source code of this file.

    │ │ │ │ +

    Go to the source code of this file.

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

    │ │ │ │ Classes

    class  gtsam::SmartFactorBase< CAMERA >
     Base class for smart factors. 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...
     
    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.
     
    │ │ │ │

    Detailed Description

    │ │ │ │ -

    Base class to create smart factors on poses or cameras.

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Antoni Rosinol
    │ │ │ │ -
    │ │ │ │ -Zsolt Kira
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ +

    Special class for optional Jacobian arguments.

    │ │ │ │ +
    Author
    Frank Dellaert
    │ │ │ │
    │ │ │ │ -Chris Beall
    │ │ │ │ +Natesh Srinivasan
    │ │ │ │ +
    Date
    Nov 28, 2014
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +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 │ │ │ │ │ -SmartFactorBase.h File Reference │ │ │ │ │ -Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ +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_:_:_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_._._. │ │ │ │ │ + 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. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Antoni Rosinol │ │ │ │ │ - Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Chris Beall │ │ │ │ │ + Natesh Srinivasan │ │ │ │ │ + Date │ │ │ │ │ + Nov 28, 2014 │ │ │ │ │ * _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 │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _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/a01289_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00698_source.html │ │ │ │┄ Files 6% similar despite different names │ │ │ │ @@ -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/inference/ISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    lago.h
    │ │ │ │ +
    ISAM.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
    │ │ │ │ -
    35#pragma once
    │ │ │ │ -
    36
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    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);
    │ │ │ │ +
    18// \callgraph
    │ │ │ │ +
    19#pragma once
    │ │ │ │ +
    20
    │ │ │ │ + │ │ │ │ +
    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;
    │ │ │ │ +
    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
    │ │ │ │ -
    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);
    │ │ │ │ +
    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
    │ │ │ │ -
    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 algorithm using boost library.
    │ │ │ │ -
    Factor Graph Values.
    │ │ │ │ -
    Linear Factor Graph where all factors are Gaussians.
    │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ +
    77} // namespace gtsam
    │ │ │ │ +
    Included from all GTSAM files.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    The Factor::error simply extracts the.
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ +
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ +
    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
    │ │ │ │ +
    ISAM(const Base &bayesTree)
    Copy constructor.
    Definition ISAM.h:52
    │ │ │ │ +
    ISAM()
    Create an empty Bayes Tree.
    Definition ISAM.h:49
    │ │ │ │ +
    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,77 +1,90 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -lago.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) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 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); │ │ │ │ │ +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; │ │ │ │ │ +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 │ │ │ │ │ -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); │ │ │ │ │ +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 │ │ │ │ │ -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. │ │ │ │ │ +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 │ │ │ │ │ -_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_:_:_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 │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _l_a_g_o_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _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/a01313.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00962.html │ │ │ │┄ Files 15% similar despite different names │ │ │ │ @@ -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/navigation/ImuFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ -Classes | │ │ │ │ -Namespaces
    │ │ │ │ -
    SmartProjectionPoseFactor.h File Reference
    │ │ │ │ +Namespaces | │ │ │ │ +Functions
    │ │ │ │ +
    ImuFactor.cpp File Reference
    │ │ │ │ │ │ │ │
    │ │ │ │ - │ │ │ │ -

    Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ -More...

    │ │ │ │ - │ │ │ │ -

    Go to the source code of this file.

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

    │ │ │ │ -Classes

    class  gtsam::SmartProjectionPoseFactor< CALIBRATION >
     If you are using the factor, please cite: L. More...
     
    struct  gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >
     traits 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

    │ │ │ │ -

    Smart factor on poses, assuming camera calibration is fixed.

    │ │ │ │ -
    Author
    Luca Carlone
    │ │ │ │ +
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Stephen Williams
    │ │ │ │ +
    │ │ │ │ +Richard Roberts
    │ │ │ │ +
    │ │ │ │ +Vadim Indelman
    │ │ │ │
    │ │ │ │ -Chris Beall
    │ │ │ │ +David Jensen │ │ │ │
    │ │ │ │ -Zsolt Kira
    │ │ │ │ +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 │ │ │ │ │ -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_:_:_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_<_ _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_._._. │ │ │ │ │ -  │ │ │ │ │ +_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 ********** │ │ │ │ │ -Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ - Chris Beall │ │ │ │ │ - Zsolt Kira │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _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 │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _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/a01328_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00491_source.html │ │ │ │┄ Files 18% similar despite different names │ │ │ │ @@ -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/hybrid/HybridGaussianISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    InitializePose.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
    │ │ │ │ -
    19#pragma once
    │ │ │ │ -
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
    20#pragma once
    │ │ │ │ +
    21
    │ │ │ │ +
    22#include <gtsam/base/Testable.h>
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    26
    │ │ │ │
    27namespace gtsam {
    │ │ │ │ -
    28namespace initialize {
    │ │ │ │ -
    29
    │ │ │ │ -
    30static constexpr Key kAnchorKey = 99999999;
    │ │ │ │ -
    31
    │ │ │ │ -
    36template <class Pose>
    │ │ │ │ -
    37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
    │ │ │ │ -
    38 NonlinearFactorGraph poseGraph;
    │ │ │ │ +
    28
    │ │ │ │ +
    │ │ │ │ +
    34class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
    │ │ │ │ +
    35 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    38 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │
    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);
    │ │ │ │ +
    42
    │ │ │ │ + │ │ │ │
    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 // 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
    │ │ │ │ - │ │ │ │ -
    Factor Graph consisting of non-linear factors.
    │ │ │ │ - │ │ │ │ +
    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> {};
    │ │ │ │ +
    91
    │ │ │ │ +
    92} // namespace gtsam
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Linearized Hybrid factor graph that uses type erasure.
    │ │ │ │ +
    Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
    │ │ │ │ +
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ -
    IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
    add is a synonym for push_back.
    Definition FactorGraph.h:210
    │ │ │ │ -
    static shared_ptr Create(size_t dim)
    Create a unit covariance noise model.
    Definition NoiseModel.h:597
    │ │ │ │ -
    In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │ +
    A Bayes tree representing a Hybrid density.
    Definition HybridBayesTree.h:64
    │ │ │ │ +
    Definition HybridGaussianFactorGraph.h:102
    │ │ │ │ +
    Definition HybridGaussianISAM.h:34
    │ │ │ │ +
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,124 +1,99 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -InitializePose.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 │ │ │ │ │ -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> │ │ │ │ │ +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 { │ │ │ │ │ -28namespace initialize { │ │ │ │ │ -29 │ │ │ │ │ -30static constexpr _K_e_y kAnchorKey = 99999999; │ │ │ │ │ -31 │ │ │ │ │ -36template │ │ │ │ │ -37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) │ │ │ │ │ -{ │ │ │ │ │ -38 NonlinearFactorGraph poseGraph; │ │ │ │ │ +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 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); │ │ │ │ │ +42 │ │ │ │ │ +44 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M(); │ │ │ │ │ 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 // 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 │ │ │ │ │ +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 {}; │ │ │ │ │ +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_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_T_r_e_e_._h │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ +_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_:_:_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_:_:_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_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._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/a01346_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00845_source.html │ │ │ │┄ Files 5% similar despite different names │ │ │ │ @@ -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/linear/GaussianISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymbolicISAM.h
    │ │ │ │ +
    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
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    23
    │ │ │ │ -
    24namespace gtsam {
    │ │ │ │ -
    25
    │ │ │ │ -
    │ │ │ │ -
    26 class GTSAM_EXPORT SymbolicISAM : public ISAM<SymbolicBayesTree>
    │ │ │ │ -
    27 {
    │ │ │ │ -
    28 public:
    │ │ │ │ - │ │ │ │ -
    30 typedef SymbolicISAM This;
    │ │ │ │ -
    31 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    32
    │ │ │ │ -
    35
    │ │ │ │ - │ │ │ │ -
    38
    │ │ │ │ -
    40 SymbolicISAM(const SymbolicBayesTree& bayesTree);
    │ │ │ │ -
    41
    │ │ │ │ -
    43
    │ │ │ │ -
    44 };
    │ │ │ │ -
    │ │ │ │ -
    45
    │ │ │ │ -
    46}
    │ │ │ │ -
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ +
    23#include <gtsam/base/Testable.h>
    │ │ │ │ +
    24
    │ │ │ │ +
    25namespace gtsam {
    │ │ │ │ +
    26
    │ │ │ │ +
    │ │ │ │ +
    27 class GTSAM_EXPORT GaussianISAM : public ISAM<GaussianBayesTree>
    │ │ │ │ +
    28 {
    │ │ │ │ +
    29 public:
    │ │ │ │ + │ │ │ │ +
    31 typedef GaussianISAM This;
    │ │ │ │ +
    32 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    33
    │ │ │ │ +
    36
    │ │ │ │ + │ │ │ │ +
    39
    │ │ │ │ +
    41 GaussianISAM(const GaussianBayesTree& bayesTree);
    │ │ │ │ +
    42
    │ │ │ │ +
    44
    │ │ │ │ +
    45 };
    │ │ │ │ +
    │ │ │ │ +
    46
    │ │ │ │ +
    48 template <>
    │ │ │ │ +
    49 struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
    │ │ │ │ +
    50
    │ │ │ │ +
    51}
    │ │ │ │ +
    Concept check for values that can be used in unit tests.
    │ │ │ │ +
    Incremental update functionality (iSAM) for BayesTree.
    │ │ │ │ +
    Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
    │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │ +
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    A Bayes tree with an update methods that implements the iSAM algorithm.
    Definition ISAM.h:31
    │ │ │ │ -
    A Bayes tree that represents the connectivity between variables but is not associated with any probab...
    Definition SymbolicBayesTree.h:51
    │ │ │ │ -
    Definition SymbolicISAM.h:27
    │ │ │ │ +
    A Bayes tree representing a Gaussian density.
    Definition GaussianBayesTree.h:52
    │ │ │ │ +
    Definition GaussianISAM.h:28
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicISAM.h │ │ │ │ │ +GaussianISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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,47 +16,60 @@ │ │ │ │ │ 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> │ │ │ │ │ +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_/_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} │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_2_7 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_I_S_A_M : public _I_S_A_M │ │ │ │ │ +28 { │ │ │ │ │ +29 public: │ │ │ │ │ +30 typedef _I_S_A_M_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ +31 typedef _G_a_u_s_s_i_a_n_I_S_A_M _T_h_i_s; │ │ │ │ │ +32 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +33 │ │ │ │ │ +36 │ │ │ │ │ +38 _G_a_u_s_s_i_a_n_I_S_A_M(); │ │ │ │ │ +39 │ │ │ │ │ +41 _G_a_u_s_s_i_a_n_I_S_A_M(const _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ +42 │ │ │ │ │ +44 │ │ │ │ │ +45 }; │ │ │ │ │ +46 │ │ │ │ │ +48 template <> │ │ │ │ │ +_4_9 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_I_S_A_M> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +50 │ │ │ │ │ +51} │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _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_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_:_:_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_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_:_:_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_I_S_A_M │ │ │ │ │ +DDeeffiinniittiioonn GaussianISAM.h:28 │ │ │ │ │ * _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 │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _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/a01355_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00245_source.html │ │ │ │┄ Files 9% similar despite different names │ │ │ │ @@ -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/discrete/DiscreteEliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    SymbolicJunctionTree.h
    │ │ │ │ +
    DiscreteEliminationTree.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
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │ -
    27 // Forward declarations
    │ │ │ │ -
    28 class SymbolicEliminationTree;
    │ │ │ │ -
    29
    │ │ │ │ -
    │ │ │ │ -
    50 class GTSAM_EXPORT SymbolicJunctionTree :
    │ │ │ │ -
    51 public JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> {
    │ │ │ │ -
    52 public:
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    55 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ -
    56
    │ │ │ │ -
    65 SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
    │ │ │ │ -
    66 };
    │ │ │ │ -
    │ │ │ │ -
    67
    │ │ │ │ - │ │ │ │ -
    70}
    │ │ │ │ -
    The junction tree.
    │ │ │ │ - │ │ │ │ - │ │ │ │ +
    │ │ │ │ +
    31 class GTSAM_EXPORT DiscreteEliminationTree :
    │ │ │ │ +
    32 public EliminationTree<DiscreteBayesNet, DiscreteFactorGraph>
    │ │ │ │ +
    33 {
    │ │ │ │ +
    34 public:
    │ │ │ │ + │ │ │ │ + │ │ │ │ +
    37 typedef boost::shared_ptr<This> shared_ptr;
    │ │ │ │ +
    38
    │ │ │ │ + │ │ │ │ +
    48 const VariableIndex& structure, const Ordering& order);
    │ │ │ │ +
    49
    │ │ │ │ + │ │ │ │ +
    56 const Ordering& order);
    │ │ │ │ +
    57
    │ │ │ │ +
    59 bool equals(const This& other, double tol = 1e-9) const;
    │ │ │ │ +
    60
    │ │ │ │ +
    61 private:
    │ │ │ │ +
    62
    │ │ │ │ +
    63 friend class ::EliminationTreeTester;
    │ │ │ │ +
    64
    │ │ │ │ +
    65 };
    │ │ │ │ +
    │ │ │ │ +
    66
    │ │ │ │ +
    67}
    │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ -
    A Cluster is just a collection of factors.
    Definition ClusterTree.h:36
    │ │ │ │ -
    A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
    Definition JunctionTree.h:50
    │ │ │ │ -
    Definition SymbolicEliminationTree.h:29
    │ │ │ │ -
    A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
    Definition SymbolicJunctionTree.h:51
    │ │ │ │ -
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition SymbolicJunctionTree.h:55
    │ │ │ │ -
    JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base
    Base class.
    Definition SymbolicJunctionTree.h:53
    │ │ │ │ -
    SymbolicJunctionTree This
    This class.
    Definition SymbolicJunctionTree.h:54
    │ │ │ │ +
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │ +
    Elimination tree for discrete factors.
    Definition DiscreteEliminationTree.h:33
    │ │ │ │ +
    DiscreteEliminationTree This
    This class.
    Definition DiscreteEliminationTree.h:36
    │ │ │ │ +
    boost::shared_ptr< This > shared_ptr
    Shared pointer to this class.
    Definition DiscreteEliminationTree.h:37
    │ │ │ │ +
    EliminationTree< DiscreteBayesNet, DiscreteFactorGraph > Base
    Base class.
    Definition DiscreteEliminationTree.h:35
    │ │ │ │ +
    A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
    Definition DiscreteFactorGraph.h:101
    │ │ │ │ +
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │ +
    Definition Ordering.h:34
    │ │ │ │ +
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicJunctionTree.h │ │ │ │ │ +DiscreteEliminationTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_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,76 @@ │ │ │ │ │ 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> │ │ │ │ │ +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 │ │ │ │ │ -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_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 │ │ │ │ │ +_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); │ │ │ │ │ +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: │ │ │ │ │ +62 │ │ │ │ │ +63 friend class ::EliminationTreeTester; │ │ │ │ │ +64 │ │ │ │ │ +65 }; │ │ │ │ │ +66 │ │ │ │ │ +67} │ │ │ │ │ +_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_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +_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_:_:_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 │ │ │ │ │ +_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 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 │ │ │ │ │ +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 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 │ │ │ │ │ +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 │ │ │ │ │ - * _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 │ │ │ │ │ + * _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/a01361_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01370_source.html │ │ │ │┄ Files 1% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │
    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
    │ │ │ │
    19#pragma once
    │ │ │ │
    20
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    24
    │ │ │ │
    25namespace gtsam {
    │ │ │ │
    26
    │ │ │ │
    │ │ │ │
    27 class GTSAM_EXPORT SymbolicEliminationTree :
    │ │ │ │
    28 public EliminationTree<SymbolicBayesNet, SymbolicFactorGraph>
    │ │ │ │
    29 {
    │ │ │ │ @@ -153,17 +153,17 @@ │ │ │ │
    63template<>
    │ │ │ │
    │ │ │ │
    64struct traits<SymbolicEliminationTree> : public Testable<SymbolicEliminationTree> {
    │ │ │ │
    65};
    │ │ │ │
    │ │ │ │
    66
    │ │ │ │
    67} //\ namespace gtsam
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │
    A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
    Definition concepts.h:30
    │ │ │ │
    Template to create a binary predicate.
    Definition Testable.h:111
    │ │ │ │
    A helper that implements the traits interface for GTSAM types.
    Definition Testable.h:151
    │ │ │ │
    An elimination tree is a data structure used intermediately during elimination.
    Definition EliminationTree.h:52
    │ │ │ │
    Definition Ordering.h:34
    │ │ │ │
    The VariableIndex class computes and stores the block column structure of a factor graph.
    Definition VariableIndex.h:43
    │ │ │ │ @@ -173,13 +173,13 @@ │ │ │ │
    SymbolicEliminationTree This
    This class.
    Definition SymbolicEliminationTree.h:32
    │ │ │ │
    Symbolic Factor Graph.
    Definition SymbolicFactorGraph.h:64
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01403_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01244_source.html │ │ │ │┄ Files 24% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorSVD.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │
    │ │ │ │ │ │ │ │
    No Matches
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
    │ │ │ │ -
    precompiled_header.h
    │ │ │ │ +
    JacobianFactorSVD.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// 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>
    │ │ │ │ - │ │ │ │ - │ │ │ │ -
    33#include <gtsam/base/FastList.h>
    │ │ │ │ -
    34#include <gtsam/base/FastMap.h>
    │ │ │ │ -
    35#include <gtsam/base/FastSet.h>
    │ │ │ │ - │ │ │ │ -
    37#include <gtsam/base/GenericValue.h>
    │ │ │ │ -
    38#include <gtsam/base/Group.h>
    │ │ │ │ -
    39#include <gtsam/base/Lie.h>
    │ │ │ │ - │ │ │ │ -
    41#include <gtsam/base/Manifold.h>
    │ │ │ │ -
    42#include <gtsam/base/Matrix.h>
    │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
    48#include <gtsam/base/Testable.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>
    │ │ │ │ - │ │ │ │ -
    57
    │ │ │ │ -
    58
    │ │ │ │ -
    Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
    │ │ │ │ -
    Base class and basic functions for Lie types.
    │ │ │ │ -
    Provides additional testing facilities for common data structures.
    │ │ │ │ -
    Special class for optional Jacobian arguments.
    │ │ │ │ -
    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
    │ │ │ │ -
    The base class for any variable that can be optimized or used in a factor.
    │ │ │ │ - │ │ │ │ -
    An easy way to control which allocator is used for Fast* collections.
    │ │ │ │ -
    typedef and functions to augment Eigen's VectorXd
    │ │ │ │ -
    A thin wrapper around std::map that uses boost's fast_pool_allocator.
    │ │ │ │ -
    Provides convenient mappings of common member functions for testing.
    │ │ │ │ -
    Concept check for values that can be used in unit tests.
    │ │ │ │ -
    A faster implementation for DSF, which uses vector rather than btree.
    │ │ │ │ -
    Timing utilities.
    │ │ │ │ -
    Group product of two Lie Groups.
    │ │ │ │ -
    Concept check class for variable types with Group properties.
    │ │ │ │ -
    A thin wrapper around std::vector that uses a custom allocator.
    │ │ │ │ -
    Base class and basic functions for Manifold types.
    │ │ │ │ -
    A thin wrapper around std::list that uses boost's fast_pool_allocator.
    │ │ │ │ -
    A thin wrapper around std::set that uses boost's fast_pool_allocator.
    │ │ │ │ -
    A matrix with column blocks of pre-defined sizes.
    │ │ │ │ -
    Typedefs for easier changing of types.
    │ │ │ │ -
    Global debugging flags.
    │ │ │ │ -
    Convenience functions for serializing data structures via boost.serialization.
    │ │ │ │ -
    Access to matrices via blocks of pre-defined sizes.
    │ │ │ │ -
    typedef and functions to augment Eigen's MatrixXd
    │ │ │ │ +
    1/*
    │ │ │ │ +
    2 * @file JacobianFactorSVD.h
    │ │ │ │ +
    3 * @date Oct 27, 2013
    │ │ │ │ +
    4 * @uthor Frank Dellaert
    │ │ │ │ +
    5 */
    │ │ │ │ +
    6
    │ │ │ │ +
    7#pragma once
    │ │ │ │ + │ │ │ │ +
    9
    │ │ │ │ +
    10namespace gtsam {
    │ │ │ │ +
    28template<size_t D, size_t ZDim>
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    39 }
    │ │ │ │ +
    │ │ │ │ +
    40
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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
    │ │ │ │ +
    │ │ │ │ + │ │ │ │ +
    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}
    │ │ │ │ +
    JacobianFactor class with fixed sized blcoks.
    │ │ │ │ +
    Global functions in a separate testing namespace.
    Definition chartTesting.h:28
    │ │ │ │ +
    FastVector< Key > KeyVector
    Define collection type once and for all - also used in wrappers.
    Definition Key.h:86
    │ │ │ │ +
    std::uint64_t Key
    Integer nonlinear key type.
    Definition types.h:100
    │ │ │ │ +
    const KeyVector & keys() const
    Access the factor's involved variable keys.
    Definition Factor.h:140
    │ │ │ │ +
    void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
    Internal function to fill blocks and set dimensions.
    Definition JacobianFactor-inl.h:60
    │ │ │ │ +
    JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
    Definition RegularJacobianFactor.h:32
    │ │ │ │ +
    JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.
    Definition JacobianFactorSVD.h:29
    │ │ │ │ +
    JacobianFactorSVD()
    Default constructor.
    Definition JacobianFactorSVD.h:38
    │ │ │ │ +
    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
    │ │ │ │ +
    JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
    Empty constructor with keys.
    Definition JacobianFactorSVD.h:42
    │ │ │ │
    │ │ │ │
    │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,115 +1,116 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -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 │ │ │ │ │ -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// 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 │ │ │ │ │ -_c_h_o_l_e_s_k_y_._h │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_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. │ │ │ │ │ -_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. │ │ │ │ │ -_V_a_l_u_e_._h │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ -_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_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 │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ -Provides convenient mappings of common member functions for testing. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ -A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ -Group product of two Lie Groups. │ │ │ │ │ -_G_r_o_u_p_._h │ │ │ │ │ -Concept check class for variable types with Group properties. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_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. │ │ │ │ │ -_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_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_d_e_b_u_g_._h │ │ │ │ │ -Global debugging flags. │ │ │ │ │ -_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ -_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 │ │ │ │ │ +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. │ │ │ │ │ +_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 │ │ │ │ │ - * _p_r_e_c_o_m_p_i_l_e_d___h_e_a_d_e_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * JJaaccoobbiiaannFFaaccttoorrSSVVDD..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8