QCQP representations for rotations and poses

QcqpProblem lowers supported factor graphs to quadratic costs and constraints. This notebook covers the exact homogeneous 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.

GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415
All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import numpy as np
import gtsam
from gtsam.symbol_shorthand import X

Exact $D=1$ homogeneous vectors¶

The exact track stores a leading homogenization coordinate followed by the variable matrix entries in column-major order. Pose lifts omit the fixed last homogeneous row.

Type	Vector dimension	Coordinates after the leading 1
Rot2	5	$\operatorname{vec}(R)$
Rot3	10	$\operatorname{vec}(R)$
Pose2	7	first two rows of $[R\ t]$
Pose3	13	first three rows of $[R\ t]$

Quadratic equalities enforce the rotation manifold and $x_0^2=1$ . A constrained FrobeniusPrior adds a linear equality fixing the complete vector, which anchors both the global frame and the sign of $x_0$ .

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, "homogenization =", vector[0, 0])

Rot2 (5, 1) homogenization = 1.0
Rot3 (10, 1) homogenization = 1.0
Pose2 (7, 1) homogenization = 1.0
Pose3 (13, 1) homogenization = 1.0

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 (7, 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=6.665e-17, equality violation=1.534e-14
pose error norms: [1.79907919e-15 1.06542957e-14 2.18739824e-14]

Matrix-valued Burer–Monteiro track¶

For Rot2 with $D\ge2$ and Rot3 with $D\ge3$ , each variable is an $N\times D$ row-Stiefel matrix $Y_i$ . The constraints enforce $Y_iY_i^\top=I$ , and a Frobenius between factor contributes

\frac{1}{2\sigma^2}\lVert Y_j-M_{ij}^\top Y_i\rVert_F^2.

(1)

Every connected component has a common right- $O(D)$ gauge: replacing every $Y_i$ by $Y_iG$ 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 $p$ , solves the Burer–Monteiro problem locally, verifies the SDP certificate, and lifts to rank $p+1$ when needed. Pose2 and Pose3 currently use only the exact $D=1$ 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-Values extraction.
The wrapper instantiates each function for Rot2, Rot3, Pose2, and Pose3, appending the type name in Python: for example, qcqpValuePose2, insertQcqpValuePose2, fromQcqpValuePose2, and extractQcqpValuesPose2.
Typed insertion preserves the C++ Matrix type, and extraction selects matching matrices before decoding them.

See the certifiable notebooks for the monolithic, chordal, and Burer–Monteiro solvers.

QCQP representations for rotations and poses

Exact D=1D=1D=1 homogeneous vectors¶

Direct augmented-Lagrangian optimization¶

Matrix-valued Burer–Monteiro track¶

Python conversion API¶

References¶

Exact $D=1$ homogeneous vectors¶