FAST-Sync - GTSAM Docs

fastSync<T> is a sparse chordal initializer for synchronization over fixed-size matrix Lie groups. It estimates all group elements jointly from relative BetweenFactor<T> measurements, then rounds the ambient matrices back to the requested group.

For the algorithmic derivation and evaluation, see the FAST-Sync paper.

import numpy as np
import gtsam

Ambient relaxation and sparse solve¶

For each relative measurement $Z_{ij}\approx X_i^{-1}X_j$ , FAST-Sync forms the block residual

X_j^\top - Z_{ij}^\top X_i^\top,

(1)

represented by a reduced Gaussian factor with blocks $[-Z_{ij}^\top, I]$ . Each measurement and back-substitution block uses a fixed-size N-by-N matrix, with N obtained from the representation returned by T::matrix(); only the complete sparse Gaussian system has dynamic storage because graph topology is known at runtime. The factor is weighted by the reciprocal isotropic standard deviation. METIS nested dissection supplies the default sparse elimination ordering, while callers may select COLAMD or another supported OrderingType, or provide a complete Ordering directly; the ordering’s final key is fixed to the identity as the gauge. Sequential Cholesky elimination and reverse block back-substitution recover every ambient matrix. Crucially, projection happens only after this complete linear solve.

A matching prior is not injected into the relaxed least-squares system. Instead, after rounding, a single global left transformation aligns the result to that prior. Consequently relative estimates are unchanged by the prior.

API and projections¶

C++ template	Python/MATLAB function	Projection
`fastSync<Rot2>`	`fastSyncRot2`	closest proper 2D rotation
`fastSync<Rot3>`	`fastSyncRot3`	closest proper 3D rotation
`fastSync<Pose2>`	`fastSyncPose2`	closest rotation plus translation
`fastSync<Pose3>`	`fastSyncPose3`	closest rotation plus translation
`fastSync<Similarity2>`	`fastSyncSimilarity2`	rotation, translation, and positive scale
`fastSync<Similarity3>`	`fastSyncSimilarity3`	rotation, translation, and positive scale
`fastSync<SL4>`	`fastSyncSL4`	orientation-corrected SVD and determinant normalization

C++ extensions specialize FastSyncProjection<T> for another fixed-size matrix Lie group. The optional second argument to fastSync is either an OrderingType, defaulting to METIS, or a complete custom Ordering. The input must contain a non-empty, connected set of matching between factors with finite, positive, isotropic Gaussian noise. Robust, constrained, and anisotropic models are rejected, as are multiple matching priors. Disconnected inputs reach Cholesky elimination and raise IndeterminantLinearSystemException.

Rot3 walkthrough¶

rotations = [gtsam.Rot3.Expmap(np.array([0.08*i, -0.03*i, 0.05*i])) for i in range(5)]
rot3_graph = gtsam.NonlinearFactorGraph()
rot3_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.08)
for edge, (i, j) in enumerate([(0, 1), (1, 2), (2, 3), (3, 4), (4, 0)]):
    measured = rotations[i].between(rotations[j])
    perturbation = gtsam.Rot3.Expmap(np.array([0.006, -0.004, 0.003]) * (edge - 2))
    rot3_graph.add(gtsam.BetweenFactorRot3(i, j, measured.compose(perturbation), rot3_model))
rot3_graph.add(gtsam.PriorFactorRot3(0, rotations[0], rot3_model))
rot3_result = gtsam.fastSyncRot3(rot3_graph)
rot3_errors_deg = [np.linalg.norm(gtsam.Rot3.Logmap(rotations[i].between(rot3_result.atRot3(i)))) * 180 / np.pi for i in range(5)]
print('Rot3 errors (degrees):', np.round(rot3_errors_deg, 3))

Rot3 errors (degrees): [0.    0.895 1.342 1.342 0.895]

Pose3 walkthrough¶

For poses, the same ambient solve estimates rotation and translation together. Rounding orthogonalizes each rotational block while retaining the recovered translation.

poses = [gtsam.Pose3(rotations[i], np.array([i, 0.2*i*i, -0.1*i])) for i in range(5)]
pose3_graph = gtsam.NonlinearFactorGraph()
pose3_model = gtsam.noiseModel.Isotropic.Sigma(6, 0.08)
for edge, (i, j) in enumerate([(0, 1), (1, 2), (2, 3), (3, 4), (4, 0)]):
    measured = poses[i].between(poses[j])
    delta = np.array([0.003, -0.002, 0.002, 0.01, -0.008, 0.006]) * (edge - 2)
    pose3_graph.add(gtsam.BetweenFactorPose3(i, j, measured.compose(gtsam.Pose3.Expmap(delta)), pose3_model))
pose3_graph.add(gtsam.PriorFactorPose3(0, poses[0], pose3_model))
pose3_result = gtsam.fastSyncPose3(pose3_graph)
translation_errors = [np.linalg.norm(poses[i].translation() - pose3_result.atPose3(i).translation()) for i in range(5)]
print('Pose3 translation errors:', np.round(translation_errors, 4))

Pose3 translation errors: [0.     0.0283 0.0423 0.0471 0.0526]

Practical notes¶

The returned Values are intended as a high-quality initialization for nonlinear optimization; noisy chordal estimates need not minimize the original geodesic objective. Arbitrary GTSAM keys are preserved. Selecting METIS in a build without METIS support reports the existing nested-dissection error instead of silently changing ordering behavior; COLAMD remains available explicitly. For a runnable all-groups example with plots, see FastSyncExample.ipynb.