---
kind: concept
slug: pose-graph-optimization
title: Pose-graph optimization
summary: >-
  Pose-graph optimization adjusts robot poses so that odometry, loop-closure and sensor constraints agree. Kimera (2101.06894) gives a pose-only back-end, a joint pose and mesh graph and per-person graphs. A continuum-robot sliding-window factor graph (2510.26623) is a related form without loop closures. Timings are author-reported on different hardware and problems.
tags: [slam, optimization, loop-closure, sliding-window, continuum-robots]
papers: ["2101.06894", "2510.26623"]
related: [simultaneous-localization-and-mapping, visual-inertial-odometry, 3d-scene-graph, continuous-time-state-estimation, bundle-adjustment]
status: draft
updated_at: 2026-11-01
---

## Overview

In Kimera, a pose graph has robot poses as vertices. Odometry links consecutive
poses, and loop closures link a pose to an earlier pose at a revisited place
[arxiv:2101.06894 §3.4]. Loop closures pass an outlier filter before they enter
the graph [arxiv:2101.06894 §3.4]. Optimising the graph moves all poses at once
to satisfy the constraints.

The visual-inertial back-end is a different structure. It is a factor graph
solved with iSAM2 in GTSAM, where 3-D feature points are eliminated from the
state instead of being kept as vertices [arxiv:2101.06894 §3.1]. It is covered
by [Visual-inertial odometry](/concepts/visual-inertial-odometry), not here.

A second paper uses the same kind of factor-graph MAP solve, but over the state
of a continuum robot and over a short sliding window of time, with no loop
closures [arxiv:2510.26623 §3.1] [arxiv:2510.26623 §3.2]. It is listed as a
variant below because it shares the graph and the solver, not because its
problem is the same.

## Variants

### Pose-only robust back-end (Kimera-RPGO)

Only robot poses are variables. The graph holds odometry edges and the loop
closures that survive outlier rejection. The paper describes it as the same
robust front-end with a pose-graph optimiser alone [arxiv:2101.06894 §3.4].
Its reported cost is up to 50 ms per update on the paper's platform
[arxiv:2101.06894 §4.8]. Card: [Kimera](/papers/2101.06894).

### Joint pose and mesh deformation graph (Kimera-PGMO)

Robot poses and a simplified mesh share one deformation graph. Each mesh
vertex carries an SE(3) transform, and mesh vertices are clustered on a 1 to
4 m voxel grid [arxiv:2101.06894 §3.4]. The objective has three terms: odometry
and loop-closure residuals, rigidity between mesh vertices, and rigidity between
each pose and the mesh vertices its camera sees [arxiv:2101.06894 eq. 1]. The
paper rewrites this as a pose graph over a combined vertex set and solves it
with GTSAM [arxiv:2101.06894 eq. 5] [arxiv:2101.06894 eq. 6]. On EuRoC V1_01 the
graph has 728 pose and 1031 mesh nodes [arxiv:2101.06894 §3.4]. Reported cost
is up to 140 ms per update [arxiv:2101.06894 §4.8]. The paper calls it a
generalisation of Kimera-RPGO.

### Per-person pose graph for humans

Each detected person gets a graph of pelvis positions. Consecutive poses are
joined by a zero-velocity prior with a permissive noise model, and a human
mesh regressor's estimate enters as a prior factor [arxiv:2101.06894 §3.5].
A new detection joins the nearest graph only if three checks pass: centroid
speed under 3 m/s, joint displacement under 3 m, and a mean difference of
under 0.1 over the 8 shape parameters [arxiv:2101.06894 §3.5]. Graphs with
fewer than 10 nodes are pruned [arxiv:2101.06894 §3.5]. In the paper's simulated
office with six humans, single-image localisation error is 1.87 m. With the
pose graph and shape check it is 0.63 m [arxiv:2101.06894 tab. 8].

### Sliding-window factor graph with marginalisation (continuum robot)

The variables are the pose, velocity and strain of a continuum robot at each
arc-length point and time node, not robot poses joined by odometry and loop
closures. Factors are motion, spatial, boundary and measurement terms. The
window is solved by Gauss-Newton. When the window grows past its length, the
oldest time step is folded into a prior with a Schur complement
[arxiv:2510.26623 §3.1] [arxiv:2510.26623 §3.2] [arxiv:2510.26623 alg. 1].
Author-reported tip position RMSE on a tendon-driven robot is 1.88% of robot
length on Out-of-Bounds, against 2.04% for a 10 s batch smoother. On the other
four trajectories it is within 0.01 points of the batch [arxiv:2510.26623 tab. 1].
Card: [continuum-robot sliding-window estimator](/papers/2510.26623).

## When to use which

| Situation | Use | Why |
|---|---|---|
| Only the trajectory must be corrected at a lower cost | Pose-only back-end | Smaller graph; up to 50 ms per update on the paper's platform [arxiv:2101.06894 §4.8] |
| A dense mesh must follow loop corrections | Joint deformation graph | Mesh vertices are variables; up to 140 ms per update [arxiv:2101.06894 §3.4] [arxiv:2101.06894 §4.8] |
| People move through the scene and must be tracked over time | Per-person pose graph | Prior and association checks are described in detail, but the thresholds come from the paper's own experiments [arxiv:2101.06894 §3.5] |
| Online state with no loop closures and a fixed cost per step | Sliding-window factor graph | Author-reported 8.7 to 10.2 ms per step for a 0.1 s window, against about 1.36 to 1.38 s per step for a 10 s batch, on one CPU [arxiv:2510.26623 tab. 1] |
| Several robots build one map | Not covered | The paper's outlier filter is described for a single robot [arxiv:2101.06894 §3.4] |

The timings above are the papers' own, on the CPUs they name, and they compare
configurations of one system each. They do not show how other pose-graph
solvers would perform. The sliding-window row is a different problem from the
Kimera rows, so the two sets of numbers are not comparable.

## Implementation sketch

Batch form of the loop-closure back-end. The paper checks each new loop
against earlier loops incrementally, adding one row and one column to the
consistency matrix [arxiv:2101.06894 §3.4].

```pseudocode
# Pose graph back-end for loop closures  [arxiv:2101.06894 §3.4]
# Vertices: robot poses. Edges: odometry and accepted loop closures.

def pose_graph_back_end(odometry, loops):
    # Odometry chains consecutive poses; it is always kept  [arxiv:2101.06894 §3.4]
    graph = PoseGraph(odometry)
    # A loop must close the odometry cycle to be a candidate  [arxiv:2101.06894 §3.4]
    candidates = [l for l in loops if closes_odometry_cycle(l, odometry)]
    # Keep the largest set of loops that agree with each other  [arxiv:2101.06894 §3.4]
    inliers = maximum_clique(consistency_graph(candidates))
    graph.add_edges(inliers)
    # Solve for all poses at once  [arxiv:2101.06894 §3.4]
    return graph.optimise()
```

Notes:

- The paper gives k = 4 deformation neighbours and a 1 to 4 m voxel size for
  mesh simplification [arxiv:2101.06894 §3.4].
- It does not state the chi-squared significance level of its consistency
  test, nor the weights ω_R and ω_t in eq. 1 [arxiv:2101.06894 §3.4].
- The loop-closure threshold α = 0.001 is the setting used for the paper's
  VIO comparison table, not a general recommendation [arxiv:2101.06894 §4.2].
- The sliding-window variant marginalises old states rather than dropping them,
  and it has no outlier filter or loop search. Its convergence test and noise
  values are not stated in the paper, so a port must set them from the code or
  from the sensors [arxiv:2510.26623 §3.1] [arxiv:2510.26623 §3.4.1].

## Contradictions

No conflicting numbers between the two cards. They solve different problems, so
their timings and accuracy figures are not directly comparable.

Within Kimera, §3.4 presents the joint graph as a generalisation of the
pose-only back-end, and it also notes that the rewrite into a pose graph matches
the original deformation graph only when rotations are used instead of affine
transforms [arxiv:2101.06894 §3.4]. Treat the equivalence as the paper's own
claim.

Within the sliding-window card, the paper's filter row in Table 1 is not tied to
a specific code path in the authors' repository [arxiv:2510.26623 §4]. This
affects that card's filter baseline, not the sliding-window numbers on this page.

## Papers

- [Kimera: from SLAM to Spatial Perception with 3D Dynamic Scene Graphs](/papers/2101.06894): a pose-only robust back-end, a joint pose and mesh deformation graph, and per-person pose graphs for humans.
- [Continuum-robot sliding-window estimator (2510.26623)](/papers/2510.26623): a sliding-window factor graph over a continuum robot's pose, velocity and strain, with Schur-complement marginalisation and no loop closures.