JacobianFactor now compute the error more efficiently by first assembling the X vector

[ProfFan]

We also now directly assemble the new GaussianConditional instead of using the copy-then-offset strategy.

During the implementation I also discovered some latent bugs related to the iteration order of the VectorValues.

• Replace FastMap-based ConcurrentMap fallback with std::unordered_map when TBB is disabled.
• Tighten BayesTree::equals logic and related Values handling.
• Silence Point2/Point3 constructor unused variable warnings.

[Copilot]

Pull request overview

This PR optimizes JacobianFactor error computation and fixes latent bugs related to iteration order in VectorValues and related containers when TBB is disabled. The main changes replace the FastMap-based fallback with std::unordered_map and ensure deterministic behavior by using key-sorted iteration where needed.

Key Changes:

• Optimized JacobianFactor error computation to assemble a combined vector and use direct matrix multiplication instead of iterative block multiplication
• Fixed VectorValues iteration order dependencies by using sorted() method in equals, operators, and other order-sensitive operations
• Replaced FastMap with std::unordered_map for ConcurrentMap when TBB is disabled

Reviewed changes

Copilot reviewed 15 out of 15 changed files in this pull request and generated 1 comment.

Show a summary per file

File	Description
gtsam/linear/JacobianFactor.cpp	Optimized unweighted_error() to assemble augmented vector and use direct matrix multiplication
gtsam/linear/GaussianConditional.cpp	Refactored likelihood() to directly construct VerticalBlockMatrix instead of copy-then-offset
gtsam/linear/VectorValues.h	Changed sorted() return type to std::map<Key, const Vector&> and moved method to public section
gtsam/linear/VectorValues.cpp	Fixed equals, operators, dot, hasSameStructure to use key-sorted iteration for deterministic comparison
gtsam/linear/Errors.cpp	Updated createErrors to use sorted order for deterministic Errors vector construction
gtsam/linear/SubgraphPreconditioner.cpp	Fixed multiplyInPlace and related methods to iterate in key-sorted order
gtsam/nonlinear/Values.cpp	Removed TBB-specific dual-iterator code path, now uses find() uniformly
gtsam/inference/BayesTree-inst.h	Tightened equals() logic to compare by key instead of relying on iteration order
gtsam/base/ConcurrentMap.h	Replaced FastMap with std::unordered_map for non-TBB builds
tests/testSubgraphPreconditioner.cpp	Added verbosity control and reformatted for consistency
gtsam/sfm/tests/testShonanAveraging.cpp	Updated expected matrix values on Apple platform (reflects deterministic algorithm behavior)
gtsam/navigation/tests/testImuBias.cpp	Silenced unused variable warning with (void) cast
gtsam/navigation/tests/testCombinedImuFactor.cpp	Removed unused variable declaration
gtsam/geometry/tests/testPoint2.cpp	Silenced unused variable warning with (void) cast
gtsam/geometry/tests/testPoint3.cpp	Silenced unused variable warning with (void) cast

[Copilot]

The sorted() method should have doxygen-style documentation explaining its purpose, return value, and usage. The method returns a std::map containing references to the original Vectors, and this lifetime dependency should be documented to help users understand that the returned map is only valid as long as the VectorValues object exists and is not modified.

[dellaert]

The JacobianFactor unweighted_error change seems like it should get its own small PR - I commented on one possible improvement.

This PR could then just be the change to unordered - which is a bit more controversial.

In both PRs it'd be nice to show the timing improvement in the PR comment, as I did in #2327.

[dellaert]

What prompted this change? likelihood is not used very much so it's not a performance bottleneck I think?

[ProfFan]

I did this to avoid hitting the slow path in unweighted_error as this used to be using firstBlock

[dellaert]

Seems like a lot of changes in this PR come from this change? Is it very much faster? Can you add performance numbers in the PR comment?

It's weird that that all this sorted business was not needed in TBB path which was already unordered...

[ProfFan]

I think TBB might have latent bugs because their "unordered" container is actually not that unordered 🤷

[ProfFan]

Result:

Std: standard GTSAM
MFS: new solver

BAL Benchmarks (Dubrovnik)

All times in seconds (Multifrontal / Standard).

dubrovnik‑16‑22106

Ordering	Before (MFS)	After (MFS)	Δ MFS	Before (Std)	After (Std)	Δ Std
Burn	0.128529	0.126969	−0.00156	0.197182	0.204591	+0.00741
Metis	0.122680	0.123158	+0.00048	0.193258	0.193156	−0.00010
Schur	0.124851	0.123400	−0.00145	0.202128	0.190944	−0.01118
Colamd	0.130227	0.125253	−0.00497	0.211059	0.201032	−0.01003

dubrovnik‑88‑64298

Ordering	Before (MFS)	After (MFS)	Δ MFS	Before (Std)	After (Std)	Δ Std
Burn	0.932189	0.945771	+0.01358	1.01873	1.04878	+0.03005
Metis	0.951794	0.901886	−0.04991	1.03761	0.986437	−0.05117
Schur	0.934710	0.926643	−0.00807	0.97489	0.990412	+0.01552
Colamd	0.890421	0.874290	−0.01613	0.989667	0.962819	−0.02685

dubrovnik‑135‑90642

Ordering	Before (MFS)	After (MFS)	Δ MFS	Before (Std)	After (Std)	Δ Std
Burn	1.39082	1.38696	−0.00386	1.50815	1.54920	+0.04105
Metis	1.47609	1.47607	−0.00002	1.68671	1.54874	−0.13797
Schur	1.47239	1.38445	−0.08794	1.72968	1.37164	−0.35804
Colamd	1.39377	1.38017	−0.01360	1.71858	1.51771	−0.20087

---

Synthetic Chain Benchmarks (timeMultifrontalSolver)

All times in seconds.

T	Before MFS	After MFS	Δ MFS	Before Std	After Std	Δ Std
10	0.002414	0.00216875	−0.00024525	0.0142875	0.0146528	+0.00036530
50	0.00983542	0.00869446	−0.00114096	0.061825	0.060038	−0.00178700
100	0.0127348	0.0115819	−0.00115290	0.111825	0.112742	+0.00091700
500	0.0292872	0.0312743	+0.00198710	0.470536	0.46525	−0.00528600
1000	0.0429256	0.0415565	−0.00136910	0.929741	0.909625	−0.02011600
5000	0.138232	0.122859	−0.01537300	4.1799	4.17908	−0.00082000

[dellaert]

Thanks ! PS, timing of MFS should not affected by error. Only shows up in nonlinear optimizer. There, BTW, we call it twice, one for delta and one for zero. I think that may be replaced with one new method errorDelta(delta) which omits the b add.