iSAM2 Updates to Support riSAM Implementation

[DanMcGann]

This is the first of 3 PRs that support the integration of riSAM into GTSAM. This PR is focused on updating iSAM2 with some additional functionality to support riSAM's implementation, and also fixes issue #2405 as the bug significantly affected the new DoglegLineSearch optimizer. Below are details on the changes:

Feature 1 - One Step Look Ahead

riSAM relies on predicting the effect of an update on the bayes tree structure to identify which factors to re-convexify. To support this this PR adds predictUpdateInfo to the iSAM2 interface and a helper function traverseTop to the BayesTree interface. Tests can be found in testGaussianISAM2 and testSymbolicBayesTree

Feature 2 - DoglegLineSearch

In riSAM, factors are routinely re-convexified, causing the region of trust (a-la Dogleg) to be uncorrelated between time-steps. At the same time we require a trust region optimizer to ensure wild steps are not taken. DoglegLineSearch functions as a line-search along the dog-leg arc. The search is performed independently for each optimziation step supporting cases like riSAM where the underlying cost function is changing significantly between optimization steps. While this was motivated by riSAM, I think there are additional scenarios where this could be useful (e.g. using smart-factors to significantly change the cost function on updates).

While testing this feature I ran across the same bug identified in #2405 as DoglegLineSearch runs compute blend often. The cause is just as identified by @william-swarmbotics, adding variables in ISAM2::update before the bayes tree is actually updated causes a dimensionality mis-match between the the gradient and newton directions. This was fixed in updateDelta by ensuring the vectors match structure.

Feature 3 - Skip redundant updateDelta

In iSAM2::update, delta is updated, but the check to identify when updates are needed ignores the deltaReplaceMask this causes redundant updateDelta calls especially in the case that relinearizationSkip = 1 (which is used for riSAM). To avoid this redundent work I added a check for the deltaReplaceMask. Though It may be better to move this check into relinarizationNeeded, let me know if there is a preference on the location of this check.

Final Notes

All functionality is tested, and I believe tests have full coverage. Notably I also added a test that is able to deterministically recreate the error in #2405 in TEST(DogLegOptimizer, ComputeBlend). Additionally, the python interface file is updated (though not tested locally due to a cmake version issue. I am trusting the CI to catch any issues there)

[Copilot]

Pull request overview

Updates iSAM2 to support riSAM integration by adding update-structure prediction utilities, introducing a Dogleg Line Search trust-region strategy, and fixing a delta-structure mismatch that could crash Dogleg blending.

Changes:

• Added ISAM2::predictUpdateInfo and BayesTree::traverseTop to enable one-step look-ahead for Bayes tree contamination.
• Introduced Dogleg Line Search optimization support (params + implementation) and added tests to exercise it.
• Fixed delta structure alignment during dogleg computations and reduced redundant updateDelta calls.

Reviewed changes

Copilot reviewed 10 out of 10 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
tests/testGaussianISAM2.cpp	Adds iSAM2 integration tests for DoglegLineSearch and predictUpdateInfo.
tests/testDoglegOptimizer.cpp	Adds unit tests for DoglegLineSearch iteration and a regression test for ComputeBlend crash.
gtsam/symbolic/tests/testSymbolicBayesTree.cpp	Adds tests for new traverseTop helper.
gtsam/nonlinear/nonlinear.i	Exposes DoglegLineSearch params + predictUpdateInfo to the Python (SWIG) interface.
gtsam/nonlinear/ISAM2Params.h	Adds ISAM2DoglegLineSearchParams and extends ISAM2Params optimization variant.
gtsam/nonlinear/ISAM2.h	Adds predictUpdateInfo API to iSAM2.
gtsam/nonlinear/ISAM2.cpp	Implements DoglegLineSearch branch, delta structure fix, redundant updateDelta skip, and predictUpdateInfo.
gtsam/nonlinear/DoglegOptimizerImpl.h	Implements DoglegLineSearch algorithm.
gtsam/inference/BayesTree.h	Declares traverseTop and traversal helpers.
gtsam/inference/BayesTree-inst.h	Implements traverseTop and traversal helpers.

[dellaert]

See some comments from co-pilot above.
CI fails with an error related to one of them - I think.

/home/runner/work/gtsam/gtsam/gtsam/symbolic/SymbolicBayesTree.cpp
In file included from /home/runner/work/gtsam/gtsam/gtsam/symbolic/SymbolicBayesTree.cpp:23:
/home/runner/work/gtsam/gtsam/gtsam/inference/BayesTree-inst.h:593:64: error: ISO C++ specifies that qualified reference to 'shared_ptr' is a constructor name rather than a type in this context, despite preceding 'typename' keyword [-Werror,-Winjected-class-name]
      this->traversePath(traversedKeys, typename sharedClique::shared_ptr(
                                                               ^
/home/runner/work/gtsam/gtsam/gtsam/symbolic/SymbolicBayesTree.cpp:30:18: note: in instantiation of member function 'gtsam::BayesTree<gtsam::SymbolicBayesTreeClique>::traversePath' requested here
  template class BayesTree<SymbolicBayesTreeClique>;
                 ^
1 error generated

[dellaert]

Sorry for the delay, I was busy with class assignments, etc.

[dellaert]

LGTM!