Handle noise and NonlinearEquality constraints #2332
Merged
+925
−418
Conversation
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
Contributor
There was a problem hiding this comment.
Pull request overview
This PR enhances the handling of nonlinear equality constraints by adding support for noise model whitening and proper handling of constraints derived from NonlinearEquality. The changes migrate from the deprecated NonlinearEquality1 class to the more flexible NonlinearEquality class with an updated API parameter order (key first, then feasible value).
Key changes:
- Integrated noise model whitening in the multifrontal solver when noise models are present
- Added constraint handling for unary fully-constrained factors derived from
NonlinearEquality - Deprecated
NonlinearEquality1in favor ofNonlinearEqualitywith updated parameter ordering
Reviewed changes
Copilot reviewed 9 out of 9 changed files in this pull request and generated no comments.
Show a summary per file
| File | Description |
|---|---|
| gtsam/nonlinear/NonlinearEquality.h | Updated documentation, deprecated NonlinearEquality1 class, reformatted evaluateError method, fixed spelling in comments |
| gtsam/constrained/NonlinearEqualityConstraint.h | Fixed spelling error in comment ("equlity" → "equality") |
| gtsam/linear/MultifrontalClique.h | Added fields for constraint handling: blockIndex_ map, fixedFrontals_ set, updated documentation |
| gtsam/linear/MultifrontalClique.cpp | Implemented whitening and constraint handling in fillAb, added markFixedFrontals helper, updated eliminateInPlace to handle fixed frontals |
| gtsam/linear/tests/testMultifrontalSolver.cpp | Added comprehensive tests for constrained factors, weighted measurements, updated existing tests with different noise models |
| tests/testNonlinearEquality.cpp | Migrated from NonlinearEquality1 to NonlinearEquality with updated parameter order (key, value, gain), modernized smart pointers, removed explicit orderings |
| tests/testBoundingConstraint.cpp | Updated UnaryEqualityConstraint calls to new parameter order |
| tests/simulated2DConstraints.h | Changed typedef from NonlinearEquality1 to NonlinearEquality |
| gtsam/nonlinear/doc/NonlinearEquality.ipynb | Removed deprecated NonlinearEquality1 documentation section, reformatted string literals |
ProfFan
reviewed
Jan 1, 2026
Member
Author
|
@ProfFan I merged the PR on this one which should alleviate concern about constraint handling. |
Allow for HessianFactors in MultifrontalSolver
Feature/marginals
dellaert
force-pushed
the
fix/constraints
branch
from
befc2b0 to
548694a
Compare
ProfFan
approved these changes
Jan 3, 2026
Collaborator
|
9950X3D with TBB |
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.
Also added burn in timing script. Schur advantage is not so apparent now:
Linux benchmarks
A bit disheartening for BAL :-( For chain (large balanced tree) still have speedups > 10, but for these BAL datasets GTSAM and new solver are essentially the same. Puzzling.
With TBB:
Without TBB: