MosekChordalSDP uses symbolic elimination to replace one large positive-semidefinite cone with overlapping clique cones, while exposing the same solve and QCQP recovery lifecycle as the monolithic formulation. After solve(), qcqpValues() and variableEVRs() are optional, independent functional queries for recovered vectors and rank-one diagnostics.
QCQP vectors carry no homogenizing coordinate. Each PSD block is instead the lifted matrix of the keys it holds, with one constant entry; linear cost and constraint terms act on the lifted vector (row-0 slice).
import numpy as np
import gtsam
from gtsam.symbol_shorthand import XChordal decomposition¶
The aggregate sparsity graph of the lifted QCQP is completed according to either ChordalOrderingType.Metis or ChordalOrderingType.Colamd. Symbolic elimination produces a Bayes tree; each clique defines a smaller PSD cone, and separator consistency constraints make the clique matrices agree on overlaps.
METIS is generally the scalable default. COLAMD is useful for deterministic comparisons and small problems. bayesTree() exposes the symbolic decomposition chosen by the solver.
num_poses = 3
step = gtsam.Pose3(
gtsam.Rot3.Rz(2.0 * np.pi / num_poses), np.array([2.0, 0.0, 0.0])
)
ground_truth = [gtsam.Pose3()]
for _ in range(1, num_poses):
ground_truth.append(ground_truth[-1].compose(step))
graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.FrobeniusPriorPose3(
X(0), ground_truth[0].matrix(), gtsam.noiseModel.Constrained.All(16)
))
for i in range(num_poses):
j = (i + 1) % num_poses
graph.add(gtsam.FrobeniusBetweenFactorPose3(
X(i), X(j), ground_truth[i].between(ground_truth[j])
))
problem = gtsam.QcqpProblem(graph)if not hasattr(gtsam, "MosekChordalSDP"):
print("This GTSAM build does not include the optional MOSEK backend.")
else:
solver = gtsam.MosekChordalSDP(problem, gtsam.ChordalOrderingType.Metis)
if not solver.solve({"intpntCoTolRelGap": 1e-10}):
raise RuntimeError("MOSEK did not return a readable primal solution")
recovered = gtsam.extractQcqpValuesPose3(solver.qcqpValues())
errors = [
np.linalg.norm(ground_truth[i].localCoordinates(recovered.atPose3(X(i))))
for i in range(num_poses)
]
print("status:", solver.problemStatus())
print("objective:", solver.objectiveValue())
print("solve time (s):", solver.solveTimeSeconds())
print("Bayes-tree cliques:", solver.bayesTree().size())
print("ordered dimensions:", solver.orderedKeyDims())
print("eigenvalue ratios:", solver.variableEVRs())
print("maximum Pose3 error:", max(errors))status: ProblemStatus::PrimalAndDualFeasible
objective: 1.2082779221600504e-11
solve time (s): 0.025856971740722656
Bayes-tree cliques: 1
ordered dimensions: {8646911284551352320: 12, 8646911284551352321: 12, 8646911284551352322: 12}
eigenvalue ratios: [21606102095761.6, 4526942104661.99, 2169262047143.9036]
maximum Pose3 error: 3.5850844871612547e-07
Use the monolithic solver as a simple reference and the chordal solver when sparsity makes the clique cones materially smaller than the full lifted matrix. Both recover the same D=1 QCQP representation and therefore use the same typed extraction functions.