QcqpProblem lowers supported factor graphs to quadratic costs and constraints. This notebook covers the exact vector representation used by Rot2, Rot3, Pose2, and Pose3, the matrix-valued Burer–Monteiro representation used by rotation problems, direct augmented-Lagrangian optimization, and typed recovery in Python.
import numpy as np
import gtsam
from gtsam.symbol_shorthand import XExact vectors¶
The exact track stores the variable matrix entries in column-major order, with no homogenizing coordinate. Pose lifts omit the fixed last row of the transformation matrix, and Rot2 keeps only its first column .
| Type | Vector dimension | Coordinates |
|---|---|---|
| Rot2 | 2 | first column of |
| Rot3 | 9 | |
| Pose2 | 6 | first two rows of |
| Pose3 | 12 | first three rows of |
Equalities enforce the rotation manifold; the Rot3 and Pose3 handedness rows carry a linear term. Affine factor residuals become linear and constant cost terms. A constrained FrobeniusPrior adds a linear equality fixing the complete vector, which anchors the global frame.
typed_values = [
(gtsam.Rot2.fromAngle(0.3), gtsam.qcqpValueRot2),
(gtsam.Rot3.RzRyRx(0.1, -0.2, 0.3), gtsam.qcqpValueRot3),
(gtsam.Pose2(1.0, -2.0, 0.3), gtsam.qcqpValuePose2),
(gtsam.Pose3(gtsam.Rot3.RzRyRx(0.1, -0.2, 0.3), np.array([1.0, -2.0, 3.0])), gtsam.qcqpValuePose3),
]
for value, to_qcqp in typed_values:
vector = to_qcqp(value)
print(type(value).__name__, vector.shape)Rot2 (2, 1)
Rot3 (9, 1)
Pose2 (6, 1)
Pose3 (12, 1)
Direct augmented-Lagrangian optimization¶
The following anchored Pose2 ring exercises the complete exact-track pipeline: factor-graph lowering, typed insertion of matrix-valued QCQP vectors, constrained optimization, and conversion back to Pose2. The typed insertion helper matters in Python because a NumPy array with shape (6, 1) would otherwise select the Vector overload of Values.insert.
num_poses = 3
step = gtsam.Pose2(2.0, 0.0, 2.0 * np.pi / num_poses)
ground_truth = [gtsam.Pose2()]
for _ in range(1, num_poses):
ground_truth.append(ground_truth[-1].compose(step))
graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.FrobeniusPriorPose2(
X(0), ground_truth[0].matrix(), gtsam.noiseModel.Constrained.All(9)
))
for i in range(num_poses):
j = (i + 1) % num_poses
graph.add(gtsam.FrobeniusBetweenFactorPose2(
X(i), X(j), ground_truth[i].between(ground_truth[j])
))
initial = gtsam.Values()
for i, pose in enumerate(ground_truth):
perturbation = np.array([0.02 * i, -0.01 * i, 0.015 * i])
gtsam.insertQcqpValuePose2(X(i), pose.retract(perturbation), initial)
problem = gtsam.QcqpProblem(graph)
params = gtsam.AugmentedLagrangianParams()
params.maxIterations = 10
params.absoluteViolationTolerance = 1e-6
result = gtsam.AugmentedLagrangianOptimizer(problem, initial, params).optimize()
recovered = gtsam.extractQcqpValuesPose2(result)
errors = np.array([
ground_truth[i].localCoordinates(recovered.atPose2(X(i)))
for i in range(num_poses)
])
cost, equality_violation, inequality_violation = problem.evaluate(result)
print(f"cost={cost:.3e}, equality violation={equality_violation:.3e}")
print("pose error norms:", np.linalg.norm(errors, axis=1))cost=3.091e-13, equality violation=6.033e-07
pose error norms: [2.03333119e-11 1.34522068e-07 3.16423550e-07]
Matrix-valued Burer–Monteiro track¶
For Rot2 with and Rot3 with , each variable is an row-Stiefel matrix . The constraints enforce , and a Frobenius between factor contributes
Every connected component has a common right- gauge: replacing every by preserves costs and constraints. Consequently absolute rotations recovered from an unaligned matrix solution are gauge-dependent. Matrix-form fixed priors are intentionally rejected because they break the Gram-matrix gauge required by the SDP/Burer–Monteiro formulation.
RiemannianStaircaseOptimizer rebuilds QcqpProblem(graph, p) at each rank , solves the Burer–Monteiro problem locally, verifies the SDP certificate, and lifts to rank when needed. Pose2 and Pose3 currently use only the exact track; the matrix track supports Rot2 and Rot3.
rotation_graph = gtsam.NonlinearFactorGraph()
rotation_graph.add(gtsam.FrobeniusBetweenFactorRot3(
X(0), X(1), gtsam.Rot3.Rz(0.4)
))
matrix_problem = gtsam.QcqpProblem(rotation_graph, 3)
print("rank-3 QCQP dimensions:", matrix_problem.dim())rank-3 QCQP dimensions: (1, 12, 0)
Python conversion API¶
Four C++ function templates provide conversion, insertion, single-value recovery, and mixed-
Valuesextraction.The wrapper instantiates each function for Rot2, Rot3, Pose2, and Pose3, appending the type name in Python: for example,
qcqpValuePose2,insertQcqpValuePose2,fromQcqpValuePose2, andextractQcqpValuesPose2.Typed insertion preserves the C++
Matrixtype, and extraction selects matching matrices before decoding them.
See the certifiable notebooks for the monolithic, chordal, and Burer–Monteiro solvers.