Graph SLAM, iSAM, bundle adjustment & structure from motion

A robot that trusts only its wheel odometry drifts: every small error in a relative motion estimate is integrated forever. Graph SLAM instead estimates many poses jointly from retained constraints. A batch solver can re-optimize the trajectory when a new constraint arrives; incremental solvers such as iSAM2 update affected Bayes-tree cliques and often avoid a full rebuild.

This page is an executed-notebook walkthrough. Every numbered In [n] cell below was executed, and the printed outputs and figures are the real results. We build the entire pipeline from scratch with numpy only — simulated lidar, ICP scan matching, pose-graph construction, loop closure, and batch Gauss–Newton optimization. That is Part I. Part II explains how iSAM and iSAM2 reuse that work as the graph grows, with a current GTSAM reference snippet. Part III then lets the structure into the optimization — bundle adjustment, run on the same world with the lidar swapped for a bearing-only “camera” — Part IV widens to structure from motion, and Part V lines the three estimation problems up side by side.

A widely used implementation. g2o (General Graph Optimization, Kümmerle et al., ICRA 2011) is an influential C++ graph-optimization back-end. The official g2o README links to g2o-python (pip install g2o-python), a separately maintained wrapper around g2o's experimental pymem Python branch. There is also the older community binding g2opy. There is also a pure-Python solver, python-graphslam (pip install graphslam), which reads the same .g2o files. Both APIs are shown later as reference snippets; the NumPy cells are the executed demo.

A typical pose-graph pipeline

1. Odometry (prediction): chain relative motion estimates → an initial guess that drifts.
2. Scan matching (ICP): align overlapping lidar scans → relative-pose measurements.
3. Graph construction: poses become nodes; each measurement becomes an edge with an information matrix.
4. Loop closure: recognize a previously visited place, verify it geometrically, and add a long-range edge.
5. Optimization (back-end): nonlinear least squares over all poses — the loop snaps shut, drift is redistributed.

Formally, with poses $\mathbf{x} = (\mathbf{x}_1,\dots,\mathbf{x}_n)$ and measured relative poses $\mathbf{z}_{ij}$ with information matrices $\Omega_{ij}$, graph SLAM solves

$$\mathbf{x}^\star \;=\; \arg\min_{\mathbf{x}} \sum_{(i,j)\in\mathcal{E}} \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j)^{\!\top}\, \Omega_{ij}\, \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j), \qquad \mathbf{e}_{ij} = \mathrm{t2v}\!\left(Z_{ij}^{-1}\,\big(X_i^{-1} X_j\big)\right),$$

i.e. the error of an edge is the mismatch between the predicted relative pose $X_i^{-1}X_j$ and the measured one $Z_{ij}$. The front-end (steps 1–4) builds this graph; the back-end (step 5) solves it.

Part I · SetupSE(2) in five functions

A 2-D pose $[x, y, \theta]$ is a rigid transform. Everything in graph SLAM is composing and inverting these, so we define the classic helpers: v2t/t2v convert between the vector and the $3{\times}3$ homogeneous-matrix form, oplus composes a pose with a relative motion, and ominus gives the relative pose between two poses.

Step 1Odometry — the drifting initial guess

We simulate a robot driving one loop around a block inside a rectangular room, taking a lidar scan (a point cloud in its own frame) at each of 64 poses. Its wheel odometry reports each relative motion corrupted by noise plus a small bias — the realistic part, because biased heading error is what makes dead reckoning bend away from the truth. Chaining (oplus-ing) the noisy increments gives the dead-reckoned trajectory: our initial guess, and nothing more.

After one lap the dead-reckoned pose is off by almost a metre in each direction and 14° in heading — and the robot has no idea.

Step 2Scan matching — ICP

Odometry says “I think I moved this much”; scan matching checks that claim against overlapping geometry. If two scans have enough non-degenerate overlap and the data association is correct, the transform that aligns them estimates the relative pose between the viewpoints. Depending on the scene and sensors this can improve on wheel odometry, but repetitive geometry, moving objects, or a poor initial guess can also make it fail.

The classic algorithm is ICP (Iterative Closest Point, Besl & McKay 1992). It alternates two steps until the alignment stops improving:

1. Correspond: transform the source scan by the current guess $T$ and pair every point with its nearest neighbour in the target scan (rejecting the worst pairs as outliers);
2. Align: solve $\;\min_{R, t} \sum_k \lVert R\,\mathbf{p}_k + \mathbf{t} - \mathbf{q}_{\mathrm{nn}(k)} \rVert^2$ in closed form via SVD (the Kabsch solution: cross-covariance of the centered point sets), and fold the correction into $T$.

Because the correspondences are only guesses, ICP needs a decent initial transform — which is exactly the role odometry plays. This division of labour repeats at every level of SLAM: a cheap prediction seeds an accurate refinement.

To see the correction clearly, match scan 10 against scan 9 but pretend the odometry guess is poor — as after a wheel slip — 40 cm and 9° off:

ICP recovers the true relative motion to a few millimetres despite the bad seed. From here on, whenever we need the relative pose between two overlapping scans, we ask ICP — seeded by whatever estimate we currently have.

Step 3Build the pose graph

Each edge stores three things:

• the pair of poses $(i, j)$ it constrains,
• the measured relative pose $\mathbf{z}_{ij}$ — here, ICP refinements of the odometry between consecutive scans,
• an information matrix $\Omega_{ij}$ (inverse covariance) saying how much to trust the measurement and how its components correlate. Here we use simple diagonal weights; a production system should estimate uncertainty from the registration geometry or calibrate it empirically.

The node values themselves are initialized from dead reckoning — wrong, but a good enough starting point for the optimizer later.

Step 4Loop closure — add global consistency

With only a chain, there is no redundant constraint: optimizing it is equivalent to chaining the sequential measurements, so their errors still accumulate. A loop closure adds an independent long-range constraint such as “I am back where I started”. Other global information — re-observed landmarks, GNSS, or map priors — can play the same role. Loop closure has two halves:

• Detection: propose candidate revisits. Our code supplies the pair $(0,63)$ directly so we can focus on the back-end. Real systems use appearance (bag-of-words over image features as in DBoW2/ORB-SLAM, scan descriptors like Scan Context, or learned place recognition), because after heavy drift the estimate itself can't be trusted to say what is nearby.
• Verification: scan matching again! Run ICP between the candidate pair, seeded by the current estimates. Production systems test overlap, inlier count, residual geometry, and consistency with the graph before adding an edge, usually with a robust loss or switchable constraint. A low ICP residual alone is only a toy acceptance test: perceptual aliasing can produce a convincing wrong match.

The accepted toy loop edge says the last pose sits 20 cm from the first, rotated $-90°$ (the robot arrives facing down the wall it left along) — while the dead-reckoned estimate claims they are 1.3 m apart. The graph now contains a contradiction, and resolving contradictions is the back-end's job.

Step 5Optimize — nonlinear least squares on the graph

We minimize $F(\mathbf{x}) = \sum \mathbf{e}_{ij}^\top \Omega_{ij} \mathbf{e}_{ij}$ by Gauss–Newton: linearize every edge error around the current estimate, $\mathbf{e}_{ij}(\mathbf{x} + \Delta\mathbf{x}) \approx \mathbf{e}_{ij} + A_{ij}\Delta\mathbf{x}_i + B_{ij}\Delta\mathbf{x}_j$, and solve the normal equations

$$H\,\Delta\mathbf{x} = -\mathbf{b}, \qquad H = \textstyle\sum J_{ij}^\top \Omega_{ij} J_{ij}, \qquad \mathbf{b} = \textstyle\sum J_{ij}^\top \Omega_{ij}\, \mathbf{e}_{ij}.$$

Two structural facts do all the work in practice:

• $H$ is sparse — each edge touches only two poses, so it contributes a single $6{\times}6$ block pattern. Real solvers (g2o, GTSAM, Ceres) exploit this with sparse Cholesky factorization and scale to hundreds of thousands of poses.
• The problem has a gauge freedom: shifting/rotating the whole map changes nothing, so $H$ is singular until we pin one pose (the code holds pose 0 fixed and solves only for the rest).

The sequential ICP constraints already improve the mean error from 57 cm to 22 cm, but their chain still accumulates registration error. Adding one verified loop constraint reduces it to under 5 cm. The loop edge did not just fix the last pose — the optimizer redistributed the drift over the entire trajectory, because every pose is connected to every other through the chain. The printed objective is a weighted least-squares cost, but it should not be given a formal $\chi^2$ interpretation here because the ICP weights were hand-tuned rather than calibrated covariances.

Step 6Fuse scans with the optimized poses

Once the poses are corrected, we can transform each stored scan into the world frame. This simple point overlay is enough to reveal the benefit: drifted poses smear walls into double images, while optimized poses align them. A deployable mapping system still needs an explicit representation (for example an occupancy grid or surfel map), filtering, and treatment of dynamic objects.

In practiceThe same thing with g2o (Python)

Our dense np.linalg.solve is $O(n^3)$; production back-ends exploit the sparsity of $H$. A widely used choice is g2o — the framework bundled by ORB-SLAM for pose-graph optimization and bundle adjustment. The separately maintained g2o-python package wraps g2o's experimental Python branch, and the back-end of this notebook becomes a handful of lines. The common .g2o text format (VERTEX_SE2 / EDGE_SE2 lines — exactly our vertices and edges) is supported by several SLAM tools; classic 2-D benchmark datasets (INTEL, M3500, MIT) are collected in Luca Carlone's dataset page.

If you want to read a full solver in Python rather than call C++ through bindings, python-graphslam mirrors the pipeline of cells 5–7 (with analytic Jacobians for $\mathbb{R}^2$, $\mathbb{R}^3$, $SE(2)$ and $SE(3)$, plus landmark edges) and reads the same files:

Part IIiSAM — reuse the solve as the graph grows

Graph SLAM defines what to optimize; iSAM changes how that optimization is updated online. The batch solver in In [7] assembles and solves the whole linearized graph again whenever a factor is added. That is perfectly reasonable for this small notebook, but wasteful for a robot receiving one odometry or loop factor at a time. Incremental Smoothing and Mapping (iSAM) targets the same full-trajectory smoothing problem while retaining useful work from the previous solve.

To connect it directly to the normal equations above, let $S_{ij}^{\top}S_{ij}=\Omega_{ij}$, $A_{ij}=S_{ij}J_{ij}$ and $\mathbf{c}_{ij}=-S_{ij}\mathbf{e}_{ij}$. Stack the blocks from every factor to obtain the square-root least-squares problem

$$\Delta\mathbf{x}^{\star}=\arg\min_{\Delta\mathbf{x}} \lVert A\Delta\mathbf{x}-\mathbf{c}\rVert^2, \qquad H=A^{\top}A.$$

A batch solver refactorizes this system after the graph or linearization changes. The original iSAM instead maintains a sparse QR factor $R$. New factor rows are folded into $R$ with orthogonal transformations, so unchanged work is not repeated. Because loop closures create fill-in and the measurement models are nonlinear, original iSAM still performs periodic batch reordering, refactorization, and relinearization.

iSAM2: make the dependencies explicit

iSAM2 represents the same square-root factorization as a Bayes tree. Each tree clique stores a conditional $p(F_k\mid S_k)$ for frontal variables $F_k$ given separator variables $S_k$:

$$p(\Delta\mathbf{x})=\prod_k p(F_k\mid S_k).$$

When measurements arrive, iSAM2 can:

1. add the new factors and initial guesses for genuinely new variables;
2. remove the cliques affected by those factors, following their dependency paths toward the root;
3. selectively relinearize variables whose estimates moved far enough, and reorder/re-eliminate that subgraph;
4. reattach unchanged subtrees and propagate the updated solution only as far as needed.

This fluid relinearization and incremental reordering remove the scheduled whole-graph batch rebuilds required by original iSAM. The target remains the same nonlinear MAP or maximum-likelihood objective; the relinearization and partial-update thresholds control the practical accuracy–latency trade-off.

Incremental does not mean constant-time or sliding-window. A sequential edge often changes only a small newest part of a well-ordered tree, but the $x_0\leftrightarrow x_{63}$ loop closure in this notebook can awaken a much larger portion and move old poses throughout the trajectory. “Affected” refers to dependencies in the Bayes tree, not physical distance. iSAM2 is also only the back-end: it does not detect loop closures, run ICP, or reject bad factors.

The call pattern is the important part: every measurement update receives only new factors plus guesses for new variables; a loop between existing poses passes an empty Values object. Because this one loop causes a large nonlinear correction, the factor-free update() gives iSAM2 another selective relinearization pass; it does not add a second copy of the graph. calculateEstimate() then retrieves the current smoothed trajectory. GTSAM also exposes relinearization thresholds, update frequency, factor removal, and marginal queries; see the official iSAM2 guide and API contract.

Part IIIBundle adjustment — let the structure into the graph

Everything above treated the world as something to be matched, never estimated: ICP condensed two whole lidar scans into a single relative-pose constraint, the graph contained poses only, and the map was rebuilt afterwards from the optimized poses. Bundle adjustment (BA) makes the opposite choice. It keeps individual feature observations and puts the structure itself — the 3-D points — into the state vector, refining poses and points jointly.

The name is photogrammetry vocabulary: each 3-D point sends a bundle of light rays to every camera that observes it, and the solver adjusts points and cameras together until the rays agree with the images. With camera poses $T_j$, points $\mathbf{X}_i$, observed pixels $\mathbf{u}_{ij}$ and camera projection function $\pi$, BA solves

$$\{T_j, \mathbf{X}_i\}^\star \;=\; \arg\min_{T,\,\mathbf{X}} \sum_{(i,j)\in\mathcal{O}} \big(\mathbf{u}_{ij} - \pi(T_j, \mathbf{X}_i)\big)^{\!\top}\, \Omega_{ij}\, \big(\mathbf{u}_{ij} - \pi(T_j, \mathbf{X}_i)\big)$$

— the reprojection error. Put this next to the pose-graph objective at the top of the page: the shape is identical, a sum of squared, information-weighted errors over a sparse graph. Only the variables and the error function changed, so the same Gauss–Newton pattern from In [7] applies, now with different Jacobians and a more useful block elimination. (The standard reference is Triggs et al., “Bundle Adjustment — A Modern Synthesis”, 2000.)

To show the shared machinery, we run BA on the same simulated world — with the lidar swapped for a monocular camera in Flatland: a sensor that reports only the bearing of a landmark in the robot frame. A bearing is a useful 2-D analogue of a calibrated pixel coordinate — a direction, but no depth. Wall corners and midpoints play the role of feature points. Note what we give up: no ICP, no odometry factors, no explicit loop-closure factor. Bearings are all the objective contains — but the demo still receives a good odometry initialization and perfect landmark identities, assumptions a real front-end must earn.

The structure needs an initial guess too. Exactly as in structure from motion, we triangulate: each observation defines a bearing line through its (drifted, dead-reckoned) pose, and their least-squares intersection initializes the landmark. A real triangulator also checks positive depth and rejects weak, nearly parallel geometry.

The error of a single observation is scalar — with $\mathbf{d} = \mathbf{l}_k - \mathbf{p}_i$,

$$e_{ik} \;=\; \mathrm{wrap}\!\big(\operatorname{atan2}(d_y, d_x) - \theta_i - \beta_{ik}\big),$$

with a $1{\times}3$ Jacobian in the pose and a $1{\times}2$ Jacobian in the landmark. Two things change relative to the pose-graph solver of In [7]:

• The gauge is bigger. A bearing-only (monocular) problem has no absolute scale: shrink the whole world around pose 0 and every bearing stays identical. So besides translation and rotation, scale is free — a 4-dof gauge in 2-D (7-dof for a monocular camera in 3-D: rigid motion + scale). Monocular SfM fixes this by clamping the first camera plus exactly one extra number: the length of the baseline to the second camera. Our demo is lazier — it pins the first two poses outright, over-constraining the gauge (6 dof pinned where 4 would do) and clamping pose 1's heading and the baseline direction to their dead-reckoned values along the way. Those small errors, like the scale, are frozen into the solution: nothing in the bearings can ever correct them — the monocular gauge ambiguity in action.
• The Schur complement. Order the unknowns as (all poses, all landmarks) and the normal equations take the block form $$\begin{bmatrix} H_{pp} & H_{pl} \\ H_{pl}^\top & H_{ll} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{x}_p \\ \Delta\mathbf{x}_l \end{bmatrix} = -\begin{bmatrix} \mathbf{b}_p \\ \mathbf{b}_l \end{bmatrix}.$$ No observation factor touches two landmarks at once, so $H_{ll}$ is block-diagonal and can be eliminated with independent tiny solves. Eliminating the landmarks leaves the smaller reduced camera system $$S = H_{pp} - H_{pl}\, H_{ll}^{-1} H_{pl}^\top,$$ solved first for the poses, then back-substituted for the landmarks. Points commonly far outnumber cameras, so this reduction is central to scalable bundle adjustment.

Three things worth savoring in that output:

• The nominal residual degrees of freedom are $516$ scalar measurements minus $210$ free parameters ($216$ unknowns less the $6$ pinned), or $306$. The final $\chi^2 \approx 305.7$ happens to be close. That is a useful sanity check, not a formal calibration result: the model is nonlinear, the pinned poses are slightly biased, and real data violate the ideal Gaussian assumptions.
• The accuracy, 5.1 cm, matches the pose-graph result of Part I (4.6 cm) — using bearings alone in the objective, conditional on supplied landmark associations, a good odometry initialization, and the fixed first two poses.
• The drift is gone even though we never added a loop-closure edge. Re-observing the same room corner from both the first and the last poses ties the two ends of the trajectory together through the structure. In feature-based visual SLAM, a correctly associated feature track spanning the revisit provides this loop information implicitly.

The left panel shows the full BA normal matrix before elimination. With only reprojection factors, $H_{pp}$ (top-left) and $H_{ll}$ (bottom-right) are block-diagonal; the sparse off-diagonal block records which camera observes which landmark. Eliminating landmarks makes cameras that share points couple in the reduced camera matrix $S$, which is generally much less sparse than $H_{pp}$. Production solvers exploit this structure directly: Ceres calls it the SPARSE_SCHUR linear solver, g2o marks point vertices as marginalized.

Part IVStructure from motion — the offline sibling

Bundle adjustment is an optimizer, not a system. Something has to detect features, match them across images, decide which matches to trust, and produce the initial poses and points that BA polishes. Structure from motion (SfM) is the surrounding pipeline that recovers camera motion and 3-D structure from multiple images. It is usually run offline and does not require temporal order, although ordered video is a perfectly valid input. The classic incremental recipe appeared in systems such as Photo Tourism (Snavely et al., 2006), with a modern reference implementation in COLMAP (Schönberger & Frahm, 2016):

1. Features: detect keypoints and descriptors (classically SIFT) in every image — the counterpart of our simulated bearings;
2. Matching + geometric verification: match descriptors between image pairs, then keep only correspondences consistent with an epipolar geometry estimated robustly, typically with RANSAC. Isolated wrong matches are expected and rejected; a coherent false image-pair geometry is the dangerous analogue of a false loop closure;
3. Initialization: pick one strong image pair, recover its relative pose from the essential matrix, triangulate the first points (we did the miniature version: fix two poses, intersect rays);
4. Incremental growth: register each new image against the existing points (PnP), then triangulate the new points it sees;
5. Repeated bundle adjustment: local BA after registrations plus periodic global refinement keeps the growing reconstruction consistent.

Global SfM variants estimate many cameras together rather than registering them one at a time. The 2024 GLOMAP method uses rotation averaging followed by joint global positioning of cameras and points. Its standalone repository was archived in 2026 after the implementation moved into COLMAP as the global_mapper. Either way, the output is camera poses plus a sparse point cloud. That output is a common starting point for multi-view stereo, NeRFs and Gaussian splatting — when a 3DGS paper says “initialized from COLMAP”, this pipeline is what it means. It also scales to very large collections: “Building Rome in a Day” (Agarwal et al., 2009) reconstructed city-scale models from 150,000 internet photos. Modern pipelines use sparse BA solvers such as Ceres.

Part VRelated problems, shared machinery

iSAM is deliberately not a fourth column below: it is a solver strategy for a growing factor graph, not a different estimation problem. The three problems are closely related, but they are not identical: pose-graph optimization and bundle adjustment are estimation problem families, while SfM is a reconstruction pipeline that uses BA. All can be expressed with sparse factors and nonlinear least squares. With Gaussian measurement noise and no priors that is maximum likelihood; adding priors gives a MAP problem. Their shared skeleton is:

• a state vector of things to estimate — poses, points, or both;
• measurements, each touching only a couple of variables, weighted by an information matrix;
• the same solver loop: linearize, assemble sparse normal equations, Gauss–Newton or Levenberg–Marquardt, iterate;
• a gauge freedom — a global rigid transform, or a similarity transform when monocular scale is unknown — removed with priors or explicit constraints;
• sensitivity to structured outliers, handled with geometric verification and robust losses;
• shared numerical tools: g2o and GTSAM provide pose and projection factors, while Ceres is a general nonlinear least-squares framework with official pose-graph and BA examples.

What actually differs:

The relationships need one final nuance. An ICP factor summarizes many scan correspondences as one relative-pose measurement, but a pose graph is not literally BA with landmarks eliminated: exact Schur elimination creates induced camera correlations that need not reduce to fixed pairwise relative-pose factors. Pose graphs are a deliberate compact model. Likewise, visual SLAM and SfM share feature geometry and BA, but SLAM adds causal tracking, relocalization, map maintenance, and bounded latency. ORB-SLAM illustrates the overlap: it runs local BA around the current keyframe, optimizes a sparse essential pose graph after loop detection for fast global correction, and can follow with global BA over keyframes and map points.

Where to go from here

Resource	What it is
RainerKuemmerle/g2o	The influential C++ graph-optimization framework (ICRA 2011). Its README links to the separately maintained g2o-python wrapper; or use uoip/g2opy (community, older).
JeffLIrion/python-graphslam	Readable pure-Python solver (pip install graphslam), supports $SE(2)$/$SE(3)$, landmarks, .g2o I/O.
AtsushiSakai/PythonRobotics	Includes a compact graph-based SLAM example in the same didactic spirit as this page.
GTSAM iSAM2 / Pose2 SLAM / Ceres Pose Graph 2D	Official examples for incremental and batch factor-graph optimization with GTSAM, and general sparse nonlinear least squares with Ceres.
Kaess et al., “iSAM: Incremental Smoothing and Mapping” (2008)	The original incremental sparse-QR method, including periodic batch reordering and relinearization.
Kaess et al., “iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree” (2012)	The Bayes-tree formulation behind incremental reordering, fluid relinearization, and partial updates.
Grisetti et al., “A Tutorial on Graph-Based SLAM” (2010)	The tutorial this notebook's derivation follows — error function, Jacobians, sparse structure, gauge.
Besl & McKay, “A Method for Registration of 3-D Shapes” (1992)	The original ICP paper behind the scan-matching step.
Triggs et al., “Bundle Adjustment — A Modern Synthesis” (2000)	The BA bible behind Part III: projection models, the Schur complement, gauge freedom, and why sparsity is everything.
COLMAP and PyCOLMAP	The incremental and global SfM pipelines of Part IV; the official Python bindings expose feature extraction, matching, mapping, and BA.

Natural extensions include robust losses and switchable constraints on loop edges, point-to-line/plane ICP and NDT for better matching, landmark vertices alongside poses (Part III did the monocular version), and comparing full-history iSAM2 with fixed-lag smoothing when memory and latency must be bounded. The visual SLAM note on this site shows the camera version of the same story — Part III's bearings become real pixels, feature tracking supplies associations, and triangulation intersects camera rays — while the Gaussian splatting note picks up exactly where Part IV's COLMAP output ends.