Fix iSAM2 support for smart factors #25
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
The old update() method still exists and calls the new one. The idea is that by using a struct, it is easier (i.e. API/ABI compatibility) to add the new parameters that we will need to add next.
Closed
dellaert
approved these changes
May 28, 2019
gtsam/nonlinear/ISAM2.h
Outdated
| * structure. This form makes easier to keep future API/ABI compatibility if | ||
| * parameters change. | ||
| * | ||
| * @param extraParams Additional parameters to control relinearization, |
There was a problem hiding this comment.
out of date? Also add other params?
gtsam/nonlinear/ISAM2.h
Outdated
| @@ -156,6 +157,20 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> { | |||
| const boost::optional<FastList<Key> >& extraReelimKeys = boost::none, | |||
| bool force_relinearize = false); | |||
|
|
|||
| /** | |||
| * Alternative signature of update() with all additional parameters in one | |||
gtsam/nonlinear/ISAM2.h
Outdated
| */ | ||
| virtual ISAM2Result update( | ||
| const NonlinearFactorGraph& newFactors, const Values& newTheta, | ||
| const ISAM2UpdateParams& up); |
There was a problem hiding this comment.
suggest renaming up to updateParams
gtsam/nonlinear/ISAM2.cpp
Outdated
| ISAM2Result ISAM2::update( | ||
| const NonlinearFactorGraph& newFactors, | ||
| const Values& newTheta, | ||
| const ISAM2UpdateParams& up) { |
There was a problem hiding this comment.
suggest renaming up to updateParams
gtsam/nonlinear/ISAM2.cpp
Outdated
| // Also, keys that were not observed in existing factors, but whose affected | ||
| // keys have been extended now (e.g. smart factors) | ||
| if (up.newAffectedKeys) { | ||
| for (const auto &f2ks : up.newAffectedKeys.value()) { |
There was a problem hiding this comment.
use different variable. f2ks ??
| bool force_relinearize{false}; | ||
|
|
||
| /** An optional set of new Keys that are now affected by factors, | ||
| * indexed by factor indices (as returned by ISAM2::update()). |
There was a problem hiding this comment.
Say motivation is smartfactors, perhaps illustrated with small example.
gtsam/nonlinear/ISAM2.cpp
Outdated
|
|
||
| // Augment it with existing factors which now affect to more variables: | ||
| if (up.newAffectedKeys) { | ||
| for (const auto &fk : *up.newAffectedKeys) { |
There was a problem hiding this comment.
fk ? In general, don't abbreviate. I know we used to do this, but I frown upon it now :-)
gtsam/nonlinear/ISAM2.cpp
Outdated
| if (up.newAffectedKeys) { | ||
| for (const auto &fk : *up.newAffectedKeys) { | ||
| const auto factorIdx = fk.first; | ||
| variableIndex_.augmentExistingFactor(factorIdx, fk.second); |
There was a problem hiding this comment.
Very scared of this imperative programming construct, but then again, iSAM2 is already scarily imperative. best we can hope for is we have good unit tests and did not forget about scary unforeseen side effects.
|
Oh, abbreviations, I forgot :-) All done. |
|
Thanks for this excellent contribution! |
varunagrawal
added a commit
that referenced
this pull request
Jan 4, 2021
09f8bbf71 Merge pull request #25 from borglab/fix/function-name 0dbfb6c13 fix function name to be the correct one git-subtree-dir: wrap git-subtree-split: 09f8bbf7172ba8b1bd3d2484795743f16e1a5893
varunagrawal
added a commit
that referenced
this pull request
Jan 4, 2021
09f8bbf71 Merge pull request #25 from borglab/fix/function-name 0dbfb6c13 fix function name to be the correct one f69f8b01f Merge pull request #24 from borglab/fix/pip 6519a6627 use pip install to overcome superuser issues b11ecf4e8 Merge pull request #23 from borglab/fix/remove-pip-args 813030108 remove pip-args since we are using setup.py 498d233e0 Merge pull request #22 from borglab/fix/package-install 846212ac3 set correct flags for installing gtwrap package 62161cd20 Merge pull request #21 from borglab/feature/script-vars 93be1d9f8 set script variables and move pybind11 loading so gtwrap can be used under gtsam 8770e3c7e Merge pull request #20 from borglab/fix/pybind-include 8c3c83618 proper placement of pybind11 include a9ad4f504 Merge pull request #19 from borglab/feature/package 99d8a12c7 added more documentation 4cbec1579 change to macro so we don't have to deal with function scopes b83e405b8 updates to completely install the package 38a64b3de new scripts which will be installed to bin directory bf9646235 Merge pull request #18 from borglab/fix/cmake-min c7c280099 Consistent cmake minimum required 42df58f62 Merge pull request #17 from borglab/fix/cleanup e580b282d version bump 4ccd66fa5 More finegrained handling of Python version 6476fd710 Merge pull request #16 from borglab/feature/better-find-python 8ac1296a0 use setup.py to install dependencies e9ac473be install dependencies and support versions of CMake<3.12 cf272dbd2 Merge pull request #15 from borglab/feature/utils ffc9cc4f7 new utils to reduce boilerplate 20e8e8b7a Merge pull request #11 from borglab/feature/package 04b844bd6 use new version of FindPython and be consistent 3f9d7a32a Merge pull request #13 from borglab/add_license c791075a6 Add LICENSE 517b67c46 correct working directory for setup.py 1b22b47ae move matlab.h to root directory 37b407214 Proper source directory path for use in other projects 61696dd5d configure PybindWrap within the cmake directory 1b91fc9af add config file so we can use find_package a1e6f4f53 small typo da9f351be updated README and housekeeping 64b8f78d5 files needed to allow for packaging bddda7f54 package structure git-subtree-dir: wrap git-subtree-split: 09f8bbf7172ba8b1bd3d2484795743f16e1a5893
varunagrawal
added a commit
that referenced
this pull request
Feb 17, 2022
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
A new optional parameter has been added to iSAM2::update(), only needed when using smart factors, or other factors that depend on a dynamic number of Keys.
A new unit test has been added: it runs a toy Stereo SLAM problem with both, batch LM, and iSAM2. It has a "verbosity" macro that can be commented out to enable printing all the details. Originally, the test failed with an error of "ill posed problem", it works now with iSAM2 giving exactly the same result than LM.
Closes #10