Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Hybrid Pruning #1215

Merged
merged 127 commits into from
Aug 23, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
127 commits
Select commit Hold shift + click to select a range
5e735e1
minor improvements to CombinedImuFactor
varunagrawal Sep 16, 2021
9dbb431
account for bias on position in jacobians of CombinedImuFactor
varunagrawal Sep 16, 2021
7e48962
Add unit test for checking covariance of CombinedImuFactor
varunagrawal Sep 17, 2021
b0fcd17
additional comments to make understanding the code easier
varunagrawal Sep 17, 2021
5656308
no need to assign negative to jacobians as they cancel out later
varunagrawal Sep 17, 2021
810f973
add details about noise propagation for CombinedImuFactor in ImuFacto…
varunagrawal Sep 17, 2021
da220dc
Add the Preintegrated IMU jacobians tech report and mention it in the…
varunagrawal Sep 18, 2021
a516415
update ImuFactor reference
varunagrawal Sep 20, 2021
61f2cf7
Merge branch 'develop' into fix/368
varunagrawal Sep 20, 2021
28d0393
add test for checking covariances between ImuFactor and CombinedImuFa…
varunagrawal Sep 19, 2021
a10c776
print statements in ImuFactor
varunagrawal Sep 19, 2021
65bbe6b
typedef for Vector15
varunagrawal Sep 19, 2021
3a3640c
updated CombinedImuFactor covariance with additional off-diagonal ele…
varunagrawal Sep 19, 2021
3132cfb
CombinedScenarioRunner
varunagrawal Sep 19, 2021
5371214
actually test the covariances and fix bug
varunagrawal Sep 20, 2021
10a7333
update test with comments
varunagrawal Sep 21, 2021
755c752
update ImuFactor doc
varunagrawal Sep 21, 2021
bbde7b9
remove print statements
varunagrawal Sep 21, 2021
a2bf0c4
minor refactoring
varunagrawal Sep 21, 2021
aa4a163
updated ImuFactor doc with details about CombinedImuFactor
varunagrawal Sep 28, 2021
0968c60
added details about covariance discretization with references
varunagrawal Sep 28, 2021
6bc9b50
add test for MC based covariance estimation
varunagrawal Sep 30, 2021
af714cd
undo name change from 984a90
varunagrawal Sep 28, 2021
e38ea50
detailed implementation of CombinedImuFactor noise propagation
varunagrawal Sep 28, 2021
3cee1b7
test passes
varunagrawal Sep 30, 2021
dfa32e5
lyx update
varunagrawal Oct 10, 2021
40e6d8b
formatting
varunagrawal Oct 10, 2021
995710f
update PDF doc
varunagrawal May 5, 2022
2d3859d
Merge branch 'develop' into fix/combined-imu
varunagrawal May 5, 2022
239dd62
Merge branch 'develop' into fix/combined-imu-cov
varunagrawal May 5, 2022
008bb93
Merge branch 'develop' into fix/368
varunagrawal May 5, 2022
63e2a59
Merge branch 'fix/368' into fix/combined-imu-cov
varunagrawal May 5, 2022
f0be857
Merge branch 'fix/combined-imu-cov' into fix/combined-imu
varunagrawal May 5, 2022
8dbbb1f
fix test
varunagrawal May 6, 2022
ce7c71b
fix test
varunagrawal May 8, 2022
a17134d
minor refactor to follow the math better
varunagrawal May 8, 2022
cb75d92
use prior naming scheme for bias jacobians
varunagrawal May 22, 2022
2927d92
add HybridNonlinearFactor and nonlinear HybridFactorGraph
varunagrawal May 28, 2022
78ea90b
Add MixtureFactor for nonlinear factor types
varunagrawal May 28, 2022
e91a354
convert to cpp
varunagrawal May 29, 2022
9cbd2ef
Base Hybrid Factor Graph
varunagrawal May 29, 2022
01b9a65
make GaussianMixtureFactor a subclass of HybridGaussianFactor
varunagrawal May 29, 2022
fe0d666
HybridFactorGraph fixes
varunagrawal May 29, 2022
cdd030b
Make MixtureFactor only work with NonlinearFactors and make some impr…
varunagrawal May 29, 2022
08fab8a
HybridNonlinearFactor linearize method
varunagrawal May 29, 2022
9279bd6
push_back for GaussianHybridFactor
varunagrawal May 29, 2022
3274cb1
clean up testHybridFactorGraph, need to add more tests
varunagrawal May 29, 2022
6c36b2c
GaussianHybridFactorGraph inherits from HybridFactorGraph
varunagrawal May 29, 2022
85f4b48
Improvements to GaussianHybridFactorGraph, make MixtureFactor a subcl…
varunagrawal May 30, 2022
53e8c32
Add NonlinearHybridFactorGraph class
varunagrawal May 30, 2022
7e18277
fix base class
varunagrawal May 30, 2022
119679a
linearize returns object instead of pointer
varunagrawal May 30, 2022
3212dde
remove unneeded method
varunagrawal May 30, 2022
0c16799
GaussianMixtureFactor inherits from HybridFactor
varunagrawal May 30, 2022
e711a62
More tests working
varunagrawal May 30, 2022
9e737db
initial pruning method
varunagrawal Jun 3, 2022
89768cf
record continuous keys separately
varunagrawal Jun 3, 2022
ad77a45
formatting and docs update
varunagrawal Jun 3, 2022
c2e5061
add pruning to HybridBayesNet
varunagrawal Jun 3, 2022
e25b0c8
Merge branch 'develop' into hybrid/pruning
varunagrawal Jun 3, 2022
28db8b2
use KeyVector and iterator constructor
varunagrawal Jun 4, 2022
f1a1f1c
Merge branch 'develop' into hybrid/pruning
varunagrawal Jun 10, 2022
bbd1e3f
update Lyx document based on Luca's review
varunagrawal Jul 3, 2022
4244345
Merge pull request #882 from borglab/fix/combined-imu
varunagrawal Jul 5, 2022
8f509dc
Do not optimize native by default
jlblancoc Jun 2, 2020
ea8e69c
Use system Eigen by default only if found
varunagrawal Jul 7, 2022
441ec33
fix small issue
varunagrawal Jul 7, 2022
c31298d
Capitalize Identity trait since it is static
varunagrawal Jul 7, 2022
34298c4
Merge branch 'hybrid/hybrid-factor-graph' into hybrid/pruning
varunagrawal Jul 10, 2022
c767dfa
Merge pull request #879 from borglab/fix/combined-imu-cov
varunagrawal Jul 20, 2022
2d41b01
change python CI cores to 1
varunagrawal Jul 20, 2022
8a33a5b
Revert "change python CI cores to 1"
varunagrawal Jul 22, 2022
322c080
Merge branch 'develop' into fix/368
varunagrawal Jul 22, 2022
3780b8c
Merge branch 'develop' into feature/nonlinear-hybrid
varunagrawal Jul 26, 2022
8ddc2ea
rename to HybridNonlinearFactorGraph
varunagrawal Jul 26, 2022
7a55341
add IsGaussian template check
varunagrawal Jul 26, 2022
43c28e7
renaming fixes
varunagrawal Jul 26, 2022
987448f
remove derived push_back in HybridNonlinearFactorGraph and HybridFact…
varunagrawal Jul 26, 2022
8471c97
add nonlinear switching system tests
varunagrawal Jul 30, 2022
8907922
get more nonlinear tests to work and make some updates
varunagrawal Aug 1, 2022
ac28b0e
fix setter and getter for biasAccOmegaInit
varunagrawal Aug 2, 2022
16124f3
get all but 2 tests passing
varunagrawal Aug 2, 2022
ee124c3
fix discrete only elimination (use EliminateForMPE)
varunagrawal Aug 2, 2022
b39c231
all tests pass!!!
varunagrawal Aug 2, 2022
0f732d7
fix discrete conditional test
varunagrawal Aug 3, 2022
6670779
Wrap DiscreteLookupTable
varunagrawal Aug 3, 2022
2fb11db
Merge branch 'develop' into hybrid/pruning
varunagrawal Aug 3, 2022
92a5868
Merge branch 'develop' into feature/nonlinear-hybrid
varunagrawal Aug 3, 2022
db56909
Merge branch 'hybrid/pruning' into feature/nonlinear-hybrid
varunagrawal Aug 3, 2022
060738c
Add Zenodo for generating citations
varunagrawal Aug 4, 2022
fcffa87
point Zenodo badge to Concept (aka aggregate) version
varunagrawal Aug 5, 2022
5965d8f
change discrete key variable from C to M
varunagrawal Aug 8, 2022
f5e046f
split HybridNonlinearFactorGraph to .h and .cpp
varunagrawal Aug 8, 2022
51d2f07
fix printing and key bug in MixtureFactor linearize
varunagrawal Aug 8, 2022
a3eacaa
fix adding priors in Switching
varunagrawal Aug 8, 2022
588f56e
HybridGaussianISAM unit tests
varunagrawal Aug 8, 2022
4ee23cf
Merge branch 'hybrid/hybrid-factor-graph' into hybrid/pruning
varunagrawal Aug 8, 2022
ba6e91b
Merge pull request #1265 from borglab/zenodo
varunagrawal Aug 9, 2022
60c88e3
fix print tests
varunagrawal Aug 10, 2022
4ea897c
cleaner printing
varunagrawal Aug 10, 2022
fbceda3
got some more tests working
varunagrawal Aug 10, 2022
103c78b
revert name change, save for another PR
varunagrawal Aug 10, 2022
0e4db30
use templetized constructor for MixtureFactor
varunagrawal Aug 10, 2022
a8720fe
Update README.md
pglira Aug 12, 2022
aa48658
more tests running
varunagrawal Aug 12, 2022
d3544d4
Merge pull request #1268 from pglira/patch-1
varunagrawal Aug 13, 2022
77bea31
one more test passing
varunagrawal Aug 13, 2022
2a974a4
Address review comments
varunagrawal Aug 15, 2022
ac20cff
add incremental pruning to HybridGaussianISAM
varunagrawal Aug 16, 2022
83b8103
last test to get running
varunagrawal Aug 17, 2022
0f53b3f
Merge pull request #1238 from borglab/fix/1182
varunagrawal Aug 21, 2022
7977f77
Merge pull request #1263 from borglab/feature/nonlinear-hybrid
varunagrawal Aug 21, 2022
07f0101
check subset rather than equality for GaussianISAM pruning
varunagrawal Aug 21, 2022
29c19ee
handle HybridConditional and explicitly set Gaussian Factor Graphs to…
varunagrawal Aug 21, 2022
f6df641
remove custom orderings, let it happen automatically
varunagrawal Aug 21, 2022
6b792c0
add note about sumFrontals
varunagrawal Aug 21, 2022
fb8d6ab
Merge pull request #1237 from borglab/jose-fixes
varunagrawal Aug 21, 2022
587678e
Merge pull request #874 from borglab/fix/368
varunagrawal Aug 21, 2022
893c5f7
cast to only HybridGaussianFactor
varunagrawal Aug 22, 2022
c0a351d
upgrade from ubuntu 18.04 to 20.04
varunagrawal Aug 22, 2022
4c9c106
Merge pull request #1271 from borglab/feature/nonlinear-incremental
varunagrawal Aug 22, 2022
84456f4
Merge pull request #1273 from borglab/hybrid-incremental
varunagrawal Aug 22, 2022
525e512
Merge pull request #1276 from borglab/ci/20.04
varunagrawal Aug 22, 2022
05b1174
Merge pull request #1277 from borglab/feature/nonlinear-hybrid
varunagrawal Aug 22, 2022
8fd6091
add new line
varunagrawal Aug 22, 2022
b07964b
name file correctly in doc string
varunagrawal Aug 22, 2022
7227965
Merge branch 'develop' into hybrid/pruning
varunagrawal Aug 22, 2022
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 12 additions & 12 deletions .github/workflows/build-linux.yml
Original file line number Diff line number Diff line change
Expand Up @@ -20,26 +20,26 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
ubuntu-20.04-gcc-7,
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
]

build_type: [Debug, Release]
build_unstable: [ON]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "5"
version: "7"

- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
version: "9"

- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"

Expand All @@ -60,9 +60,9 @@ jobs:
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update

sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y update
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev

if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
Expand Down
36 changes: 18 additions & 18 deletions .github/workflows/build-python.yml
Original file line number Diff line number Diff line change
Expand Up @@ -19,34 +19,34 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-18.04-gcc-5,
ubuntu-18.04-gcc-9,
ubuntu-18.04-clang-9,
ubuntu-20.04-gcc-7,
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
macOS-11-xcode-13.4.1,
ubuntu-18.04-gcc-5-tbb,
ubuntu-20.04-gcc-7-tbb,
]

build_type: [Debug, Release]
python_version: [3]
include:
- name: ubuntu-18.04-gcc-5
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7
os: ubuntu-20.04
compiler: gcc
version: "5"
version: "7"

- name: ubuntu-18.04-gcc-9
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-9
os: ubuntu-20.04
compiler: gcc
version: "9"

- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"

# NOTE temporarily added this as it is a required check.
- name: ubuntu-18.04-clang-9
os: ubuntu-18.04
- name: ubuntu-20.04-clang-9
os: ubuntu-20.04
compiler: clang
version: "9"
build_type: Debug
Expand All @@ -57,10 +57,10 @@ jobs:
compiler: xcode
version: "13.4.1"

- name: ubuntu-18.04-gcc-5-tbb
os: ubuntu-18.04
- name: ubuntu-20.04-gcc-7-tbb
os: ubuntu-20.04
compiler: gcc
version: "5"
version: "7"
flag: tbb

steps:
Expand All @@ -79,9 +79,9 @@ jobs:
gpg -a --export $LLVM_KEY | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi

sudo apt-get -y update

sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libboost-all-dev
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev

if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
Expand Down
14 changes: 7 additions & 7 deletions .github/workflows/build-special.yml
Original file line number Diff line number Diff line change
Expand Up @@ -32,31 +32,31 @@ jobs:

include:
- name: ubuntu-gcc-deprecated
os: ubuntu-18.04
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: deprecated

- name: ubuntu-gcc-quaternions
os: ubuntu-18.04
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: quaternions

- name: ubuntu-gcc-tbb
os: ubuntu-18.04
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: tbb

- name: ubuntu-cayleymap
os: ubuntu-18.04
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: cayley

- name: ubuntu-system-libs
os: ubuntu-18.04
os: ubuntu-20.04
compiler: gcc
version: "9"
flag: system-libs
Expand All @@ -74,9 +74,9 @@ jobs:
gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
fi
sudo apt-get -y update

sudo apt-get -y install cmake build-essential pkg-config libpython-dev python-numpy libicu-dev
sudo apt-get -y update
sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev

if [ "${{ matrix.compiler }}" = "gcc" ]; then
sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
Expand Down
25 changes: 24 additions & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,29 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali

We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.

## Citation

If you are using GTSAM for academic work, please use the following citation:

```
@software{gtsam,
author = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
title = {borglab/gtsam},
month = may,
year = 2022,
publisher = {Zenodo},
version = {4.2a7},
doi = {10.5281/zenodo.5794541},
url = {https://doi.org/10.5281/zenodo.5794541}
}
```

You can also get the latest citation available from Zenodo below:

[![DOI](https://zenodo.org/badge/86362856.svg)](https://doi.org/10.5281/zenodo.5794541)

Specific formats are available in the bottom-right corner of the Zenodo page.

## The Preintegrated IMU Factor

GTSAM includes a state of the art IMU handling scheme based on
Expand All @@ -73,7 +96,7 @@ GTSAM includes a state of the art IMU handling scheme based on
Our implementation improves on this using integration on the manifold, as detailed in

- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, _"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors"_, Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, _"IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation"_, Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)

If you are using the factor in academic work, please cite the publications above.

Expand Down
2 changes: 1 addition & 1 deletion cmake/GtsamBuildTypes.cmake
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
endif()

if (NOT MSVC)
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
# Add as public flag so all dependant projects also use it, as required
# by Eigen to avid crashes due to SIMD vectorization:
Expand Down
9 changes: 6 additions & 3 deletions cmake/HandleEigen.cmake
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen

option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
# Default: Use system's Eigen if found automatically:
find_package(Eigen3 QUIET)
set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)

if(NOT GTSAM_USE_SYSTEM_EIGEN)
# This option only makes sense if using the embedded copy of Eigen, it is
Expand All @@ -11,7 +14,7 @@ endif()

# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
find_package(Eigen3 REQUIRED)
find_package(Eigen3 REQUIRED) # need to find again as REQUIRED

# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
Expand Down
Loading