RiemannianStaircaseOptimizer solves matrix-valued Rot2/Rot3 QCQPs through a low-rank Burer–Monteiro factorization, verifies an SDP dual certificate, and increases rank only when the certificate exposes a negative-curvature direction.
import numpy as np
import gtsam
from gtsam.symbol_shorthand import XFrom QCQP to the staircase¶
The SDP variable is factorized as with . At rank , the optimizer:
constructs
QcqpProblem(graph, p);solves the rank- constrained problem with
AugmentedLagrangianOptimizer;builds the certificate ;
certifies when ;
otherwise appends a column along the most-negative eigenvector and continues at rank .
The current matrix QCQP traits support Rot2 and Rot3. Their solutions share a common right-orthogonal gauge, so rounded absolute rotations must be gauge-aligned before comparison.
num_rotations = 4
delta = 2.0 * np.pi / num_rotations
truth = [gtsam.Rot2.fromAngle(i * delta) for i in range(num_rotations)]
graph = gtsam.NonlinearFactorGraph()
initial = gtsam.Values()
for i in range(num_rotations):
j = (i + 1) % num_rotations
graph.add(gtsam.FrobeniusBetweenFactorRot2(
X(i), X(j), truth[i].between(truth[j])
))
perturbed = gtsam.Rot2.fromAngle(i * delta + 0.02 * i)
initial.insert(X(i), perturbed.matrix().T)alm = gtsam.AugmentedLagrangianParams()
alm.maxIterations = 80
alm.absoluteViolationTolerance = 1e-8
params = gtsam.RiemannianStaircaseParams()
params.pMin = 2
params.pMax = 4
params.eta = 1e-3
params.setAlmParams(alm)
optimizer = gtsam.RiemannianStaircaseOptimizer(graph, initial, params)
result = optimizer.optimize()
print("certified:", result.certified)
print("final rank:", result.finalRank)
print("certificate eigenvalue/bound:", result.minEigenvalue)
print("ranks visited:", result.getRanksVisited())certified: True
final rank: 2
certificate eigenvalue/bound: -0.001
ranks visited: [2.]
result.values contains the final unrounded rank- matrices. When certification succeeds, result.roundedValues() contains the SVD projection to the intrinsic rotation rank. See RiemannianStaircaseResult.ipynb for diagnostics and recovery details.