Pose-graph optimization

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 2101.06894 §3.4. Loop closures pass an outlier filter before they enter the graph 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 2101.06894 §3.1. It is covered by 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 2510.26623 §3.1 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 2101.06894 §3.4. Its reported cost is up to 50 ms per update on the paper's platform 2101.06894 §4.8. Card: Kimera.

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 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 2101.06894 eq. 1. The paper rewrites this as a pose graph over a combined vertex set and solves it with GTSAM 2101.06894 eq. 5 2101.06894 eq. 6. On EuRoC V1_01 the graph has 728 pose and 1031 mesh nodes 2101.06894 §3.4. Reported cost is up to 140 ms per update 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 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 2101.06894 §3.5. Graphs with fewer than 10 nodes are pruned 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 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 2510.26623 §3.1 2510.26623 §3.2 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 2510.26623 tab. 1. Card: continuum-robot sliding-window estimator.

When to use which

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 2101.06894 §3.4.

# 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 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 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 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 2510.26623 §3.1 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 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 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: 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): a sliding-window factor graph over a continuum robot's pose, velocity and strain, with Schur-complement marginalisation and no loop closures.