Pose Graph Optimization

Pose graph optimization estimates a sequence of poses, the positions and orientations of a robot, a vehicle or a camera, from measurements of the relative pose between pairs of them. The measurements come from odometry between consecutive poses and from loop closures, which register two views taken at the same place at different times; the unknowns are rigid motions in $SE(2)$ or $SE(3)$, or similarity transformations in $Sim(3)$ for a single camera whose scale drifts. The estimate is the set of poses that best agrees with all the measurements, found by nonlinear least squares. It is the back-end of graph-based SLAM, one of the estimators of robot perception, where it removes the drift that accumulates in visual odometry, and the same problem appears in computer vision as estimating camera motions from pairwise reconstructions and in sensor networks; Rosen et al. (2019) call the general form synchronization over the special Euclidean group.

The SLAM article states the objective and solves a small planar loop. This article covers the error functions on the different groups, Gauss–Newton on manifolds, and the three problems that decide whether the optimizer returns a usable map: a starting point far from the optimum, wrong loop closures, and the non-convexity that rotations bring. The problem is a factor graph with only pose variables and binary factors, so that article’s sparse linear algebra, ordering and incremental solvers apply unchanged.

Problem

A sensor visits poses $x_1, \dots, x_n$. Each measurement $z_{ij}$ is a noisy observation of the pose of $x_j$ expressed in the frame of $x_i$, $x_i^{-1} x_j$, with an information (inverse covariance) matrix $\Omega_{ij}$. Grisetti et al. (2010), who credit the graph formulation to Lu and Milios (1997), call these virtual measurements, and write that it took several years to become popular because solving it with standard techniques was costly, until insights into its structure and sparse linear algebra made it efficient; Olson, Leonard and Teller (2006) call Lu and Milios’s brute-force implementation impractical. In this setting a front-end has already turned the raw sensor data, laser scans or images, into relative poses, and the back-end sees only the graph. With Gaussian noise and known data association, the maximum-likelihood trajectory minimizes

$$F(x) = \sum_{\langle i,j \rangle \in \mathcal{C}} e_{ij}(x_i, x_j)^\top \, \Omega_{ij} \, e_{ij}(x_i, x_j),$$

where $e_{ij}$ measures the discrepancy between $z_{ij}$ and the relative pose predicted by the current estimate (Grisetti et al., 2010, eqs. 3–5). $F$ depends only on relative poses, so it is unchanged when the whole trajectory is moved rigidly, and one pose is fixed to remove this gauge freedom.

Three properties make this least-squares problem harder than it looks. The unknowns are rotations, which do not form a vector space. The objective is non-convex in them, so iterative solvers can stop at local minima far from the maximum-likelihood estimate (Rosen et al., 2019, Fig. 1, shows several on a parking-garage graph). And the measurements are not all correct: a place recognizer that confuses two similar places adds a loop closure between poses that are far apart, and Sünderhauf and Protzel (2012) describe how least-squares back-ends assume that the front-end’s data association is right.

Inputs and Outputs

• Input: the graph: for every edge $(i, j)$, a relative pose $z_{ij}$ and an information matrix $\Omega_{ij}$; usually labels saying which edges are odometry and which are loop closures; an initial guess for every pose.
• Output: the estimated poses. Grisetti et al.’s algorithm also returns the information matrix $H^\ast$ at the solution, the Gaussian approximation of the posterior, from whose inverse a front-end can read the marginal covariances it needs to search for loop closures (Grisetti et al., 2010, §IV-B and §V-A). Robust back-ends also return a weight or switch value per loop closure, which says how far each was trusted.

Intuition

Picture the poses as nodes joined by springs, one per measurement, whose rest pose is the measured relative pose and whose stiffness is the information matrix. Odometry springs form a chain, and a loop closure joins the end of a long chain to its beginning; the optimizer spreads the gap over all springs according to their stiffness, as the SLAM article’s example shows for one loop.

Two things go wrong with the springs picture. First, a spring that bends a pose’s orientation moves every later pose in the chain, and the further the chain the larger the motion, so the energy is not a nice bowl in the orientations: Carlone et al. (2015) put it plainly: “rotations are the actual reason why SLAM is a difficult problem”. Second, a wrong loop closure is a spring attached to the wrong place. Least squares will stretch the whole graph to satisfy it, distributing its error as faithfully as that of any good spring.

The remedies follow from these two observations. Start the optimizer close to the answer, by estimating the orientations first with a method that does not need an initial guess. And give each loop closure a way to stop pulling when it disagrees too much with the rest of the graph, either by a robust cost or by an explicit variable that switches it off.

Algorithm

The standard batch procedure, following Grisetti et al. (2010, Algorithms 1 and 2) with the initialization and robust steps described below:

1. Build the graph. One node per pose; one edge per odometry measurement and per accepted loop closure, each with $z_{ij}$ and $\Omega_{ij}$.
2. Initialize. Either chain the odometry from the first pose, or estimate all rotations first (chordal relaxation or another rotation-averaging method), then the translations by linear least squares.
3. Linearize. At the current estimate, compute every edge error $e_{ij}$ and its Jacobians $A_{ij}$ and $B_{ij}$ with respect to small perturbations of $x_i$ and $x_j$ in local coordinates.
4. Reweight (robust back-ends only). From each loop closure’s current error, compute its weight (a robust kernel, dynamic covariance scaling or a graduated non-convexity weight), or include its switch variable among the unknowns.
5. Build and solve the normal equations. Accumulate $H = \sum J_{ij}^\top \Omega_{ij} J_{ij}$ and $b = \sum J_{ij}^\top \Omega_{ij} e_{ij}$ block by block, fix the first pose, and solve $H \Delta = -b$ by sparse Cholesky factorization (with damping for Levenberg–Marquardt).
6. Update on the manifold. Apply each pose’s increment through the group operation, $x_i \leftarrow x_i \boxplus \Delta_i$.
7. Iterate steps 3–6 until the cost stops decreasing; for graduated non-convexity, repeat the whole solve while the kernel is made gradually less convex.
8. Report the poses, optionally $H^\ast$ for uncertainty, and the loop closures that were down-weighted.

Mathematical Formulation

Relative-pose constraints on SE(2), SE(3) and Sim(3)

Write a pose as $x_i = (R_i, t_i)$, the rotation and position of the body in the world frame (a body-to-world transform, $T_{wc}$ in the notation of camera pose, which explains why the direction must be stated), so that $x_i^{-1} x_j = (R_i^\top R_j,\; R_i^\top (t_j - t_i))$. A measurement $z_{ij} = (R_{ij}, t_{ij})$ predicts exactly this, and the standard error compares the two in the measurement’s frame:

$$e_{ij} = \operatorname{Log}\!\left( z_{ij}^{-1} \, x_i^{-1} x_j \right)^{\vee},$$

the logarithm of the discrepancy transformation, which is zero when the measurement is satisfied. A common simplification keeps the rotation part, $\operatorname{Log}(R_{ij}^\top R_i^\top R_j)$, and replaces the translation part by $R_{ij}^\top\big(R_i^\top(t_j - t_i) - t_{ij}\big)$; the two agree to first order in the size of the discrepancy. Carlone et al. (2015, eqs. 1–2) write the problem with a distance between the measured and predicted rotations and the Euclidean distance between the translations. The geometric transformations article describes the groups, their Lie algebras and their exponential maps, and rotation representations the logarithm of a rotation and its numerical care.

• $SE(2)$, three degrees of freedom: planar robots with wheel odometry and 2D laser scan matching. The rotation is a single angle, so Grisetti et al. (2010, §V-A) note that planar poses need no over-parametrization and gain nothing from the manifold machinery below, apart from normalizing angles.
• $SE(3)$, six degrees of freedom: 3D laser mapping, stereo and RGB-D cameras, and, in Dellaert and Kaess’s (2017, §6.2) examples, quadrotors and ground vehicles on uneven terrain. The rotation needs the manifold treatment.
• $Sim(3)$, seven degrees of freedom: a single camera reconstructs the scene only up to scale, and the scale drifts. Strasdat, Montiel and Davison (2010, §IV-B) minimize residuals $\log_{Sim(3)}(\Delta S_{ij} S_i S_j^{-1})$ in the 7-dimensional Lie algebra, with relative constraints defined as $\Delta T_{ij} = T_j T_i^{-1}$, the reverse composition of the one above; geometric transformations describes their conversion to and from similarities and the scale gauge, and ORB-SLAM the same construction in its Essential Graph.

Other parametrizations reduce the problem further. A visual-inertial system observes roll and pitch through gravity, and visual odometry describes VINS-Mono’s pose graph over the remaining four degrees of freedom.

Gauss–Newton and Levenberg–Marquardt on the manifold

Linearizing each error around the current guess $\breve{x}$, $e_{ij}(\breve{x} + \Delta) \approx e_{ij} + J_{ij}\Delta$, and substituting into $F$ gives a quadratic in $\Delta$ whose minimum solves

$$H \Delta^\ast = -b, \qquad H = \sum_{\langle i,j\rangle} J_{ij}^\top \Omega_{ij} J_{ij}, \quad b = \sum_{\langle i,j\rangle} J_{ij}^\top \Omega_{ij} e_{ij}$$

(Grisetti et al., 2010, eqs. 6–15). Because $e_{ij}$ depends on two poses only, $J_{ij}$ has two nonzero blocks, $A_{ij}$ for $x_i$ and $B_{ij}$ for $x_j$, and each edge adds to four blocks of $H$: $H_{ii}$, $H_{ij}$, $H_{ji}$ and $H_{jj}$. The number of nonzero blocks of $H$ is therefore the number of nodes plus twice the number of edges, and $H$ is factored by sparse Cholesky. Grisetti et al. fix the first pose by adding the identity to its diagonal block, $H_{11} \mathrel{+}= I$; removing its block row and column is the alternative they mention.

The update $\breve{x} + \Delta$ is not defined for rotations. Euler angles are minimal but have singularities, and an over-parametrization such as a rotation matrix or a quaternion leaves the group when a vector is added to it. Grisetti et al. (2010, §IV-C) therefore compute the increment in a minimal local parametrization around the current estimate and apply it with an operator $\boxplus$:

$$\tilde{e}_{ij}(\Delta\tilde{x}_i, \Delta\tilde{x}_j) = e_{ij}(\breve{x}_i \boxplus \Delta\tilde{x}_i,\; \breve{x}_j \boxplus \Delta\tilde{x}_j), \qquad x^\ast = \breve{x} \boxplus \Delta\tilde{x}^\ast .$$

The Jacobians with respect to the local increments are the ordinary Jacobians times the derivative of $\boxplus$ at zero, $\tilde{A}_{ij} = A_{ij} M_i$, $\tilde{B}_{ij} = B_{ij} M_j$, so the linear system keeps its sparsity and size; for 3D poses they use the vector part of a unit quaternion as the local rotation increment. Dellaert and Kaess (2017, §6.2.2) use the exponential map, $x_0 \oplus \xi = x_0 \exp \hat{\xi}$ with $\xi \in \mathbb{R}^3$ for $SE(2)$ and $\mathbb{R}^6$ for $SE(3)$. Which side the perturbation is applied on, and in which order rotation and translation are stacked, are conventions that differ between libraries; they change the Jacobians but not the solution.

Levenberg–Marquardt adds damping, $(H + \lambda D)\Delta = -b$; the nonlinear least squares article derives both methods. Powell’s dog-leg, a trust-region method combining the Gauss–Newton and gradient steps, is what Rosen et al. (2019) used as their state-of-the-art local baseline, from GTSAM.

On a simulated 3D graph of a robot moving on the surface of a sphere, with large measurement errors and an odometry initialization, Gauss–Newton with Euler angles first reduced the error and then diverged, while the manifold version converged to the right solution (Grisetti et al., 2010, §V-B, Figs. 11–12). The authors note that the parametrization mainly affects convergence, not the cost of an iteration.

Why rotations make it hard

Carlone et al. (2015, §II-A) make three observations about the 3D objective. If the rotations were known, $\hat{R}_i$, it would reduce to

$$\min_{\{t_i\}} \sum_{(i,j)} \big\lVert t_j - t_i - \hat{R}_i t_{ij} \big\rVert^2 ,$$

a linear least-squares problem with a unique solution. Translations enter the residuals linearly, so their initial guess is irrelevant. And in common SLAM problems the translation terms have a minor influence on the rotation estimate, so a good rotation estimate can be found from the rotation measurements alone:

$$\min_{\{R_i\} \subset SO(3)} \sum_{(i,j)} d\big(R_{ij},\, R_i^\top R_j\big)^2 .$$

This rotation-only problem is rotation averaging (or rotation synchronization). Carlone et al. compare three choices of the distance $d$ between rotations: the angle of $R_a^\top R_b$ (the geodesic distance), the chordal distance $\lVert R_a - R_b \rVert_F = 2\sqrt{2}\sin(\theta/2)$, and a quaternion distance $2\sin(\theta/4)$, which agree up to constants for small angles.

Initialization: chordal relaxation and rotation averaging

The chordal relaxation, which Carlone et al. (2015, §III-B) take from Martinec and Pajdla (2007), uses the chordal distance. The objective $\sum \lVert R_i R_{ij} - R_j \rVert_F^2$ is quadratic in the entries of the matrices $R_i$. Dropping the constraint that each $R_i$ be a rotation leaves a linear least-squares problem in the rows of the matrices; each resulting $3 \times 3$ matrix $M_i$ is then replaced by the nearest rotation in the Frobenius norm, from the singular value decomposition $M_i = U \Sigma V^\top$,

$$R_i^\star = U \operatorname{diag}\!\big(1, 1, \det(U V^\top)\big) V^\top .$$

The unconstrained problem is homogeneous, with the all-zero solution, because a global rotation is unobservable; Carlone et al. fix the first rotation to the identity (Remark 1), whereas Martinec and Pajdla took the best three linearly independent least-squares solutions from a sparse eigenvalue solver. Martinec and Pajdla designed the method to register camera rotations in multiview reconstruction, after a quaternion formulation whose unequal quaternion lengths had given some relative rotations too little weight. With the rotations fixed, the translations follow from the linear problem above, and the result initializes Gauss–Newton. Carlone et al. also extend the relaxation with measured gravity directions, which add linear terms on the last row of each rotation.

Carlone et al. (2015) survey four more rotation estimators: the closed-form solution for a graph with a single loop, which distributes the rotation that the loop fails to close equally among its edges; Govindu’s quaternion relaxation, with the analysis of Hartley et al., which must first resolve the sign ambiguity of each quaternion; a semidefinite programming relaxation due to Fredriksson and Olsson; and Tron and Vidal’s gradient method. In their simulated torus and cube scenarios with growing rotation noise, Gauss–Newton from odometry had the lowest breakdown point, the quaternion and semidefinite relaxations were slightly more resilient but had larger errors for rotation noise above 0.15 rad, and only the chordal relaxation and the gradient method degraded gracefully. The semidefinite solver produced no solution for the 1,000-node cube, and on their single-loop scenario the semidefinite and gradient methods needed about three orders of magnitude more CPU time than the chordal, quaternion and single-loop methods. They released the chordal and gradient initializations in GTSAM, where InitializePose3 provides both; for planar graphs GTSAM’s lago.h implements LAGO, a linear approximation by Carlone, Aragues, Castellanos and Bona that also estimates orientations first.

Other answers to a poor initial guess change the optimizer instead of the starting point. Olson, Leonard and Teller (2006) apply a variant of stochastic gradient descent, one constraint at a time, to an incremental pose state, the differences between successive poses, so that correcting one constraint moves many poses at once, at a cost of $O(\log N)$ per update. They report that it recovers the large-scale shape of a pose graph quickly from a badly corrupted initial estimate but generally does not reach the exact minimum, and suggest using it to bootstrap another solver. Carlone et al. (2015) note that methods with a larger basin of convergence, including Levenberg–Marquardt, stochastic gradient descent and robust kernels, are usually slow because of their conservative updates, which motivated computing a good initial guess instead.

Robust kernels

A single wrong loop closure has an unbounded influence on a least-squares estimate. The nonlinear least squares article introduces M-estimators such as the Huber and Cauchy losses and their solution by iteratively reweighted least squares; in a pose graph each iteration scales an edge’s information matrix by a weight computed from its current squared error $e_{ij}^\top \Omega_{ij} e_{ij}$. Dellaert and Kaess (2017, §7.4) report that GTSAM, iSAM and g2o all offer such robust error functions, that Huber and pseudo-Huber are commonly used, and that the Tukey biweight and the Cauchy function are less recommended.

Robust kernels have two limits in pose graphs. A convex kernel such as Huber keeps a bounded but constant pull from every outlier: Sünderhauf and Protzel (2012, Fig. 4) found the Huber-supported least-squares back-end unable to cope with their added false loop closures. A kernel that flattens out judges an edge by its error at the current estimate, so it depends on the initialization. Sünderhauf and Protzel’s (2013) Ring example, below, shows the first half of this for max-mixtures; in our reading the converse also holds, and a wrong loop closure looks like an inlier when the starting point already agrees with it.

Switchable constraints

Sünderhauf and Protzel (2012) make the graph’s topology part of the optimization. Each loop closure that might be wrong gets a continuous switch variable $s_{ij}$, and the cost becomes

$$\sum_i \lVert f(x_i, u_i) - x_{i+1} \rVert^2_{\Sigma_i} + \sum_{ij} \big\lVert \Psi(s_{ij}) \big(f(x_i, u_{ij}) - x_j\big) \big\rVert^2_{\Lambda_{ij}} + \sum_{ij} \lVert \gamma_{ij} - s_{ij} \rVert^2_{\Xi_{ij}}$$

(their eq. 1, in their notation: odometry terms, switchable loop-closure terms and switch priors). The switch function $\Psi$ maps $s_{ij}$ to a weight in $[0, 1]$; after trying a sigmoid, whose flat regions stalled the optimizer, they recommend the linear $\Psi(s) = s$ with $0 \le s \le 1$. Driving $s_{ij}$ to zero removes the loop closure, or equivalently scales its covariance to infinity. The prior, with mean $\gamma_{ij} = 1$ and variance $\Xi_{ij}$, makes switching off cost something; $\Xi_{ij}$ is the method’s only free parameter. On the Manhattan dataset with 1, 10 and 100 random outliers, the relative pose error was lowest and nearly constant for $\Xi$ between 0.3 and 1.5, so they set $\Xi = 1$ for all their experiments. The odometry terms are unchanged: only loop closures are switchable.

Dynamic covariance scaling

Agarwal, Tipaldi, Spinello, Stachniss and Burgard (2013) observed that at a stationary point of the switchable-constraints cost each switch variable has a closed-form value. Writing $\chi^2_l$ for a loop closure’s squared error and $\Phi = \Xi^{-1}$, the derivative with respect to $s$ vanishes at $s = \Phi / (\chi^2_l + \Phi)$. From the bound this implies on the cost, they choose

$$s = \min\!\left(1,\; \frac{2\Phi}{\Phi + \chi_l^2}\right)$$

and scale the loop closure’s information matrix by $s^2$ (their eqs. 8–15). This dynamic covariance scaling (DCS) needs no extra variables, so it can be added to an existing back-end as a robust kernel. The authors relate it to iteratively reweighted least squares and note that with $\Phi = 1$ and without the cap at 1 its weight is similar to that of the Geman–McClure M-estimator, $w(x) = 1/(1 + x^2)^2$. Following Sünderhauf and Protzel’s $\Xi = 1$, they used $\Phi = 1$ unless stated otherwise. GTSAM 4.3.0 implements DCS as a loss function whose weight is $1$ for $x^2 \le c$ and $4c^2/(c + x^2)^2$ beyond, the square of the scale above.

Graduated non-convexity

Graduated non-convexity (GNC) attacks the dependence on the starting point directly: it starts with a convex surrogate of a robust cost and makes it gradually closer to the true, non-convex one, re-solving at each step. The GNC paper of Yang, Antonante, Tzoumas and Carlone (2020) was not read for this article; the description here follows the authors’ MATLAB implementation and GTSAM’s, which cite it. For the truncated least squares cost, which counts an edge’s squared error up to a threshold $\bar{c}^2$ and is constant beyond, the implementation alternates between a weighted least-squares solve and a weight update, with each edge’s weight

$$w = \begin{cases} 0, & r^2 \ge \frac{\mu + 1}{\mu}\, \bar{c}^2 \\ 1, & r^2 \le \frac{\mu}{\mu + 1}\, \bar{c}^2 \\ \bar{c}\sqrt{\mu(\mu + 1)}/r - \mu, & \text{otherwise} \end{cases}$$

where $r^2$ is the edge’s squared residual. The control parameter $\mu$ starts small, computed from the largest residual of the unweighted solution so that no edge starts at weight zero, and is multiplied by 1.4 after each solve until all weights are 0 or 1 (gnc.m, gncWeightsUpdate.m); gnc.m calls a non-minimal solver, which needs no starting point. Measurements known to be inliers, such as odometry, can keep weight 1. GTSAM 4.3.0’s GncOptimizer offers the truncated least squares and Geman–McClure losses with the same default step of 1.4 and, by default, an inlier threshold from the 0.99 quantile of the chi-square distribution with the measurement’s dimension. Its weight is the formula above written in a normalized parameter $\mu/(\mu + 1)$ (LossFunctions.cpp), but it differs from gnc.m in two places: $\mu$ starts from the residuals at the initial values rather than at the unweighted solution, and each weighted solve restarts from the initial values.

Certifiable optimization: SE-Sync

All the methods above are local searches. Rosen, Carlone, Bandeira and Leonard (2019) model rotation noise with an isotropic Langevin distribution and translation noise as Gaussian, which makes the maximum-likelihood problem

$$\min_{t_i \in \mathbb{R}^d,\; R_i \in SO(d)} \sum_{(i,j)} \kappa_{ij} \big\lVert R_j - R_i \tilde{R}_{ij} \big\rVert_F^2 + \tau_{ij} \big\lVert t_j - t_i - R_i \tilde{t}_{ij} \big\rVert_2^2$$

(their Problem 1), a chordal cost on the rotations with precisions $\kappa_{ij}$ and $\tau_{ij}$. For fixed rotations it is quadratic in the translations, which are eliminated in closed form, leaving a rotation-only problem $\min \operatorname{tr}(\tilde{Q} R^\top R)$. Relaxing $SO(d)$ to $O(d)$ and taking the Lagrangian dual gives a convex semidefinite program. Their Proposition 2 states that if the data matrix $\tilde{Q}$ is close enough to its noiseless value (in spectral norm), the semidefinite program has a unique solution, which factors into the globally optimal rotations; and when that happens, the optimality of the recovered estimate can be verified afterwards, which is what they mean by certifiably correct.

SE-Sync solves the semidefinite program at scale through a low-rank factorization (after Burer and Monteiro), a Riemannian truncated-Newton trust-region method that needs only sparse matrix–vector products, a “Riemannian staircase” that raises the rank until a suitable critical point is found, and a rounding step. Without a good initialization it still reaches the global optimum, though a chordal initialization saves time. On their simulated cube scenario (1,000 poses by default, 50 realizations per setting), the relaxation was exact for rotation noise up to an RMS angular error of about 20°, which they describe as roughly an order of magnitude more than typical robotics and vision sensors (Rosen et al., 2019, §6.1).

Parameters

• Information matrices $\Omega_{ij}$. They set the relative weight of rotation and translation and of each edge. Robust methods compare each error to a threshold in these whitened units, so a mis-scaled covariance is read as an outlier or hides one.
• Initialization. Odometry chaining, a spanning tree, or a rotation-first estimate such as the chordal relaxation (Carlone et al., 2015).
• Optimizer. Gauss–Newton, Levenberg–Marquardt with its damping schedule, or dog-leg; the stopping rule matters: Carlone et al. note that g2o stops after 5 iterations by default, whereas GTSAM’s criterion is based on the cost.
• Kernel width. The threshold of Huber or Cauchy, $\Xi$ for switchable constraints (1 in Sünderhauf and Protzel’s experiments), $\Phi$ for DCS (1 by default in Agarwal et al.), and the inlier threshold $\bar{c}^2$ and step factor (1.4) for GNC. The squared whitened error of an inlier with $k$ degrees of freedom follows a chi-square distribution with $k$ degrees of freedom when the covariances are right, so its expected value is 3 for a planar edge and 6 for a 3D one; in our reading this is why a threshold that suits one problem dimension and noise model need not suit another, and why GTSAM ties its GNC threshold to the chi-square quantile of the measurement dimension.
• Which edges are robust. In the switchable-constraints and DCS papers only loop closures are; odometry is trusted.

Complexity

One Gauss–Newton or Levenberg–Marquardt iteration costs the linearization, linear in the number of edges, plus the sparse factorization of $H$, whose cost depends on the fill-in of the elimination order; the factor graph article explains ordering heuristics and the incremental solvers iSAM and iSAM2, which update the factorization as edges arrive. On the Intel Research Lab data (1,802 nodes and 3,546 edges in their construction), Grisetti et al. (2010, §V-A) report an optimization in 100 ms on a 2.4 GHz Core 2 laptop.

The chordal initialization costs two sparse linear least-squares solves, with $9n$ and $3n$ unknowns. Switchable constraints add one variable and one prior per loop closure, which Agarwal et al. (2013) argue increases the cost of each iteration and can slow convergence; DCS, which adds none, was faster than switchable constraints in all their timing comparisons, by up to a factor of 10 on the examples of their Fig. 1. GNC repeats a full weighted solve for each value of $\mu$. Olson et al.’s stochastic gradient descent needs $O(N + M)$ memory for $N$ poses and $M$ constraints. SE-Sync caches its constant data matrices and needs no Jacobian refactorization per iteration; Rosen et al. attribute its speed to the exact Hessian and the truncated-Newton steps.

Implementation

Pose graph optimization is available in the libraries described in the factor graph article (GTSAM, g2o, Ceres). GTSAM 4.3.0 adds the pieces discussed here: robust losses including Huber, Cauchy, Geman–McClure, truncated least squares and DCS (LossFunctions.h), the GNC optimizer, and chordal and gradient-based 3D initialization (InitializePose3). The authors’ SE-Sync repository provides MATLAB and C++ implementations, a chordal initialization and a set of benchmark graphs in g2o format, including three KITTI sequences (as of November 2026). Vertigo provides switchable constraints and max-mixtures for g2o and GTSAM, with standard datasets and a script that adds false loop closures under the four policies described below (README).

Example: a wrong loop closure, robust back-ends and the initialization

The script builds a two-level parking garage whose levels are identical: the same walls, at the same places, with the same landmark identities, level 2 three metres above level 1. A forward-looking camera drives a lap of level 1, up a ramp, a full lap of level 2, down a second ramp and part of a third pass on level 1: 168 keyframes over 252 m. Each keyframe renders image measurements of the landmarks in view (pixel positions with 1-pixel noise and depth with 1% noise, as from an RGB-D camera). Every edge of the pose graph comes from these images: the landmarks two views share are back-projected and aligned with the Kabsch algorithm. Odometry edges join consecutive keyframes and ten true loop closures join every third keyframe of the third pass to the nearest keyframe of the first. A wrong loop closure joins a keyframe on level 2 to the keyframe below it on level 1: because the levels look the same, the views share landmark identities and the Kabsch fit looks valid, but it leaves out the 3 m height difference. The script tries no wrong loop, one, and a group of ten consecutive ones, the kind a front-end produces along a stretch of repeated structure. Which loops to propose is the place recognizer’s decision, simulated here.

Conventions: poses are camera-to-world, $(R_i, t_i)$ with camera axes $x$ right, $y$ down and $z$ forward; the residual stacks the rotation error $\operatorname{Log}(R_{ij}^\top R_i^\top R_j)$ and the translation error $R_{ij}^\top(R_i^\top(t_j - t_i) - t_{ij})$ of the section above, whitened by per-axis standard deviations measured from the front-end’s noise; a pose is updated by $R_i \leftarrow R_i \operatorname{Exp}(\delta\phi_i)$ on the right and $t_i \leftarrow t_i + \delta t_i$ in the world frame; the first pose is fixed by dropping its columns. Levenberg–Marquardt solves every variant, with analytic Jacobians and a sparse solver. Robust kernels act on loop closures only, as in the switchable-constraints and DCS papers; Huber and Cauchy weights are computed from the whitened error norm, and switch variables are clipped to $[0, 1]$ after each step. The chordal initialization uses every edge, wrong ones included, with equal weights, as a front-end that does not know which loops are wrong would.

The sources leave several settings open, and the script prints each both ways. Huber and Cauchy have no published width for this problem, so they run at $c = 1$ and at $c = \sqrt{\chi^2_{0.99}(6)} \approx 4.1$. DCS and switchable constraints run at the published defaults, $\Phi = 1$ (Agarwal et al., 2013, §V-A) and $\Xi = 1$ (Sünderhauf and Protzel, 2012, §IV-A), and at $\Phi = \chi^2_{0.99}(6) \approx 16.8$ (with $\Xi = 1/\Phi$, since Agarwal et al. define $\Phi = \Xi^{-1}$), the rule GTSAM 4.3.0 uses for its default GNC inlier threshold: the 0.99 quantile of the chi-square distribution with the measurement’s dimension. GNC with truncated least squares runs in two versions that differ in where the control parameter starts and where each weighted solve starts. The first follows the authors’ gnc.m: $\mu$ is set from the residuals of the unweighted least-squares solution, and, since gnc.m leaves the starting point to its solver, each solve here starts from the previous one. The second follows GTSAM 4.3.0’s GncOptimizer: the parameter is set from the loop closures’ residuals at the initial values (GTSAM also includes edges marked as known inliers), the weights from the current solution, and every solve restarts from the initial values. The second version was added after the first run, whose single GNC row followed gnc.m and kept the wrong loops; we then compared the two reference implementations, found that they differ on exactly these points, and added GTSAM’s as a second row. Besides the new function, only the first version’s label and the lines that select the function changed, and no other printed line changed. Part B removes the wrong loops, replaces every measurement by the true relative pose with rotation noise $\sigma_R$ (0.01 to 0.2 rad) and 5 cm translation noise, and counts on five graphs per level how often Gauss–Newton and Levenberg–Marquardt from each initialization reach the lowest cost any of the four found on that graph.

The script uses NumPy 2.3.5 and SciPy 1.16.3 and runs in about 9 seconds on a laptop.

# Pose graph optimization: a wrong loop closure against robust back-ends, and initialization.
import numpy as np
from scipy.spatial.transform import Rotation as Rot
from scipy.stats import chi2
import scipy.sparse as sp
from scipy.sparse.linalg import spsolve

STEP, F, W_IMG, H_IMG, RANGE = 1.5, 300.0, 640, 480, 15.0
SIG_PX, SIG_DEPTH = 1.0, 0.01
R_C, DS = 3.0, 0.01
LAP = [("S", 24), ("C",), ("S", 12), ("C",), ("S", 24), ("C",), ("S", 12), ("C",)]
seglen = lambda s: s[1] if s[0] == "S" else np.pi / 2 * R_C


# ------------------------------------------------------------------ scene
def build_scene(rng):
    plan = []
    def add(segs, z0, z1):
        L = sum(seglen(s) for s in segs); acc = 0.0
        for s in segs:
            l = seglen(s)
            plan.append((s[0], l, z0 + (z1 - z0) * acc / L, z0 + (z1 - z0) * (acc + l) / L)); acc += l
    add(LAP[:6], 0, 0); add(LAP[6:7], 0, 3); add(LAP[7:8], 3, 3); add(LAP, 3, 3)
    add(LAP[0:1], 3, 0); add(LAP[1:5], 0, 0)
    pts, heads = [], []
    x = y = th = 0.0
    for kind, l, z0, z1 in plan:
        n = int(round(l / DS))
        for m in range(n):
            pts.append((x, y, z0 + (z1 - z0) * m / n)); heads.append(th)
            x += DS * np.cos(th); y += DS * np.sin(th)
            if kind == "C":
                th += DS / R_C
    pts, heads = np.array(pts), np.array(heads)
    arc = np.r_[0, np.cumsum(np.linalg.norm(np.diff(pts, axis=0), axis=1))]
    idx = np.searchsorted(arc, np.arange(0, arc[-1], STEP))
    P, TH = pts[idx], heads[idx]
    pitch = np.arctan2(np.gradient(P[:, 2]), np.gradient(arc[idx]))
    R = np.array([cam_rot(a, b) for a, b in zip(TH, pitch)])   # world-from-camera
    t = P + [0, 0, 1.5]
    # landmarks: walls 3.5 m either side of the lap, the same on both levels
    lp, lh = [], []
    x = y = th = 0.0
    for s in LAP:
        for m in range(int(round(seglen(s) / DS))):
            lp.append((x, y)); lh.append(th)
            x += DS * np.cos(th); y += DS * np.sin(th)
            if s[0] == "C":
                th += DS / R_C
    lp, lh = np.array(lp), np.array(lh)
    nrm = np.column_stack([-np.sin(lh), np.cos(lh)])
    walls = []
    for side in (+1, -1):
        k = rng.integers(0, len(lp), int(4 * len(lp) * DS))
        walls.append(lp[k] + side * 3.5 * nrm[k])
    walls = np.vstack(walls)
    lvl1 = np.column_stack([walls, rng.uniform(0.3, 2.7, len(walls))])
    ramp, rfloor = [], []
    for k, (kind, l, z0, z1) in enumerate(plan):
        if z0 != z1:
            start = int(round(sum(p[1] for p in plan[:k]) / DS))
            sel = rng.integers(start, start + int(round(l / DS)), int(24 * l))
            side = rng.choice([-1, 1], len(sel))
            nr = np.column_stack([-np.sin(heads[sel]), np.cos(heads[sel])])
            ramp.append(np.column_stack([pts[sel, :2] + side[:, None] * 3.5 * nr,
                                         pts[sel, 2] + rng.uniform(0.3, 2.7, len(sel))]))
            rfloor.append(pts[sel, 2])
    ramp, rfloor = np.vstack(ramp), np.concatenate(rfloor)
    n1 = len(lvl1)
    scene = dict(R=R, t=t, floor=P[:, 2], plan=plan, arc=arc[idx],
                 L_xyz=np.vstack([lvl1, lvl1 + [0, 0, 3.0], ramp]),
                 L_id=np.r_[np.arange(n1), np.arange(n1), n1 + np.arange(len(ramp))],   # level 2 reuses level 1's identities
                 L_floor=np.r_[np.zeros(n1), np.full(n1, 3.0), rfloor],
                 L_ramp=np.r_[np.zeros(2 * n1, bool), np.ones(len(ramp), bool)])
    return scene


def cam_rot(th, pitch):
    h = np.array([np.cos(th) * np.cos(pitch), np.sin(th) * np.cos(pitch), np.sin(pitch)])
    r = np.cross(h, [0, 0, 1.0]); r /= np.linalg.norm(r)
    return np.column_stack([r, np.cross(h, r), h])          # camera x right, y down, z forward


def observe(sc, i, rng):
    """Image measurements of keyframe i: landmark ids, pixels with 1 px noise, depth with 1 % noise."""
    pc = (sc["L_xyz"] - sc["t"][i]) @ sc["R"][i]
    z = np.where(np.abs(pc[:, 2]) < 1e-9, 1e-9, pc[:, 2])
    u, v = F * pc[:, 0] / z + W_IMG / 2, F * pc[:, 1] / z + H_IMG / 2
    ok = (z > 0.5) & (np.linalg.norm(pc, axis=1) < RANGE) & (u >= 0) & (u < W_IMG) & (v >= 0) & (v < H_IMG)
    ok &= np.abs(sc["L_floor"] - sc["floor"][i]) < np.where(sc["L_ramp"], 1.4, 0.75)
    k = np.where(ok)[0]
    uv = np.column_stack([u[k], v[k]]) + SIG_PX * rng.normal(size=(len(k), 2))
    return sc["L_id"][k], uv, z[k] * (1 + SIG_DEPTH * rng.normal(size=len(k)))


def backproject(uv, d):
    return np.column_stack([(uv[:, 0] - W_IMG / 2) / F * d, (uv[:, 1] - H_IMG / 2) / F * d, d])


def kabsch(A, B):
    ca, cb = A.mean(0), B.mean(0)
    U, _, Vt = np.linalg.svd((A - ca).T @ (B - cb))
    R = U @ np.diag([1, 1, np.sign(np.linalg.det(U @ Vt))]) @ Vt
    return R, ca - R @ cb


def measure(obs, i, j):
    """Relative pose of keyframe j in the frame of i from landmarks with the same identity."""
    (ii, uvi, di), (jj, uvj, dj) = obs[i], obs[j]
    _, a, b = np.intersect1d(ii, jj, return_indices=True)
    assert len(a) >= 8, (i, j, len(a))
    return kabsch(backproject(uvi[a], di[a]), backproject(uvj[b], dj[b]))


# ------------------------------------------------------------------ pose graph
def hat(v):
    O = np.zeros(v.shape[:-1] + (3, 3))
    O[..., 0, 1], O[..., 0, 2], O[..., 1, 2] = -v[..., 2], v[..., 1], -v[..., 0]
    return O - np.swapaxes(O, -1, -2)


def jr_inv(phi):
    th = np.linalg.norm(phi, axis=-1)[..., None, None]
    K = hat(phi)
    small = th < 1e-6
    ths = np.where(small, 1.0, th)
    c = np.where(small, 1 / 12, 1 / ths**2 - (1 + np.cos(ths)) / (2 * ths * np.sin(ths)))
    return np.eye(3) + 0.5 * K + c * K @ K


def residuals(R, t, E, jac=False):
    """Whitened residuals e = W [Log(Rz^T Ri^T Rj); Rz^T(Ri^T(tj - ti) - tz)] for edges E."""
    I, J, Rz, tz, Wd = E["i"], E["j"], E["Rz"], E["tz"], E["w"]
    Ri, Rj = R[I], R[J]
    Ee = np.swapaxes(Rz, 1, 2) @ np.swapaxes(Ri, 1, 2) @ Rj
    phi = Rot.from_matrix(Ee).as_rotvec()
    v = np.einsum("nba,nb->na", Ri, t[J] - t[I])            # Ri^T (tj - ti)
    u = np.einsum("nba,nb->na", Rz, v - tz)
    e = np.hstack([phi, u]) * Wd
    if not jac:
        return e
    Jinv = jr_inv(phi)
    RzT, RiT = np.swapaxes(Rz, 1, 2), np.swapaxes(Ri, 1, 2)
    A = np.zeros((len(I), 6, 6)); B = np.zeros((len(I), 6, 6))
    A[:, :3, :3] = -Jinv @ np.swapaxes(Rj, 1, 2) @ Ri
    A[:, 3:, :3] = RzT @ hat(v)
    A[:, 3:, 3:] = -RzT @ RiT
    B[:, :3, :3] = Jinv
    B[:, 3:, 3:] = RzT @ RiT
    return e, A * Wd[:, :, None], B * Wd[:, :, None]


def retract(R, t, d):
    d = d.reshape(-1, 6)
    R2, t2 = R.copy(), t.copy()
    R2[1:] = R[1:] @ Rot.from_rotvec(d[:, :3]).as_matrix()
    t2[1:] = t[1:] + d[:, 3:]
    return R2, t2


# robust losses on the whitened norm x = |e| (0.5 x^2 convention), and IRLS weights
def loss_weight(kind, x2, c):
    x = np.sqrt(x2)
    if kind == "ls":
        return 0.5 * x2, np.ones_like(x2)
    if kind == "huber":
        return np.where(x <= c, 0.5 * x2, c * x - 0.5 * c * c), np.where(x <= c, 1.0, c / np.maximum(x, 1e-300))
    if kind == "cauchy":
        return 0.5 * c * c * np.log1p(x2 / c**2), 1 / (1 + x2 / c**2)
    if kind == "dcs":     # Agarwal et al. (2013): s = min(1, 2 Phi / (Phi + chi2)), weight s^2; loss as in GTSAM 4.3.0
        return np.where(x2 <= c, 0.5 * x2, 1.5 * c - 2 * c * c / (c + x2)), np.minimum(1, 2 * c / (c + x2)) ** 2
    raise ValueError(kind)


def solve(R, t, E, loop, kind="ls", c=1.0, method="lm", fixed_w=None, iters=200, switch=False):
    """Gauss-Newton or Levenberg-Marquardt on SE(3) with pose 0 fixed.
    kind: loss on loop edges (odometry stays least squares); fixed_w: given weights (GNC);
    switch: switchable constraints with linear switch function and prior variance Xi = c."""
    N, M = len(R), len(E["i"])
    nl = int(loop.sum()); li = np.where(loop)[0]
    s = np.ones(nl)
    lam = 1e-4

    def cost_and_w(R, t, s):
        e = residuals(R, t, E)
        x2 = np.sum(e**2, 1)
        rho, w = 0.5 * x2, np.ones(M)
        if fixed_w is not None:
            rho, w = 0.5 * x2 * fixed_w, fixed_w.copy()
        elif switch:
            rho = rho.copy(); rho[li] = 0.5 * s**2 * x2[li] + 0.5 * (1 - s) ** 2 / c
            w[li] = s**2
        else:
            rl, wl = loss_weight(kind, x2[li], c)
            rho[li], w[li] = rl, wl
        return rho.sum(), w, e

    F0, w, e = cost_and_w(R, t, s)
    nvar = 6 * (N - 1) + (nl if switch else 0)
    cols = np.concatenate([6 * (E["i"] - 1)[:, None] + np.arange(6), 6 * (E["j"] - 1)[:, None] + np.arange(6)], 1)
    cols = np.broadcast_to(cols[:, None, :], (M, 6, 12))
    rows = np.broadcast_to(np.arange(6 * M).reshape(M, 6, 1), (M, 6, 12))
    msk = (cols >= 0).ravel()            # pose 0 is fixed: its columns are dropped
    for it in range(iters):
        e, A, B = residuals(R, t, E, jac=True)
        scale = np.sqrt(w)               # row scaling: sqrt of IRLS / fixed weight, or the switch value s
        if switch:
            scale = np.ones(M); scale[li] = s
        vals = np.concatenate([A, B], axis=2) * scale[:, None, None]
        Jm = sp.coo_matrix((vals.ravel()[msk], (rows.ravel()[msk], cols.ravel()[msk])), shape=(6 * M, nvar))
        r = (e * scale[:, None]).ravel()
        if switch:      # d(s e)/ds = e, and the switch prior rows (1 - s)/sqrt(Xi)
            Js = sp.coo_matrix((e[li].ravel(), ((6 * li[:, None] + np.arange(6)).ravel(),
                                np.repeat(6 * (N - 1) + np.arange(nl), 6))), shape=(6 * M, nvar))
            Jp = sp.coo_matrix((-np.ones(nl) / np.sqrt(c), (np.arange(nl), 6 * (N - 1) + np.arange(nl))), shape=(nl, nvar))
            Jm = sp.vstack([Jm + Js, Jp])
            r = np.r_[r, (1 - s) / np.sqrt(c)]
        Jm = Jm.tocsr()
        H = (Jm.T @ Jm).tocsc(); g = Jm.T @ r
        while True:
            Hd = H + lam * sp.diags(H.diagonal()) if method == "lm" else H
            d = -spsolve(Hd, g)
            R2, t2 = retract(R, t, d[: 6 * (N - 1)])
            s2 = np.clip(s + d[6 * (N - 1):], 0, 1) if switch else s
            F1, w1, _ = cost_and_w(R2, t2, s2)
            if method == "gn" or F1 < F0:
                break
            lam *= 10
            if lam > 1e12:
                return R, t, s, w, F0, it
        conv = abs(F0 - F1) < 1e-10 * max(F0, 1e-12) or np.linalg.norm(d) < 1e-10
        R, t, s, w, F0 = R2, t2, s2, w1, F1
        if method == "lm":
            lam = max(lam / 10, 1e-12)
        if conv:
            break
    return R, t, s, w, F0, it + 1


def gnc_tls(R, t, E, loop, method="lm"):
    """GNC with truncated least squares on loop edges, after MIT-SPARK gnc.m / gncWeightsUpdate.m."""
    barc2 = chi2.ppf(0.99, 6)
    R, t, _, _, _, _ = solve(R, t, E, loop, method=method)
    x2 = np.sum(residuals(R, t, E) ** 2, 1)
    w = np.ones(len(x2))
    mu = 1 / (2 * x2[loop].max() / barc2 - 1)
    if mu <= 0:
        return R, t, w
    prev = 0.0
    for it in range(1000):
        r = x2[loop]
        th1, th2 = (mu + 1) / mu * barc2, mu / (mu + 1) * barc2
        wl = np.where(r >= th1, 0.0, np.where(r <= th2, 1.0, np.sqrt(barc2 * mu * (mu + 1) / np.maximum(r, 1e-300)) - mu))
        w[loop] = wl
        R, t, _, _, _, _ = solve(R, t, E, loop, method=method, fixed_w=w)
        x2 = np.sum(residuals(R, t, E) ** 2, 1)
        fc = np.sum(x2 * w)
        if abs(fc - prev) < 1e-12 or np.all((wl == 0) | (wl == 1)):
            break
        prev = fc
        mu *= 1.4
    return R, t, w


def gnc_tls_gtsam(R0, t0, E, loop, method="lm"):
    """GNC-TLS as in GTSAM 4.3.0's GncOptimizer: mu from the residuals at the initial values,
    weights from the current solution, every weighted solve restarted from the initial values."""
    barc2 = chi2.ppf(0.99, 6)
    R, t, _, _, _, _ = solve(R0, t0, E, loop, method=method)
    x20 = np.sum(residuals(R0, t0, E) ** 2, 1)[loop]
    pos = 2 * x20 - barc2 > 0
    w = np.ones(len(loop))
    if not pos.any():
        return R, t, w
    mu = np.min(barc2 / (2 * x20[pos] - barc2))
    prev = None
    for it in range(100):
        r = np.sum(residuals(R, t, E) ** 2, 1)[loop]
        th1, th2 = (mu + 1) / mu * barc2, mu / (mu + 1) * barc2
        w[loop] = np.where(r >= th1, 0.0, np.where(r <= th2, 1.0, np.sqrt(barc2 * mu * (mu + 1) / np.maximum(r, 1e-300)) - mu))
        R, t, _, _, _, _ = solve(R0, t0, E, loop, method=method, fixed_w=w)
        cost = np.sum(np.sum(residuals(R, t, E) ** 2, 1) * w)
        if np.all(np.abs(w[loop] - np.round(w[loop])) < 1e-4) or (prev is not None and abs(cost - prev) / max(prev, 1e-7) < 1e-5):
            break
        prev = cost
        mu *= 1.4
    return R, t, w


# ------------------------------------------------------------------ initializations
def init_odometry(E, N):
    R = np.zeros((N, 3, 3)); t = np.zeros((N, 3)); R[0] = np.eye(3)
    for k in np.where(E["odo"])[0]:
        i, j = E["i"][k], E["j"][k]
        R[j] = R[i] @ E["Rz"][k]; t[j] = t[i] + R[i] @ E["tz"][k]
    return R, t


def init_chordal(E, N):
    """Chordal relaxation (Martinec and Pajdla, as in Carlone et al. 2015): min sum |Ri Rz - Rj|_F^2,
    R0 = I, without orthogonality; project each to SO(3); then translations by linear least squares."""
    M = len(E["i"])
    rows, cols, vals = [], [], []
    for k in range(M):          # vec(Ri Rz) = (Rz^T kron I3) vec(Ri), column-major vec
        i, j = E["i"][k], E["j"][k]
        K = np.kron(E["Rz"][k].T, np.eye(3))
        for a in range(9):
            for b in range(9):
                if K[a, b] != 0:
                    rows.append(9 * k + a); cols.append(9 * i + b); vals.append(K[a, b])
            rows.append(9 * k + a); cols.append(9 * j + a); vals.append(-1.0)
    A = sp.coo_matrix((vals, (rows, cols)), shape=(9 * M, 9 * N)).tocsc()
    x0 = np.eye(3).ravel(order="F")
    rhs = -A[:, :9] @ x0
    Ar = A[:, 9:]
    x = spsolve((Ar.T @ Ar).tocsc(), Ar.T @ rhs)
    Rl = np.vstack([x0, x.reshape(-1, 9)])
    R = np.zeros((N, 3, 3))
    for i in range(N):
        U, _, Vt = np.linalg.svd(Rl[i].reshape(3, 3, order="F"))
        R[i] = U @ np.diag([1, 1, np.linalg.det(U @ Vt)]) @ Vt
    # translations: tj - ti = Ri tz
    rows, cols, vals, b = [], [], [], []
    for k in range(M):
        i, j = E["i"][k], E["j"][k]
        for a in range(3):
            rows += [3 * k + a, 3 * k + a]; cols += [3 * j + a, 3 * i + a]; vals += [1.0, -1.0]
        b.append(R[i] @ E["tz"][k])
    A = sp.coo_matrix((vals, (rows, cols)), shape=(3 * M, 3 * N)).tocsc()[:, 3:]
    tt = spsolve((A.T @ A).tocsc(), A.T @ np.concatenate(b))
    return R, np.vstack([np.zeros(3), tt.reshape(-1, 3)])


def ate(t_est, t_gt):
    """RMS position error after rigid (no scale) alignment."""
    A, B = t_gt - t_gt.mean(0), t_est - t_est.mean(0)
    U, _, Vt = np.linalg.svd(A.T @ B)
    Rm = U @ np.diag([1, 1, np.sign(np.linalg.det(U @ Vt))]) @ Vt
    return np.sqrt(np.mean(np.sum((B @ Rm.T - A) ** 2, 1)))


# ================================================================== experiments
rng = np.random.default_rng(1)
sc = build_scene(rng)
N = len(sc["R"]); Rg, tg = sc["R"], sc["t"]
obs = [observe(sc, i, rng) for i in range(N)]
SIG = np.r_[np.radians([0.473, 0.176, 0.272]), 0.027, 0.0536, 0.0154]   # front-end noise per axis (pilot)

def kf_of(k):           # keyframes on segment k of the route
    s0 = sum(p[1] for p in sc["plan"][:k])
    return np.where((sc["arc"] >= s0) & (sc["arc"] < s0 + sc["plan"][k][1]))[0]
pass1 = np.concatenate([kf_of(k) for k in (1, 2, 3, 4, 5)])     # level 1, first pass
pass3 = np.concatenate([kf_of(k) for k in (17, 18, 19, 20)])    # level 1, third pass
lvl2_S3, lvl1_S3 = kf_of(12), kf_of(4)                           # the same straight on levels 2 and 1
edges = [(i, i + 1, "odo") for i in range(N - 1)]
for k in pass3[::3]:                                             # true loop closures
    edges.append((pass1[np.argmin(np.linalg.norm(tg[pass1] - tg[k], axis=1))], k, "loop"))
below = lambda k: lvl1_S3[np.argmin(np.linalg.norm(tg[lvl1_S3, :2] - tg[k, :2], axis=1))]
mid = lvl2_S3[len(lvl2_S3) // 2]
scenarios = {"no wrong loop": [], "one wrong loop": [mid], "10 wrong loops": list(lvl2_S3[3:13])}

def make_E(edge_list):
    meas = [measure(obs, i, j) for i, j, _ in edge_list]
    kind = np.array([k for _, _, k in edge_list])
    return dict(i=np.array([e[0] for e in edge_list]), j=np.array([e[1] for e in edge_list]),
                Rz=np.array([m[0] for m in meas]), tz=np.array([m[1] for m in meas]),
                w=np.tile(1 / SIG, (len(edge_list), 1)), odo=kind == "odo", loop=kind != "odo", wrong=kind == "wrong")

PHI = chi2.ppf(0.99, 6)
backs = [("least squares", "ls", 1), ("Huber c=1", "huber", 1), ("Huber c=4.1", "huber", np.sqrt(PHI)),
         ("Cauchy c=1", "cauchy", 1), ("Cauchy c=4.1", "cauchy", np.sqrt(PHI)), ("DCS Phi=1", "dcs", 1),
         ("DCS Phi=16.8", "dcs", PHI), ("switch Xi=1", "sc", 1), ("switch Xi=1/16.8", "sc", 1 / PHI), ("GNC-TLS (gnc.m)", "gnc", 0),
         ("GNC-TLS (GTSAM)", "gncg", 0)]

def run(E, Ri, ti, kind, c):
    if kind in ("gnc", "gncg"):
        R, t, w = (gnc_tls if kind == "gnc" else gnc_tls_gtsam)(Ri, ti, E, E["loop"])
        return t, w
    R, t, s, w, _, _ = solve(Ri, ti, E, E["loop"], kind="ls" if kind == "sc" else kind, c=c, switch=kind == "sc")
    if kind == "sc":
        w = np.ones(len(E["i"])); w[E["loop"]] = s**2
    return t, w

print("Part A: ATE (m) and loop weights; wrong-loop weight = largest weight given to a wrong loop")
for name, wrong in scenarios.items():
    E = make_E(edges + [(below(k), k, "wrong") for k in wrong])
    true = E["loop"] & ~E["wrong"]
    inits = {"odometry": init_odometry(E, N), "chordal": init_chordal(E, N)}
    print(f"\n{name}: init ATE odometry {ate(inits['odometry'][1], tg):.2f}, chordal {ate(inits['chordal'][1], tg):.2f}")
    print("  back-end            ATE odo  ATE chord  wrong w (odo, chord)  min true w (odo, chord)")
    for bname, kind, c in backs:
        out = [run(E, *inits[k], kind, c) for k in ("odometry", "chordal")]
        ww = [f"{w[E['wrong']].max():.2f}" if E["wrong"].any() else "  - " for _, w in out]
        print(f"  {bname:18s} {ate(out[0][0], tg):7.3f} {ate(out[1][0], tg):9.3f}       {ww[0]:>5s} {ww[1]:>5s}"
              f"            {out[0][1][true].min():.2f}  {out[1][1][true].min():.2f}")

print("\nPart B: no wrong loop, rotation noise sigma_R on every edge, 5 graphs per level;")
print("runs (of 5) reaching the lowest cost found on that graph, and median relative cost gap")
SIG_T = 0.05
def make_B(sig_r, seed):
    r = np.random.default_rng(seed)
    E = make_E(edges)       # edge structure; measurements replaced below
    for k, (i, j, _) in enumerate(edges):
        E["Rz"][k] = Rg[i].T @ Rg[j] @ Rot.from_rotvec(sig_r * r.normal(size=3)).as_matrix()
        E["tz"][k] = Rg[i].T @ (tg[j] - tg[i]) + SIG_T * r.normal(size=3)
    E["w"] = np.tile(np.r_[np.full(3, 1 / sig_r), np.full(3, SIG_T ** -1)], (len(edges), 1))
    return E
combos = [(i, m) for i in ("odometry", "chordal") for m in ("gn", "lm")]
print("  sigma_R   " + "  ".join(f"{i[:4]}+{m.upper()}:hit gap" for i, m in combos))
for sig_r in (0.01, 0.05, 0.1, 0.2):
    costs = np.zeros((5, 4))
    for seed in range(5):
        E = make_B(sig_r, seed)
        inits = {"odometry": init_odometry(E, N), "chordal": init_chordal(E, N)}
        for c, (i, m) in enumerate(combos):
            costs[seed, c] = solve(*inits[i], E, E["loop"], method=m)[4]
    gap = costs / costs.min(1, keepdims=True) - 1
    print(f"  {sig_r:5.2f}     " + "  ".join(f"{(g < 1e-6).sum():9d} {np.median(g):7.1e}" for g in gap.T))

Output:

Part A: ATE (m) and loop weights; wrong-loop weight = largest weight given to a wrong loop

no wrong loop: init ATE odometry 0.90, chordal 0.41
  back-end            ATE odo  ATE chord  wrong w (odo, chord)  min true w (odo, chord)
  least squares        0.404     0.404          -     -             1.00  1.00
  Huber c=1            0.412     0.412          -     -             0.67  0.67
  Huber c=4.1          0.404     0.404          -     -             1.00  1.00
  Cauchy c=1           0.503     0.503          -     -             0.03  0.03
  Cauchy c=4.1         0.405     0.405          -     -             0.93  0.93
  DCS Phi=1            0.894     0.524          -     -             0.00  0.00
  DCS Phi=16.8         0.404     0.404          -     -             1.00  1.00
  switch Xi=1          0.895     0.526          -     -             0.00  0.00
  switch Xi=1/16.8     0.407     0.407          -     -             0.84  0.84
  GNC-TLS (gnc.m)      0.404     0.404          -     -             1.00  1.00
  GNC-TLS (GTSAM)      0.404     0.404          -     -             1.00  1.00

one wrong loop: init ATE odometry 0.90, chordal 0.99
  back-end            ATE odo  ATE chord  wrong w (odo, chord)  min true w (odo, chord)
  least squares        1.226     1.226        1.00  1.00            1.00  1.00
  Huber c=1            1.243     1.243        1.00  1.00            0.35  0.35
  Huber c=4.1          1.226     1.226        1.00  1.00            1.00  1.00
  Cauchy c=1           0.510     0.510        0.00  0.00            0.03  0.03
  Cauchy c=4.1         1.227     1.227        0.96  0.96            0.88  0.88
  DCS Phi=1            0.894     0.524        0.00  0.00            0.00  0.00
  DCS Phi=16.8         0.405     1.226        0.00  1.00            1.00  1.00
  switch Xi=1          0.895     0.526        0.00  0.00            0.00  0.00
  switch Xi=1/16.8     0.407     1.229        0.00  0.91            0.84  0.71
  GNC-TLS (gnc.m)      1.226     1.226        1.00  1.00            1.00  1.00
  GNC-TLS (GTSAM)      0.404     1.226        0.00  1.00            1.00  1.00

10 wrong loops: init ATE odometry 0.90, chordal 1.11
  back-end            ATE odo  ATE chord  wrong w (odo, chord)  min true w (odo, chord)
  least squares        1.261     1.261        1.00  1.00            1.00  1.00
  Huber c=1            1.279     1.279        1.00  1.00            0.33  0.33
  Huber c=4.1          1.261     1.261        1.00  1.00            1.00  1.00
  Cauchy c=1           1.327     1.327        0.95  0.95            0.02  0.02
  Cauchy c=4.1         1.262     1.262        1.00  1.00            0.88  0.88
  DCS Phi=1            0.894     1.336        0.00  1.00            0.00  0.00
  DCS Phi=16.8         0.496     1.261        0.00  1.00            1.00  1.00
  switch Xi=1          0.895     1.504        0.00  0.90            0.00  0.00
  switch Xi=1/16.8     1.266     1.266        0.99  0.99            0.69  0.69
  GNC-TLS (gnc.m)      1.261     1.261        1.00  1.00            1.00  1.00
  GNC-TLS (GTSAM)      1.261     1.261        1.00  1.00            1.00  1.00

Part B: no wrong loop, rotation noise sigma_R on every edge, 5 graphs per level;
runs (of 5) reaching the lowest cost found on that graph, and median relative cost gap
  sigma_R   odom+GN:hit gap  odom+LM:hit gap  chor+GN:hit gap  chor+LM:hit gap
   0.01             5 1.2e-14          5 1.3e-14          5 0.0e+00          5 1.1e-14
   0.05             5 9.3e-14          5 3.3e-13          5 3.3e-12          5 5.4e-13
   0.10             4 9.1e-12          4 4.9e-12          5 5.4e-12          5 0.0e+00
   0.20             2 1.4e+00          2 1.4e+00          5 1.0e-11          5 0.0e+00

No wrong loop. Least squares closes the true loops and more than halves the error of the odometry start. The 0.40 m that remains is not small; in our reading most of it lies on level 2, which no loop closure constrains (the script does not print the error per level). This row is the base result every robust variant should reach, and with the published defaults DCS and switchable constraints do not: from odometry they switch off true loops (the smallest weight is 0) and return essentially the odometry trajectory’s error, and from the chordal start they still switch at least one off. The reason is in the construction of the costs. A correct 3D loop closure whitened by the right covariances has, at the true poses, a squared error that follows a chi-square distribution with 6 degrees of freedom, so it averages 6, and DCS begins to scale a loop down as soon as its squared error exceeds $\Phi$; with $\Phi = 1$ most correct loops would be scaled down at the true poses, and, in our reading, once a loop’s weight drops the estimate is free to move away from it. From odometry, after 90 cm of drift measured in centimetre-level standard deviations, every true loop looks like a gross outlier. With $\Phi = 16.8$ DCS keeps every true loop and reaches the base result, switchable constraints nearly (smallest true-loop weight 0.84), and so do the wider Cauchy kernel and both GNC versions; Cauchy with $c = 1$ crushes a true loop to weight 0.03 for the same reason as $\Phi = 1$.

One wrong loop. Least squares bends the trajectory to pull level 2 down to level 1, tripling the error, and Huber, whose pull never fades, follows it. Apart from the settings that also suppress true loops, the robust variants reject the wrong loop only from the odometry start. From odometry, DCS and switchable constraints at the dimension-matched setting give the wrong loop weight 0 and return to within a few millimetres of the outlier-free error, and so does GNC in GTSAM’s version. From the chordal start the same methods keep the wrong loop at full or nearly full weight and end where least squares does. The chordal start is computed from all edges, wrong one included, and its error (0.99 m) is already worse than odometry’s; in our reading it partly satisfies the wrong loop, so a kernel that judges each loop by its error at the current estimate sees no outlier. GNC in the gnc.m version fails from both starts for a related reason in the code’s construction: its control parameter comes from the residuals of the unweighted least-squares solution, which has bent to absorb the wrong loop, whereas GTSAM’s version takes it from the starting values, where from odometry the wrong loop still disagrees by 3 m, and restarts every solve from there; the script does not separate the two differences. Cauchy with $c = 1$ rejects the wrong loop but, having also suppressed a true loop, ends at 0.51 m.

Ten wrong loops. A consistent group defeats almost everything. Only DCS at $\Phi = 16.8$ from odometry rejects all ten while keeping the true loops; its error is still higher than the outlier-free one, which in our reading comes from the ten small but nonzero weights it leaves on the wrong loops (printed as 0.00 after rounding), adding up to a pull. DCS and switchable constraints at the defaults reject the wrong loops from odometry only by rejecting the true ones too.

Initialization (Part B). At low rotation noise all four solvers reach the same cost on every graph. As the noise grows, more graphs trap both Gauss–Newton and Levenberg–Marquardt started from odometry at a much higher cost, while both reach the lowest cost every time from the chordal start. Damping does not rescue a bad start: the two methods fail equally often. This is the pattern Carlone et al. (2015) reported on the torus and cube graphs, at a much smaller scale. Part A shows its other side: the same chordal start, computed from every edge, hands a wrong loop to the robust back-ends already satisfied.

Properties and Behavior

• Local convergence. Gauss–Newton and Levenberg–Marquardt converge to a nearby stationary point. Rosen et al. (2019, Fig. 1) show three suboptimal critical points of a parking-garage graph found by local search, next to the global optimum, and argue that such estimates can be so poor as to be unusable even at fairly low noise.
• Initialization decides. In Carlone et al.’s (2015, Table I) comparison on seven 3D graphs, Gauss–Newton from odometry (g2o and GTSAM) was stuck in local minima on sphere-a, torus, cube, cubicle and rim, and a spanning-tree initialization still failed on sphere-a, cubicle and rim; chordal initialization followed by Gauss–Newton gave very accurate results on all seven. On the easy sphere graph a single Gauss–Newton iteration from the chordal estimate matched 5 to 7 iterations from odometry, and the CPU time fell from 1.19 s (g2o) to 0.52 s.
• Cost is not quality. Olson et al. (2006) found that a multilevel relaxation method reduced the cost faster than theirs on a 3,500-pose graph, yet produced maps that did not resemble the ground truth until it had run for about 1,800 seconds, and concluded that log probability is not an adequate measure of graph quality.
• Robustness has its own failure modes. In Sünderhauf and Protzel’s (2012) evaluation, switchable constraints failed in 2 of 2,500 trials with up to 1,000 added outliers (one in Sphere2500 with 300 random outliers, one in Olson’s version of Manhattan with 750), each time because a single false loop closure stayed on, leaving maps that were locally intact but globally distorted; combining switchable constraints with a Huber kernel resolved both. Their 2013 comparison found that the max-mixture model of Olson and Agarwal, which lets each loop closure choose between its Gaussian and a broad null hypothesis, failed to close the loop of a simple Ring dataset with no false loop closures at all, because the large odometry error made it select the outlier component; switchable constraints coped.
• No single winner. In the same comparison, RRR (Latif et al.) was best on the real Bovisa-06 and Bicocca datasets, while switchable constraints and max-mixtures were clearly better on the synthetic Manhattan and City10000 graphs, and the authors concluded that none of the three performed best everywhere (Sünderhauf and Protzel, 2013, §V).
• Global optimality is checkable. On each of the eleven benchmark graphs in Rosen et al.’s tables, SE-Sync and Powell’s dog-leg from a chordal initialization reached the same globally optimal objective value; SE-Sync was faster on all but the garage graph, by a factor of 3.05 on average excluding the grid graph, where it was 27 times faster (Rosen et al., 2019, §6.2, Tables 1–2).

Benchmarks

Pose graph benchmarks are graphs in text formats such as g2o’s, distributed with GTSAM, iSAM, g2o, Vertigo and the SE-Sync repository. The papers report different counts for graphs of the same name, because versions differ in their initial estimates and in how the graph was built; results should be compared only within one version.

Graph	Kind	Poses and edges as reported
Manhattan	synthetic 2D	3,500 poses and 5,600 constraints (Olson et al., 2006); 3,500 poses and 2,099 loop closures, in Olson’s original version and g2o’s, whose initial estimate is much closer to the ground truth (Sünderhauf and Protzel, 2012); 3,500 poses and 5,453 measurements (Rosen et al., 2019)
City10000	synthetic 2D, from iSAM	10,000 poses, 10,688 loop closures (Sünderhauf and Protzel, 2012); 20,687 measurements (Rosen et al., 2019)
Intel Research Lab	real 2D laser, from g2o	943 poses, 894 loop closures (Sünderhauf and Protzel, 2012); 1,728 poses, 2,512 measurements (Rosen et al., 2019); 1,802 nodes, 3,546 edges in Grisetti et al.’s (2010) own construction
Sphere2500 / sphere	synthetic 3D, from iSAM and GTSAM	2,500 poses, 2,450 loop closures (Sünderhauf and Protzel, 2012); 4,949 measurements (Carlone et al., 2015; Rosen et al., 2019); sphere-a, a noisier g2o version, 2,200 poses and 8,647 measurements (Carlone et al., 2015)
Parking garage	real 3D, from g2o and Vertigo	1,661 poses, 4,615 loop closures (Sünderhauf and Protzel, 2012); 6,275 measurements (Carlone et al., 2015; Rosen et al., 2019)
torus, cube (grid)	synthetic 3D	5,000 poses and 9,048 measurements; 8,000 poses and 22,236 measurements (Carlone et al., 2015; Rosen et al., 2019)
cubicle, rim	real 3D laser, ICP constraints, Georgia Tech	5,750 and 10,195 poses, 16,869 and 29,743 measurements (Carlone et al., 2015)
CSAIL, ais2klinik	real 2D	1,045 and 15,115 poses, 1,172 and 16,727 measurements (Rosen et al., 2019)

Protocols for outliers. Most robust back-ends are evaluated on these graphs after adding false loop closures, since the published graphs contain only correct ones (Sünderhauf and Protzel, 2012, §III-B). Sünderhauf and Protzel (2012) defined four policies: random (between any two poses), local (between nearby poses, since nearby places look more alike), randomly grouped and locally grouped (20 consecutive, mutually consistent false loop closures, imitating a front-end that keeps confusing two similar corridors). They added 0 to 1,000 outliers, 10 trials per policy and count, and scored the result by relative pose error against the ground truth (or, for Intel and the garage, against the outlier-free solution) and by the precision and recall of the loop closures switched off. Agarwal et al. (2013) used the same Vertigo script with groups of 10 and up to 5,000 outliers, 400 graphs per dataset, plus the Bicocca and Lincoln Labs multi-session graphs whose false loop closures came from real place recognition (bag of words and GIST front-ends, processed by Latif et al.). Global-optimality studies report the objective value and a bound on suboptimality instead (Rosen et al., 2019), and initialization studies report the cost reached and whether the trajectory is visually correct (Carlone et al., 2015). When ground truth trajectories exist, the absolute trajectory error and relative pose error defined there apply.

Reported results. With switchable constraints and $\Xi = 1$, the success rate was 100% on the g2o version of Manhattan, City10000 and Intel and 99.8% on Olson’s Manhattan and Sphere2500 (outlier ratios up to 47.6%, 9.4%, 111.9%, 47.6% and 40.8% respectively), with a recall above 99.99% at 100% precision for the loop closures it disabled; the parking garage was a special case that the authors discuss separately (Sünderhauf and Protzel, 2012, Table II and §IV). DCS converged to the correct solution with up to 5,000 outliers on ManhattanG2O, Intel and Sphere2500; on ManhattanOlson and City10000 it always converged with local outliers but was sensitive to non-local ones, City10000 failing in some non-local cases above 1,500 outliers (Agarwal et al., 2013, Fig. 2). On Bicocca, DCS took 1.56 s against the 314 s reported for RRR, and its best result there used $\Phi = 5$ (Agarwal et al., 2013, §V-B).

Limitations

• Garbage in. The back-end sees only the virtual measurements. False loop closures that agree with each other, such as a group produced by a long stretch of repeated structure, support each other, and Agarwal et al. (2013, §V-D) call randomly grouped outliers the most difficult and realistic scenario. Non-local outliers took switchable constraints longer to resolve (Sünderhauf and Protzel, 2012, §IV-C) and caused DCS’s failures on two datasets (Agarwal et al., 2013, §V-B). Loop closure detection, which proposes these edges and verifies them geometrically, carries much of the burden.
• Sensitivity to the starting point. Local solvers, robust kernels that flatten out, and max-mixtures all judge measurements at the current estimate, so a poor initial guess can turn correct loop closures into apparent outliers, as in the Ring example above.
• Parameters that do not transfer. Kernel widths, $\Xi$, $\Phi$ and GNC thresholds are expressed in whitened units, and their published defaults were chosen on particular datasets; besides $\Phi = 5$ on Bicocca, Sünderhauf and Protzel (2013) note that setting $\Xi = 0.1$ improved switchable constraints on Bovisa-06.
• Gaussian assumptions. The covariance at the solution is a linearization, and the robust methods above still return a single Gaussian estimate; Dellaert and Kaess (2017, §7.4) contrast them with non-Gaussian inference that can represent several modes.
• Certificates need moderate noise. SE-Sync’s guarantee holds below a noise threshold that depends on the problem, and outliers break its noise model; Rosen et al. list outlier-robust extensions as future work.
• Pose-only. A pose graph keeps the relative poses but not the landmarks or raw data that produced them, so the map has to be carried along afterwards. Strasdat et al. (2010) moved each point with the corrected pose of a frame that saw it and then ran ten iterations of structure-only bundle adjustment; a full bundle adjustment over all 766 keyframes brought no notable further improvement in their experiment.

Variants

• Incremental smoothing. iSAM and iSAM2, described in the factor graph article, update the factorization as poses and loop closures arrive.
• Relaxation and gradient methods. Grisetti et al. (2010, §III) review relaxation methods, including multilevel relaxation, and credit Dellaert and Kaess with first exploiting sparse matrix factorization for the linearized problem; Grisetti et al. extended Olson et al.’s stochastic gradient descent with a tree parametrization.
• Other robust back-ends. Beyond the methods above: max-mixtures (Olson and Agarwal), and RRR (Latif et al.), which, as Agarwal et al. (2013) describe it, finds the largest set of mutually consistent clusters of loop closures and rejects the others instead of keeping them with a low weight.
• Similarity, rotation-first and certifiable variants are the $Sim(3)$ graphs, chordal and LAGO initializations and SE-Sync described above.