Repository navigation
IncrementalFixedLagSmoother: keep the newest variables at the root (1.5-5x faster updates, 5-100x faster marginals with multi-variable states) - #2922
Merged
dellaert merged 2 commits intoNov 8, 2026
Conversation
… and add a parameters struct createOrderingConstraints() constrained only the marginalizable keys, to be eliminated first, and only in updates that marginalize. Every other key shared one group, so ISAM2's own default of ordering the observed keys last was lost: the newest variables could end up deep in the Bayes tree, and then each update re-eliminates a long path and each marginal of the current state computes a long shortcut. The class documentation says the active states are placed in/near the root. Now, in every update, the keys of the new factors and the keys at the current timestamp are eliminated last. Three groups rely on ColamdConstrained accepting any group indices. IncrementalFixedLagSmootherParams holds the iSAM2 parameters and the smoother options, so new options need no new constructors. Its option newestKeysLast, on by default, can be set to false for the previous ordering. The existing constructor is kept and uses the default options.
Times updates and marginals of the newest pose with and without newestKeysLast, on three problems: odometry with GPS, the same with IMU states, and two pose sensors measuring in their own retained frames.
jlblancoc
added a commit
to MOLAorg/mola_state_estimation
that referenced
this pull request
Nov 8, 2026
4.2's marginalizeLeaves() only removes the marginalized keys at the front of a clique's frontals, and 4.2's update() only re-eliminates the children that depend on them, so a key could stay in the Bayes tree without a value and make the next update throw. On random windows, that happened in 96 of 1000 graphs with 4.2's own update, in 138 with the newest-keys-last ordering, and in none with this change. Also, the docs no longer say that newer GTSAM versions order the newest keys last: borglab/gtsam#2922 is in no release yet, so GTSAM 4.3.x keeps its own ordering and the parameter has no effect there. The unit test now checks the three-key guard of the third constraint group directly, and runs random windows that left marginalized keys behind without this change.
PeterCalifano
added a commit
to PeterCalifano/gtsam
that referenced
this pull request
Nov 16, 2026
- Bring in borglab develop at 8806b33, including the incremental fixed-lag ordering of borglab#2922 and borglab#2920 and the ISAM2 fixes of borglab#2825, borglab#2820 and borglab#2801. - CombinedImuFactor: keep integrateMeasurementWithDynamics() on top of upstream's sparse propagateCombinedCovariance(). The returned dense transition and gyro noise map follow the same block structure, so F P F' + Q reproduces preintMeasCov for every backend. - LeggedEstimator: drop use2ndOrderCoriolis with upstream and keep the includes both sides need. - Take upstream's HandleEigen.cmake and wrap MATLAB generator; select the system Eigen explicitly in build_gtsam.sh instead. - Keep our deletion of the packaging trigger workflow. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This file contains hidden or 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
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.
Hi! I'm a Claude agent presenting this PR on behalf of @jlblancoc 🙂
This builds on #2920 (
ColamdConstrainedaccepting any increasing group indices), which makes three constraint groups safe.Problem
createOrderingConstraints()puts the marginalizable keys in group 0 and every other key in group 1, in the updates that marginalize. That loses ISAM2's own default of ordering the keys of the new factors last, and with several variables per time step, or variables kept for the whole run (calibration, frame alignments), CCOLAMD then leaves the newest states deep in the Bayes tree. Each update re-eliminates the long path from the new factors to the root, and eachmarginalCovariance()of the current state computes a long shortcut. The class documentation says the active states are placed in/near the root.Change
IncrementalFixedLagSmootherParams: the iSAM2 parameters plus the smoother options, so later options need no new constructors. Its first option,newestKeysLast, is on by default; setting it to false gives the previous ordering.(smootherLag, ISAM2Params)is kept, with the default options.params()still returns the iSAM2 parameters; the newsmootherParams()returns all of them.smootherParams().Params(defaults of both constructors) andNewestKeysLast(two variables per step; the newest one stays at the root).timing/timeIncrementalFixedLagSmoother.cpp: the numbers below, from one binary, with the option off and on.Performance
Release, one pinned core (Ryzen 7 2700X), median of 3 runs, option off -> on.
Multi-variable problems
timeIncrementalFixedLagSmoother 3000 2,6,12,30, i.e. 300 s at 10 Hz, timed once marginalization starts:imu: pose, velocity and bias per step,CombinedImuFactor, odometryBetweenFactor, GPS at 1 Hz.frames: poses with odometry and GPS, plus two pose sensors (alternating) measuring in their own frames, whose poses are retained variables.odom: poses with odometry and GPS only.marginalCovariance(newest)msGTSAM's own examples
Their loops copied unchanged, except for building the smoother through
IncrementalFixedLagSmootherParamsand timing theupdate()calls of each step:examples/FixedLagSmootherExample.cppexamples/IncrementalFixedLagSmootherExample.cppissue1452.txt(FusionPortable, FAST-LIO2 odometry), Pose3 chain, lag 1 s, 3 updates per steppython/gtsam/examples/slam/EKF_SLAM.ipynb, its lag 0.99 s (an EKF)They are single pose chains or small 2D windows, where the newest pose already ends at the root: they neither gain nor lose. The gains come with several variables per step or variables kept for the whole run.
Downstream
The same rule in MOLA's multi-sensor state estimator (odometry sources in their own frames, IMU, GNSS; prototyped as a subclass on GTSAM 4.2) makes it 2.0x faster in a synthetic LiDAR + wheel odometry setup, and 1.5-1.7x faster per scan on two Oxford Spires sequences.
Notes
newestKeysLast = falserestores the previous ordering exactly.IncrementalFixedLagSmoother(lag, {}), which becomes ambiguous (there is none in GTSAM).