GTSAM deep dive

This page goes from the idea to the code. Each layer is explained from first principles, then mapped to the files that implement it. Source: borglab/gtsam develop @ a74146d, version 4.3.2.

0. The one-paragraph version

You have unknowns \(X = \{x_1,\dots,x_n\}\) and measurements \(z_i\). Each measurement depends on a small subset of unknowns, \(z_i = h_i(X_i) + \nu_i\) with Gaussian noise \(\nu_i \sim \mathcal{N}(0,\Sigma_i)\). The MAP estimate is

GTSAM represents each term as a factor and the whole sum as a factor graph. It then minimizes the sum by repeating three steps:

1. Linearize every factor at the current estimate, producing a Gaussian factor graph.
2. Eliminate that linear graph variable by variable, producing a Bayes net or Bayes tree, i.e. a sparse triangular system.
3. Back-substitute to get a tangent-space step \(\delta\), then retract each variable onto its manifold.

Everything else in the repo is either a type of variable (geometry), a type of factor (slam/navigation/sfm), a smarter way to do step 2 (orderings, multifrontal, iSAM2), or tooling.

1. Architecture at a glance

Library layers, bottom to top. Each layer only depends on the layers below it.

*Non-test .h/.cpp lines.

        python / matlab (wrap)                   ← generated from *.i
 ┌───────────────────────────────────────────┐
 │ slam  navigation  sfm  sam  hybrid  discrete│  ← factors & variable types
 ├───────────────────────────────────────────┤
 │ nonlinear   (Values, NonlinearFactorGraph,  │
 │              LM/GN/Dogleg, ISAM2, Expression)│
 ├───────────────────────────────────────────┤
 │ linear      (Jacobian/HessianFactor, noise, │
 │              GaussianBayesTree, PCG)        │
 ├───────────────────────────────────────────┤
 │ inference   (templated elimination machinery)│ ← symbolic is its simplest instantiation
 ├───────────────────────────────────────────┤
 │ geometry    base   (Lie/manifold traits)    │
 └───────────────────────────────────────────┘
            Eigen · CCOLAMD · METIS · TBB(opt) · Boost(opt)

2. Layer 1: manifolds without inheritance (gtsam/base)

2.1 The problem

An optimizer needs, for every variable type \(T\):

• its dimension \(d\) (the tangent-space size, e.g. 6 for Pose3, 3 for Rot3),
• Retract(x, δ) → T, which applies a step,
• Local(x, y) → ℝ^d, which measures the difference from \(x\) to \(y\) in the tangent space at \(x\),
• equality-with-tolerance and printing, for tests.

GTSAM must support types it doesn't own, such as Eigen::Vector3d and double, so it can't require a common base class. The solution is traits.

2.2 traits<T>: the central concept

gtsam/base/Manifold.h defines internal::Manifold<Class>, Lie.h defines internal::LieGroup<Class>, and VectorSpace.h defines internal::VectorSpace<Class>. A type opts in by specializing traits:

template <> struct traits<Pose3> : public internal::LieGroup<Pose3> {};

These helpers forward to the class's own retract/localCoordinates/Expmap/Logmap and expose dimension, TangentVector, and ChartJacobian. Concept checks (GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>)) fail at compile time if a method is missing. There's also a runtime invariant you should copy into any port's tests:

check_manifold_invariants(a, b):  Local(a,a) ≈ 0   and   Retract(a, Local(a,b)) ≈ b

Hierarchy of structure: manifold_tag ⊂ group_tag ⊂ lie_group_tag ⊂ vector_space_tag. Vector spaces get Retract = + and Local = − for free.

2.3 Lie groups via CRTP

internal::LieGroup<Class, N> in Lie.h is a CRTP base. Given operator*, inverse(), AdjointMap(), and static Expmap/Logmap, it derives compose, between, retract, and localCoordinates, with Jacobians:

That's literally the code: if (H1) *H1 = g.inverse().AdjointMap(); in compose, and if (H) *H = -derived().AdjointMap(); in inverse.

4.3 adds constructed Lie groups: ProductLieGroup<G,H> (G×H), SemidirectLieGroup (G⋉H), and TangentLieGroup (TG). This is how compound states like NavState, \(SE_2(3)\), and Gal3 get their structure without hand-written exp/log maps.

2.4 OptionalJacobian<Rows, Cols>: zero-cost optional outputs

Almost every geometric function has the signature

Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Hself = {},
                                             OptionalJacobian<3,3> Hpoint = {}) const;

OptionalJacobian wraps an Eigen::Map over the caller's matrix memory, or nothing. The callee writes if (H) *H = ...;. This gives:

• no allocation when the Jacobian isn't requested (most calls during line search / error evaluation),
• fixed-size Jacobians at compile time, so the stack matrices get Eigen's SIMD paths,
• one function for both value and derivative, so they can't drift apart.

It uses placement-new to retarget the map (usurp). If you port to a language without this pattern, the closest equivalents are an out-parameter Option<&mut Matrix> (Rust) or nullptr-able spans.

2.5 Value / GenericValue<T>: type erasure for heterogeneous containers

An optimization problem mixes Pose3, Point3, Vector3 (bias), Cal3_S2… in one container. Value is the abstract base with dim(), retract_(Vector), localCoordinates_(Value), clone_(), equals_(), and print(). GenericValue<T> implements it by calling traits<T>. This is the bridge from compile-time generic code (traits) to run-time polymorphic code (factor graphs).

2.6 Block matrices

• VerticalBlockMatrix: one dense column-major matrix \([A_1 | A_2 | \dots | b]\) with column-block offsets. It is the storage of JacobianFactor.
• SymmetricBlockMatrix: the upper triangle of \(\begin{bmatrix} G & g \\ g^\top & f\end{bmatrix}\) in blocks. It is the storage of HessianFactor, and is updated in place during Cholesky.

2.7 Misc. foundations

• Testable.h / TestableAssertions.h: assert_equal(expected, actual, tol). Every class has print() and equals(other, tol), and the test suite depends on both.
• numericalDerivative.h: central differences on manifolds (numericalDerivative21<…> etc.). This is how almost all analytic Jacobians in the codebase are tested. It's the first thing to port.
• timing.h: the gttic/gttoc hierarchical profiler (compiled out unless GTSAM_ENABLE_TIMING).
• FastMap/FastSet/FastList: STL containers with a pool allocator, now mostly plain STL.
• DSFMap/DSFVector: union-find (used for tracks in SfM, and in Kruskal).
• treeTraversal/: depth-first forest traversal with pre-/post-order visitors, parallelized with TBB when enabled. All Bayes tree algorithms go through it.

3. Layer 2: keys, factors, graphs (gtsam/inference)

3.1 Keys

A Key is a uint64_t. A Symbol('x', 3) packs a char into the top 8 bits and an index into the low 56 bits. LabeledSymbol adds a robot-id char, and EdgeKey packs two uint32s. Keys are opaque everywhere, and printing goes through a KeyFormatter.

3.2 Factor and FactorGraph<F>

Factor is just a KeyVector: the variables it touches. FactorGraph<F> is a vector<shared_ptr<F>> that may contain nulls, because removed factors leave holes so that indices stay stable. iSAM2 relies on this.

3.3 Why elimination is the heart of everything

Take a linear factor graph \(\sum_i \|A_i X_i - b_i\|^2\). To eliminate a variable \(x_j\):

1. Gather every factor that touches \(x_j\), using VariableIndex (variable → factor indices).
2. Combine them into one dense factor over \(x_j\) and its neighbors \(S_j\) (the separator).
3. Factorize it into a conditional \(p(x_j \mid S_j)\) (a row block of \(R\)) and a new factor on \(S_j\) (the Schur complement).
4. Put the new factor back into the graph. This is where fill-in comes from: \(S_j\) are now all connected.

Doing this for every variable in an ordering yields a Bayes net, a directed acyclic graph of conditionals. That's exactly an upper-triangular \(R\) in sparse form, solved by back-substitution. The algorithm is identical for any factor type that can be combined and split. GTSAM therefore implements it once, templated, in inference/, and instantiates it for symbolic, Gaussian, discrete, and hybrid factors.

// The pluggable dense kernel: (factors touching keys, keys) → (conditional, remaining factor)
using Eliminate = std::function<std::pair<shared_ptr<Conditional>, shared_ptr<Factor>>(
                                const FactorGraph&, const Ordering&)>;

For Gaussian graphs, EliminatePreferCholesky is the default (HessianFactor path; it falls back to QR when constrained noise is present). EliminateQR is the Jacobian/Householder path.

3.4 Orderings

Ordering::Colamd (default; constrained variants ColamdConstrainedLast/First force some keys to the end, which iSAM2 uses to keep the newest poses at the root), Ordering::Metis (nested dissection, better for large SfM graphs), Natural, or Custom. The ordering determines fill-in, which determines everything about performance. It is computed on the symbolic structure only.

3.5 From elimination tree to Bayes tree

• EliminationTree: one node per variable. A node's parent is the first variable eliminated in its separator. Siblings are independent, so you can parallelize across them.
• JunctionTree (a ClusterTree): merges chains of nodes into cliques, i.e. sets of variables eliminated together as one dense block. This is multifrontal elimination: bigger dense ops, better BLAS efficiency.
• BayesTree: the result. Each clique holds a conditional \(p(F_k \mid S_k)\) (frontals given separator). It's a directed tree, so:
  • solving = one top-down pass (optimize),
  • marginals for a clique = walk to the root (marginalFactor, with shortcuts cached in cliques),
  • incremental updates touch only the path from changed variables to the root. That last property is the iSAM2 idea.

eliminateSequential produces a BayesNet and eliminateMultifrontal produces a BayesTree. Both are mixed in by EliminateableFactorGraph<FG> (CRTP), so GaussianFactorGraph, SymbolicFactorGraph, DiscreteFactorGraph, and HybridGaussianFactorGraph all get the same API.

3.6 gtsam/symbolic

The same machinery with factors that carry keys only, no numbers. It's used to predict structure, to test the inference templates, and by iSAM2 to rebuild the top of the tree. If you port the inference layer, port symbolic first. It is the cleanest test bed (the testSymbolic* tests check exact tree shapes).

4. Layer 3: Gaussian world (gtsam/linear)

4.1 Noise models: whitening

A measurement with covariance \(\Sigma\) contributes \(\|r\|^2_\Sigma = \|\Sigma^{-1/2} r\|^2\). noiseModel::Base provides whiten(r) = \(\Sigma^{-1/2}r\), and also WhitenSystem(A, b), which whitens Jacobians too, so everything downstream sees unit covariance ("whitened" least squares). Hierarchy:

4.2 JacobianFactor

This is the linear factor \(\tfrac12\|A X - b\|^2\) stored in a VerticalBlockMatrix (\([A_1 | \dots | A_k | b]\)). Key operations: error, gradient, hessianDiagonal, transposeMultiplyAdd (for iterative solvers), and eliminate. EliminateQR stacks all involved factors into one big Jacobian (columns ordered: frontals first, then separator), runs in-place Householder QR, then splitConditional(nrFrontals) cuts off the top rows as a GaussianConditional and leaves the remainder as a new JacobianFactor on the separator.

4.3 HessianFactor

This is the information form \(\tfrac12 x^\top G x - x^\top g + \tfrac12 f\) stored in a SymmetricBlockMatrix. Combining factors is just adding their Hessian blocks (cheap), and eliminating is partial Cholesky on the frontal block. That's why it is the default: for typical SLAM it's faster than stacking and QR-ing tall Jacobians. QR is more stable when the system is ill-conditioned.

4.4 GaussianConditional, GaussianBayesNet, GaussianBayesTree, VectorValues

• GaussianConditional: \(R x_F + S x_S = d\) (plus a noise model). solve(parents) does back-substitution for the frontals.
• VectorValues: Key → Vector, the tangent-space counterpart of Values.
• GaussianBayesTree::optimize() solves top-down. optimizeGradientSearch and gradientAtZero support Dogleg.

4.5 Solvers

• Direct, sequential or multifrontal: chosen by NonlinearOptimizerParams::linearSolverType: MULTIFRONTAL_CHOLESKY (default), MULTIFRONTAL_QR, SEQUENTIAL_*, Iterative, CHOLMOD, plus the new MultifrontalSolver (4.3: a reusable, imperative solver with packed storage that is set up once and refactorized each iteration; it is what NonlinearMultifrontalSolver uses in LM).
• Iterative: ConjugateGradientSolver, PCGSolver with Preconditioner (Jacobi, block-Jacobi), and SubgraphSolver: a spanning tree of the graph is solved directly and used as a preconditioner for the rest (Dellaert & Jian). It's good for huge pose graphs.
• CUDA (linear/cuda, experimental, GTSAM_ENABLE_CUDA): device sparse SPD system via cuDSS, and matrix-free PCG.
• KalmanFilter: a linear KF expressed as factor-graph elimination. It's a good pedagogical file.

5. Layer 4: nonlinear (gtsam/nonlinear)

5.1 Values

std::map<Key, std::unique_ptr<Value>>. at<T>(key) does a checked dynamic_cast to GenericValue<T>. retract(VectorValues) maps each variable with its own manifold retract. localCoordinates does the inverse. Every optimizer iteration is: linearize at Values → solve → Values.retract(delta).

5.2 NonlinearFactor → NoiseModelFactor → NoiseModelFactorN<T1..Tn>

• NonlinearFactor: error(Values) → double, linearize(Values) → GaussianFactor, dim().

• NoiseModelFactor: adds a noise model. Its linearize is the canonical recipe (NonlinearFactor.cpp):

  std::vector<Matrix> A(size());
  Vector b = -unwhitenedError(x, A);   // residual + Jacobians wrt each variable
  noiseModel_->WhitenSystem(A, b);     // Σ^{-1/2}
  return std::make_shared<JacobianFactor>(terms(keys, A), b);
  

• NoiseModelFactorN<ValueTypes...>: variadic template. You implement

  Vector evaluateError(const T1& x1, const T2& x2,
                       OptionalMatrixType H1, OptionalMatrixType H2) const override;
  

  and the base pulls typed values out of Values and sizes the Jacobians. Writing a custom factor = writing this one function correctly. BetweenFactor<T> is the canonical example: error = Local(measured, x1.between(x2)), with Jacobians from between.

5.3 Expression<T>: reverse-mode autodiff on manifolds

Expression<Point2> e = project(transformTo(pose, landmark)) builds a tree of ExpressionNodes. Evaluating with Jacobians records an ExecutionTrace (forward pass, storing local Jacobians in a stack-allocated CallRecord arena). It then runs reverse accumulation (reverseAD1/2) into a JacobianMap keyed by variable. The templates are heavy because they keep fixed-size Jacobian types through the whole chain. ExpressionFactor<T> turns an expression plus measurement into a factor without writing derivatives. AdaptAutoDiff bridges Ceres-style autodiff functors.

5.4 Optimizers

All inherit NonlinearOptimizer, which has iterate() and optimize() (loop until converged by relative/absolute error decrease or max iterations).

How LM damping is actually implemented (internal/LevenbergMarquardtState.h::buildDampedSystem): GTSAM does not modify \(J^\top J\) in place. It adds a prior factor per variable, \(\sqrt{\lambda}\,I\) (or \(\sqrt{\lambda\,\mathrm{diag}(J^\top J)}\) when diagonalDamping), to the linear graph, then eliminates the augmented graph normally. The tryLambda loop:

1. build the damped system for the current \(\lambda\) and solve for \(\delta\),
2. compute the linearized cost change (model-predicted decrease) and the actual cost change \(f(x) - f(x \oplus \delta)\),
3. modelFidelity = actual / predicted; accept if it exceeds minModelFidelity, then decrease \(\lambda\) (Nielsen-style or by lambdaFactor),
4. otherwise increase \(\lambda\) and retry. Give up at lambdaUpperBound.

Reproducing this exactly matters if you want bit-comparable iteration counts.

5.5 ISAM2: incremental smoothing

The Bayes tree makes incremental inference cheap. A new factor on variables \(K\) only invalidates the cliques containing \(K\) and their ancestors up to the root. ISAM2::update(newFactors, newTheta) (in ISAM2.cpp), in the order the code runs it:

1. add new variables to \(\Theta\) (addVariables), and new factors to the nonlinear graph and VariableIndex;
2. optionally evaluate the error before the update;
3. mark keys involved in new or removed factors;
4. every relinearizeSkip updates, mark variables whose current delta exceeds relinearizeThreshold (fluid relinearization: only variables that moved a lot get a new linearization point);
5. findFluid: mark the cliques containing those variables plus their ancestors;
6. retract \(\Theta_J \leftarrow \Theta_J \oplus \Delta_J\) for relinearized variables;
7. linearize the new factors;
8. if fill-in grew past adaptiveReorderThreshold, force a batch reorder (4.3 feature);
9. recalculate: detach the top of the tree containing marked cliques, keep the orphan subtrees below, re-eliminate the top's factors plus cached boundary factors from the orphans with a constrained COLAMD (new variables last), re-attach the orphans. If more than about 65% of the variables are affected, do a full batch instead.

delta_ is updated lazily (updateDelta, with wildfire threshold propagation, or Dogleg). calculateEstimate() = \(\Theta \oplus \Delta\). Parameters: ISAM2Params (GaussNewton or Dogleg, factorization CHOLESKY/QR, relinearizeThreshold, relinearizeSkip, cacheLinearizedFactors, findUnusedFactorSlots…).

Related: NonlinearISAM (iSAM1, periodic batch relinearization), BatchFixedLagSmoother / IncrementalFixedLagSmoother (marginalize old states, keep a window), ExtendedKalmanFilter (a factor graph with a window of one), and Marginals (covariances from the Bayes tree).

6. Layer 5: geometry (gtsam/geometry)

Each type comes with traits, Expmap/Logmap plus their derivatives (ExpmapDerivative, LogmapDerivative = right Jacobians), AdjointMap, and exhaustive numerical-derivative tests.

7. Layer 6: domain factors

7.1 gtsam/slam

PriorFactor<T>, BetweenFactor<T>, GenericProjectionFactor (pose + landmark → pixel), GeneralSFMFactor (also optimizes calibration), StereoFactor, RangeFactor, BearingFactor, BearingRangeFactor, OrientedPlane3Factor, EssentialMatrixFactor, and smart factors (SmartProjectionPoseFactor, SmartProjectionRigFactor). Smart factors don't add landmarks as variables. They triangulate internally and Schur-complement the landmark out (RegularImplicitSchurFactor, JacobianFactorQ/SVD), which is a big win in VIO. lago.h does linear initialization of planar pose graphs. InitializePose3 does chordal rotation init. dataset.h handles g2o/TORO file I/O (common test data in examples/Data).

7.2 gtsam/navigation: IMU preintegration

Between two keyframes \(i,j\) you have hundreds of IMU samples. Integrating them into the state would put hundreds of variables into the graph. Instead, preintegrate them into a relative motion \(\Delta R_{ij}, \Delta v_{ij}, \Delta p_{ij}\) that doesn't depend on the absolute state at \(i\), plus its covariance and its Jacobian with respect to the bias. When the bias estimate changes slightly, correct to first order instead of re-integrating (Forster et al.).

• PreintegrationParams (gravity, noise densities, body-sensor transform), PreintegratedImuMeasurements.
• Implementations selected at compile time: TangentPreintegration (default, GTSAM_TANGENT_PREINTEGRATION), ManifoldPreintegration, and 4.3's LieGroupPreintegration on \(SE_2(3)\).
• ImuFactor (5-way: pose_i, vel_i, pose_j, vel_j, bias), ImuFactor2 (NavState), CombinedImuFactor (includes bias random walk).
• NavState = (R, p, v). GNSS: GPSFactor, PseudorangeFactor, CarrierPhaseFactor, DopplerFactor. AttitudeFactor, MagFactor, BarometricFactor. Filters: InvariantEKF, LieGroupEKF, ManifoldEKF, EquivariantFilter.

7.3 gtsam/sfm

SfmData/SfmTrack (BAL format), ShonanAveraging (certifiable rotation averaging by lifting to \(SO(p)\) and doing a Riemannian staircase), TranslationRecovery (1DSfM-style), MFAS (outlier rejection for translation directions), DsfTrackGenerator, TransferFactor (fundamental-matrix transfer), and SfmLevenbergMarquardt (a specialized BA optimizer, with a CUDA path).

8. Beyond Gaussian

• discrete: factors over finite domains stored as DecisionTree / AlgebraicDecisionTree (ADDs: compressed, leaf-merged). The same elimination yields DiscreteBayesNet / DiscreteBayesTree, with max-product (MPE) or sum-product. DiscreteSearch does A*/k-best. CSP helpers include AllDiff.
• hybrid: mixtures where a discrete mode selects a Gaussian (HybridGaussianFactor, HybridGaussianConditional). Elimination produces hybrid Bayes nets/trees, and HybridSmoother prunes modes to stay tractable. Use cases: multi-hypothesis data association, contact modes for legged robots.
• constrained: equality and inequality constraints (NonlinearEqualityConstraint), with PenaltyOptimizer, AugmentedLagrangianOptimizer, and QP/LP ActiveSetSolver.
• basis: Chebyshev, Fourier, and spline bases plus BasisFactors, for continuous-time trajectories. Related: WnoaFactor (white-noise-on-acceleration GP prior) in nonlinear/.
• certifiable: SDP relaxations and the Riemannian staircase for global-optimality certificates.

9. Tooling, build, bindings, tests

• CMake: cmake/Handle*.cmake, one file per dependency. Key options:
  • Required: Eigen (bundled by default).
  • Optional: Boost (GTSAM_USE_BOOST_FEATURES, GTSAM_ENABLE_BOOST_SERIALIZATION; default OFF for ROS 2 builds), TBB, METIS (GTSAM_SUPPORT_NESTED_DISSECTION), MKL, GeographicLib, CUDA, MOSEK.
  • Math-changing: GTSAM_USE_QUATERNIONS, GTSAM_POSE3_EXPMAP, GTSAM_ROT3_EXPMAP, GTSAM_TANGENT_PREINTEGRATION, GTSAM_LIEGROUP_PREINTEGRATION, GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR, GTSAM_SLOW_BUT_CORRECT_EXPMAP.
  • Pitfall: GTSAM_BUILD_WITH_MARCH_NATIVE. Eigen alignment must match between GTSAM and its consumers, and a mismatch crashes at runtime.
• Windows export: GTSAM_EXPORT macros (see Using-GTSAM-EXPORT.md). Template classes are tricky.
• Serialization: Boost.Serialization (serialize() templates on every class) for save/load and for the Python pickle path.
• wrap: gtsam/*.i files declare a C++-like interface (see geometry.i). wrap/gtwrap parses them (pyparsing) and generates pybind11 C++ and MATLAB MEX code. Template instantiation is spelled out explicitly in the .i files.
• Tests: CppUnitLite (a tiny xUnit, TEST(Group, Name)), 291 test files / ~3,200 cases under gtsam/*/tests. They use assert_equal, EXPECT_CORRECT_FACTOR_JACOBIANS (factorTesting.h, which compares a factor's analytic Jacobians against numerical derivatives), and EXPECT_CORRECT_EXPRESSION_JACOBIANS. There are 116 Python tests in python/gtsam/tests.
• Docs: Jupyter notebooks per class in gtsam/*/doc/*.ipynb (myst site), plus Doxygen.
• Examples: examples/ has 78 programs (Pose2SLAMExample, VisualISAM2Example, ImuFactorsExample, SFMExample…) and examples/Data holds the standard datasets (g2o, BAL, KITTI snippets). These are your end-to-end regression targets.

10. Recommended code-reading path

1. examples/OdometryExample.cpp → Pose2SLAMExample.cpp → PlanarSLAMExample.cpp
2. base/Manifold.h, base/Lie.h, base/OptionalJacobian.h, geometry/Pose2.{h,cpp}
3. nonlinear/NonlinearFactor.{h,cpp}, NoiseModelFactorN.h, slam/BetweenFactor.h
4. linear/NoiseModel.h, JacobianFactor.cpp (EliminateQR, splitConditional), HessianFactor.cpp (EliminateCholesky)
5. inference/EliminateableFactorGraph-inst.h, EliminationTree-inst.h, JunctionTree-inst.h, BayesTree-inst.h
6. nonlinear/LevenbergMarquardtOptimizer.cpp + internal/LevenbergMarquardtState.h
7. nonlinear/ISAM2.cpp + ISAM2-impl.h (alongside the iSAM2 paper)
8. geometry/Rot3*.cpp, SO3.cpp, Pose3.cpp (exp/log, derivatives)
9. navigation/PreintegrationBase.cpp, TangentPreintegration.cpp, ImuFactor.cpp (alongside Forster et al.)
10. nonlinear/Expression*.h + internal/* (last; it's the hardest template code)