From a07dce130335e97c43b5028e0aa8cc67c98ecca3 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 28 Nov 2024 19:49:03 +0100 Subject: [PATCH 01/15] Create new constraints test --- _test/test_cliques.py | 52 +++++++++++++++------- _test/test_homqcqp.py | 6 +-- _test/utils.py => cert_tools/test_tools.py | 27 ++++++++++- 3 files changed, 64 insertions(+), 21 deletions(-) rename _test/utils.py => cert_tools/test_tools.py (92%) diff --git a/_test/test_cliques.py b/_test/test_cliques.py index 475ac56..fddb71e 100644 --- a/_test/test_cliques.py +++ b/_test/test_cliques.py @@ -1,15 +1,16 @@ -import itertools import os import numpy as np -from poly_matrix import PolyMatrix - from cert_tools import HomQCQP +from cert_tools.test_tools import constraints_test, cost_test, get_chain_rot_prob + +from poly_matrix import PolyMatrix root_dir = os.path.abspath(os.path.dirname(__file__) + "/../") def generate_random_matrix(seed=0): + """Creates random block-tridiagonal arrowhead matrix""" np.random.seed(seed) dim_x = 2 X = PolyMatrix() @@ -24,23 +25,40 @@ def generate_random_matrix(seed=0): return X -def test_decompositions(): - def test_cost(cliques, C_gt): - C = PolyMatrix() - mat_decomp = problem.decompose_matrix(problem.C, method="split") - for clique in cliques: - C += mat_decomp[clique.index] - np.testing.assert_allclose(C.get_matrix_dense(variables), C_gt) +def test_symmetric(): + """Making sure that setting symmetric to True after the fact doesn't break anything. + This test could probably go inside poly_matrix.""" + X_poly = generate_random_matrix() + X_sparse = X_poly.get_matrix() + + X_poly_test, __ = PolyMatrix.init_from_sparse(X_sparse, X_poly.variable_dict_i) + X_poly_test.symmetric = True + for key_i in X_poly.variable_dict_i: + for key_j in X_poly.adjacency_j[key_i]: + np.testing.assert_allclose(X_poly_test[key_i, key_j], X_poly[key_i, key_j]) + + X_poly_test, __ = PolyMatrix.init_from_sparse(X_sparse, X_poly.variable_dict_i) + for key_i in X_poly.variable_dict_i: + for key_j in X_poly.adjacency_j[key_i]: + np.testing.assert_allclose(X_poly_test[key_i, key_j], X_poly[key_i, key_j]) + + +def test_constraint_decomposition(): + + problem = get_chain_rot_prob() + problem.clique_decomposition() + constraints_test(problem) + + +def test_cost_decomposition(): problem = HomQCQP() problem.C = generate_random_matrix() problem.As = [] - variables = problem.C.get_variables() - C_gt = problem.C.get_matrix_dense(variables) # will create a clique decomposition that is not always the same problem.clique_decomposition() - test_cost(problem.cliques, C_gt) + cost_test(problem) clique_list = [ {"h", "x_1", "x_2"}, @@ -48,7 +66,7 @@ def test_cost(cliques, C_gt): {"h", "x_3", "x_4"}, ] problem.clique_decomposition(clique_data=clique_list) - test_cost(problem.cliques, C_gt) + cost_test(problem) for var_list, clique in zip(clique_list, problem.cliques): assert set(clique.var_list).difference(var_list) == set() @@ -59,11 +77,13 @@ def test_cost(cliques, C_gt): "parents": [1, 2, 2], } problem.clique_decomposition(clique_data=clique_data) - test_cost(problem.cliques, C_gt) + cost_test(problem) for var_list, clique in zip(clique_list, problem.cliques): assert set(clique.var_list).difference(var_list) == set() if __name__ == "__main__": - test_decompositions() + test_symmetric() + test_constraint_decomposition() + test_cost_decomposition() print("all tests passed.") diff --git a/_test/test_homqcqp.py b/_test/test_homqcqp.py index 082365c..8425b9f 100644 --- a/_test/test_homqcqp.py +++ b/_test/test_homqcqp.py @@ -2,14 +2,14 @@ import unittest import numpy as np -from poly_matrix import PolyMatrix -from utils import get_chain_rot_prob, get_loop_rot_prob - from cert_tools import HomQCQP from cert_tools.hom_qcqp import greedy_cover from cert_tools.linalg_tools import svec from cert_tools.sdp_solvers import solve_sdp_homqcqp from cert_tools.sparse_solvers import solve_clarabel, solve_dsdp +from cert_tools.test_tools import get_chain_rot_prob, get_loop_rot_prob + +from poly_matrix import PolyMatrix class TestHomQCQP(unittest.TestCase): diff --git a/_test/utils.py b/cert_tools/test_tools.py similarity index 92% rename from _test/utils.py rename to cert_tools/test_tools.py index 9905cc9..e06d856 100644 --- a/_test/utils.py +++ b/cert_tools/test_tools.py @@ -1,13 +1,36 @@ import numpy as np -from poly_matrix import PolyMatrix +from cert_tools import HomQCQP from pylgmath import so3op -from cert_tools import HomQCQP +from poly_matrix import PolyMatrix # Global Defaults ER_MIN = 1e6 +def cost_test(problem): + variables = problem.C.get_variables() + C = PolyMatrix() + mat_decomp = problem.decompose_matrix(problem.C, method="split") + for mat in mat_decomp.values(): + C += mat + np.testing.assert_allclose( + C.get_matrix_dense(variables), problem.C.get_matrix_dense(variables) + ) + + +def constraints_test(problem): + for A_gt in problem.As: + variables = A_gt.get_variables() + A = PolyMatrix() + mat_decomp = problem.decompose_matrix(A_gt, method="split") + for mat in mat_decomp.values(): + A += mat + np.testing.assert_allclose( + A.get_matrix_dense(variables), A_gt.get_matrix_dense(variables) + ) + + class RotSynchLoopProblem(HomQCQP): """Class to generate and solve a rotation synchronization problem with loop constraints. The problem is generated with ground truth rotations and noisy From f4fb94f35fbe31b108aad57bcf25ca0acc6451f6 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 28 Nov 2024 19:49:15 +0100 Subject: [PATCH 02/15] Create new static initializer --- cert_tools/hom_qcqp.py | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index db9d169..daabf2f 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -8,13 +8,13 @@ import plotly.graph_objects as go import plotly.io as pio import scipy.sparse as sp +from cert_tools.base_clique import BaseClique +from cert_tools.linalg_tools import find_dependent_columns, svec from cvxopt import amd, spmatrix from igraph import Graph -from poly_matrix import PolyMatrix from scipy.linalg import polar -from cert_tools.base_clique import BaseClique -from cert_tools.linalg_tools import find_dependent_columns, svec +from poly_matrix import PolyMatrix class HomQCQP(object): @@ -44,6 +44,24 @@ def __init__(self, homog_var="h"): self.var_clique_map = {} # Variable to clique mapping (maps to set) self.h = homog_var # Homogenizing variable name + @staticmethod + def init_from_lifter(lifter): + problem = HomQCQP() + problem.C = lifter.get_Q_from_y(lifter.y_, output_poly=True) + + A_sparse_list = lifter.get_A_learned_simple() + A_poly_list = [] + for A in A_sparse_list: + A_poly, __ = PolyMatrix.init_from_sparse(A, lifter.var_dict) + A_poly.symmetric = True + A_poly_list.append(A_poly) + problem.As = A_poly_list + + # TODO(FD) not sure if we should do this here or wait. + # problem.get_asg() # to suppress warning in clique_decomposition + # problem.clique_decomposition() + return problem + def define_objective(self, *args, **kwargs) -> PolyMatrix: """Function should define the cost matrix for the problem NOTE: Defined by inhereting class @@ -389,6 +407,11 @@ def decompose_matrix(self, pmat: PolyMatrix, method="split"): """Decompose a matrix according to clique decomposition. Returns a dictionary with the key being the clique number and the value being a PolyMatrix that contains decomposed matrix on that clique.""" assert isinstance(pmat, PolyMatrix), TypeError("Input should be a PolyMatrix") assert pmat.symmetric, ValueError("PolyMatrix input should be symmetric") + + if not len(self.var_clique_map): + raise ValueError( + "var_clique_map is empty. Did you run clique_decomposition?" + ) dmat = {} # defined decomposed matrix dictionary # Loop through elements of polymatrix and gather information about cliques and edges edges = [] From 3336d4c3953454aef5c1e13f4dbdf74ee6bd2411 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Nov 2024 11:02:32 +0100 Subject: [PATCH 03/15] Fix var_sizes in init_from_lifter --- cert_tools/hom_qcqp.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index daabf2f..05e8168 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -48,6 +48,7 @@ def __init__(self, homog_var="h"): def init_from_lifter(lifter): problem = HomQCQP() problem.C = lifter.get_Q_from_y(lifter.y_, output_poly=True) + problem.var_sizes = lifter.var_dict A_sparse_list = lifter.get_A_learned_simple() A_poly_list = [] @@ -58,7 +59,7 @@ def init_from_lifter(lifter): problem.As = A_poly_list # TODO(FD) not sure if we should do this here or wait. - # problem.get_asg() # to suppress warning in clique_decomposition + # problem.get_asg(lifter.var_dict) # to suppress warning in clique_decomposition # problem.clique_decomposition() return problem @@ -174,7 +175,14 @@ def clique_decomposition( ): """Uses CHOMPACK to get the maximal cliques and build the clique tree The clique objects are stored in a list. Each clique object stores information - about its parents and children, as well as separators""" + about its parents and children, as well as separators + + Args: + elim_order: currently, can choose from "amd" or None (no reordering). + clique_data (list): optional, can pass the fixed order to be used + + """ + if len(self.asg.vs) == 0: warnings.warn("Aggregate sparsity graph not defined. Building now.") # build aggregate sparsity graph @@ -183,6 +191,11 @@ def clique_decomposition( if len(clique_data) == 0: if elim_order == "amd": p = amd.order + elif elim_order is None: + p = list(range(len(self.var_sizes))) + else: + raise ValueError(elim_order) + # Convert adjacency to sparsity pattern nvars = len(self.var_sizes) A = self.asg.get_adjacency_sparse() + sp.eye(nvars) From 6475e281861c0b7f3fdfd9c23f5d9cbdf8882183 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Nov 2024 11:03:39 +0100 Subject: [PATCH 04/15] Change naming: junction_tree=>problem and from=>use_primal --- cert_tools/sparse_solvers.py | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/cert_tools/sparse_solvers.py b/cert_tools/sparse_solvers.py index d66f3df..a98a369 100644 --- a/cert_tools/sparse_solvers.py +++ b/cert_tools/sparse_solvers.py @@ -9,19 +9,19 @@ import mosek.fusion.pythonic as fu import numpy as np import scipy.sparse as sp -from igraph import Graph -from poly_matrix import PolyMatrix - from cert_tools.base_clique import BaseClique from cert_tools.fusion_tools import get_slice, mat_fusion from cert_tools.hom_qcqp import HomQCQP -from cert_tools.linalg_tools import smat, svec +from cert_tools.linalg_tools import smat from cert_tools.sdp_solvers import ( adjust_tol, adjust_tol_fusion, options_cvxpy, options_fusion, ) +from igraph import Graph + +from poly_matrix import PolyMatrix CONSTRAIN_ALL_OVERLAP = False @@ -175,7 +175,7 @@ def solve_clarabel(problem: HomQCQP, use_decomp=False): def solve_dsdp( problem: HomQCQP, - form="primal", + use_primal=True, reduce_constrs=None, verbose=False, tol=TOL, @@ -190,7 +190,7 @@ def solve_dsdp( tol (float, optional): Tolerance for solver. Defaults to TOL. adjust (bool, optional): If true, adjust the cost matrix. Defaults to False. """ - if form == "primal": + if use_primal: out = solve_dsdp_primal( problem=problem, reduce_constrs=reduce_constrs, @@ -199,7 +199,7 @@ def solve_dsdp( adjust=adjust, decomp_methods=decomp_methods, ) - elif form == "dual": + else: out = solve_dsdp_dual( problem=problem, verbose=verbose, @@ -223,6 +223,9 @@ def solve_dsdp_dual( tol (float, optional): Tolerance for solver. Defaults to TOL. adjust (bool, optional): If true, adjust the cost matrix. Defaults to False. """ + if adjust: + print("Warning: adjust currently has no effect.") + T0 = time() # List of constraints with homogenizing constraint. A_h = PolyMatrix() @@ -500,7 +503,7 @@ def print_tuples(rows, cols, vals): print(f"({rows[i]},{cols[i]},{vals[i]})") -def solve_oneshot_primal_fusion(junction_tree, verbose=False, tol=TOL, adjust=False): +def solve_oneshot_primal_fusion(problem: HomQCQP, verbose=False, tol=TOL, adjust=False): """ junction_tree: a Graph structure that corresponds to the junction tree of the factor graph for the problem @@ -511,7 +514,7 @@ def solve_oneshot_primal_fusion(junction_tree, verbose=False, tol=TOL, adjust=Fa raise ValueError("adjust_Q does not work when dealing with cliques") # Get list of clique objects - clique_list = junction_tree.vs["clique_obj"] + clique_list = problem.vs["clique_obj"] assert isinstance(clique_list[0], BaseClique) X_dim = clique_list[0].X_dim @@ -548,11 +551,11 @@ def solve_oneshot_primal_fusion(junction_tree, verbose=False, tol=TOL, adjust=Fa A_0_constraints.append(con) # Loop through edges in the junction tree - for iEdge, edge in enumerate(junction_tree.get_edgelist()): + for iEdge, edge in enumerate(problem.get_edgelist()): # Get cliques associated with edge - cl = junction_tree.vs["clique_obj"][edge[0]] - ck = junction_tree.vs["clique_obj"][edge[1]] - for l in junction_tree.es["sepset"][iEdge]: + cl = problem.vs["clique_obj"][edge[0]] + ck = problem.vs["clique_obj"][edge[1]] + for l in problem.es["sepset"][iEdge]: for rl, rk in zip(cl.get_ranges(l), ck.get_ranges(l)): # cl.X_var[rl[0], rl[1]] == ck.X[rk[0], rk[1]]) left_start = [rl[0][0], rl[1][0]] @@ -667,8 +670,7 @@ def solve_oneshot_primal_cvxpy(clique_list, verbose=False, tol=TOL): def solve_oneshot( - junction_tree=None, - clique_list=None, + problem: HomQCQP, use_primal=True, use_fusion=False, verbose=False, @@ -677,9 +679,9 @@ def solve_oneshot( if not use_primal: print("Defaulting to primal because dual cliques not implemented yet.") if use_fusion: - return solve_oneshot_primal_fusion(junction_tree, verbose=verbose, tol=tol) + return solve_oneshot_primal_fusion(problem, verbose=verbose, tol=tol) else: - return solve_oneshot_primal_cvxpy(clique_list, verbose=verbose, tol=tol) + return solve_oneshot_primal_cvxpy(problem, verbose=verbose, tol=tol) # return solve_oneshot_dual_cvxpy( # clique_list, verbose=verbose, tol=tol, adjust=adjust # ) From c1a417328e890098180ed2410f3bbebd4d086152 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Nov 2024 11:27:00 +0100 Subject: [PATCH 05/15] Test fixed-order decomposition --- _test/test_cliques.py | 45 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/_test/test_cliques.py b/_test/test_cliques.py index fddb71e..505f75b 100644 --- a/_test/test_cliques.py +++ b/_test/test_cliques.py @@ -14,7 +14,10 @@ def generate_random_matrix(seed=0): np.random.seed(seed) dim_x = 2 X = PolyMatrix() - for i in range(1, 4): + n_vars = 4 + var_sizes = {"h": 1} + var_sizes.update({f"x_{i}": dim_x for i in range(1, n_vars)}) + for i in range(1, n_vars): random_fill = np.random.rand(1 + 2 * dim_x, 1 + 2 * dim_x) random_fill += random_fill.T clique, __ = PolyMatrix.init_from_sparse( @@ -22,13 +25,13 @@ def generate_random_matrix(seed=0): ) clique.symmetric = True X += clique - return X + return X, var_sizes def test_symmetric(): """Making sure that setting symmetric to True after the fact doesn't break anything. This test could probably go inside poly_matrix.""" - X_poly = generate_random_matrix() + X_poly, var_sizes = generate_random_matrix() X_sparse = X_poly.get_matrix() X_poly_test, __ = PolyMatrix.init_from_sparse(X_sparse, X_poly.variable_dict_i) @@ -53,7 +56,7 @@ def test_constraint_decomposition(): def test_cost_decomposition(): problem = HomQCQP() - problem.C = generate_random_matrix() + problem.C, var_sizes = generate_random_matrix() problem.As = [] # will create a clique decomposition that is not always the same @@ -82,7 +85,41 @@ def test_cost_decomposition(): assert set(clique.var_list).difference(var_list) == set() +def get_chain_clique_data(var_sizes, fixed=["h"], variable=["x_", "z_"]): + clique_data = [] + indices = [ + int(v.split(variable[0])[1].strip("_")) for v in var_sizes if variable[0] in v + ] + # debug start + if len(variable) > 1: + for v in variable: + for i in indices: + assert f"{v}{i}" in var_sizes + # debug end + for i, j in zip(indices[:-1], indices[1:]): + clique_data.append( + set(fixed + [f"{v}{i}" for v in variable] + [f"{v}{j}" for v in variable]) + ) + return clique_data + + +def test_fixed_decomposition(): + """Example of how to do a clique decomposition keeping the order of variables within each clique.""" + problem = HomQCQP() + problem.C, var_sizes = generate_random_matrix() + problem.As = [] + + problem.get_asg(var_list=var_sizes) + + clique_data = get_chain_clique_data(var_sizes, fixed=["h"], variable=["x_"]) + problem.clique_decomposition(clique_data=clique_data) + + for c, vars in zip(problem.cliques, clique_data): + np.testing.assert_allclose(c.var_list, vars) + + if __name__ == "__main__": + test_fixed_decomposition() test_symmetric() test_constraint_decomposition() test_cost_decomposition() From 62a12cf7752bb7f2885f67ff9366348ff2387e9a Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Nov 2024 20:18:27 +0100 Subject: [PATCH 06/15] Make tests pass --- _test/test_cliques.py | 13 ++++++++----- _test/test_homqcqp.py | 10 ++++++++-- cert_tools/sparse_solvers.py | 2 +- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/_test/test_cliques.py b/_test/test_cliques.py index 505f75b..9c01651 100644 --- a/_test/test_cliques.py +++ b/_test/test_cliques.py @@ -1,11 +1,12 @@ import os import numpy as np -from cert_tools import HomQCQP -from cert_tools.test_tools import constraints_test, cost_test, get_chain_rot_prob - from poly_matrix import PolyMatrix +from cert_tools import HomQCQP +from cert_tools.test_tools import (constraints_test, cost_test, + get_chain_rot_prob) + root_dir = os.path.abspath(os.path.dirname(__file__) + "/../") @@ -104,7 +105,8 @@ def get_chain_clique_data(var_sizes, fixed=["h"], variable=["x_", "z_"]): def test_fixed_decomposition(): - """Example of how to do a clique decomposition keeping the order of variables within each clique.""" + """Example of how to do a clique decomposition keeping the order of variables within + each clique.""" problem = HomQCQP() problem.C, var_sizes = generate_random_matrix() problem.As = [] @@ -115,7 +117,7 @@ def test_fixed_decomposition(): problem.clique_decomposition(clique_data=clique_data) for c, vars in zip(problem.cliques, clique_data): - np.testing.assert_allclose(c.var_list, vars) + assert len(set(c.var_list) - vars) == 0 if __name__ == "__main__": @@ -124,3 +126,4 @@ def test_fixed_decomposition(): test_constraint_decomposition() test_cost_decomposition() print("all tests passed.") + print("all tests passed.") diff --git a/_test/test_homqcqp.py b/_test/test_homqcqp.py index 8425b9f..50ef7cb 100644 --- a/_test/test_homqcqp.py +++ b/_test/test_homqcqp.py @@ -2,6 +2,8 @@ import unittest import numpy as np +#import pytest + from cert_tools import HomQCQP from cert_tools.hom_qcqp import greedy_cover from cert_tools.linalg_tools import svec @@ -203,10 +205,14 @@ def test_decompose_matrix(self): problem.clique_decomposition() # get clique decomposition C = problem.C - # args self.assertRaises(ValueError, problem.decompose_matrix, C, method="banana") + # with pytest.raises(ValueError): + # problem.decompose_matrix(C, method="banana") + A = np.zeros((4, 4)) self.assertRaises(AssertionError, problem.decompose_matrix, A) + # with pytest.raises(AssertionError): + # problem.decompose_matrix(A) # functionality for method in ["split", "first", "greedy-cover"]: @@ -341,7 +347,7 @@ def test_solve_dual_dsdp(self, rank1=False): problem = get_chain_rot_prob(N=nvars, locked_pose=locked_pose) problem.clique_decomposition() # get cliques # Solve decomposed problem (Interior Point Version) - c_list, info = solve_dsdp(problem, form="dual", verbose=True, tol=1e-8) + c_list, info = solve_dsdp(problem, use_primal=False, verbose=True, tol=1e-8) # Solve non-decomposed problem X, _, time = solve_sdp_homqcqp(problem, tol=1e-8, verbose=True) diff --git a/cert_tools/sparse_solvers.py b/cert_tools/sparse_solvers.py index a98a369..99bc5d7 100644 --- a/cert_tools/sparse_solvers.py +++ b/cert_tools/sparse_solvers.py @@ -505,7 +505,7 @@ def print_tuples(rows, cols, vals): def solve_oneshot_primal_fusion(problem: HomQCQP, verbose=False, tol=TOL, adjust=False): """ - junction_tree: a Graph structure that corresponds to the junction tree + problem: a Graph structure that corresponds to the junction tree of the factor graph for the problem """ if adjust: From ad8d4cd913b98abf1b5c22329fc288682e29a2e7 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 2 Dec 2024 20:23:30 +0100 Subject: [PATCH 07/15] Update ADMM structure --- _test/test_admm.py | 211 ++++++++++++++++++++++++++++++ _test/test_cliques.py | 25 +--- cert_tools/admm_clique.py | 258 +++++++++++-------------------------- cert_tools/admm_solvers.py | 111 +++++++--------- cert_tools/base_clique.py | 18 +++ cert_tools/fusion_tools.py | 7 + cert_tools/hom_qcqp.py | 34 +++-- cert_tools/test_tools.py | 9 +- 8 files changed, 394 insertions(+), 279 deletions(-) create mode 100644 _test/test_admm.py diff --git a/_test/test_admm.py b/_test/test_admm.py new file mode 100644 index 0000000..7e3c764 --- /dev/null +++ b/_test/test_admm.py @@ -0,0 +1,211 @@ +""" +ADMM test problem: + + [1-1 0 0 0] [4-2 0 0 0] [9-3 0 0 0] + [0 1 0 0 0] [0 1 0 0 0] [0 1 0 0 0] +min < [0 0 0 0 0], X_1 > + < [0 0 0 0 0], X_2 > + < [0 0 0 0 0], X_3 > + [0 0 0 0 0] [0 0 0 0 0] [0 0 0 1 0] + [0 0 0 0 0] [0 0 0 0 0] [0 0 0 0 0] + + homogeneous constraints: + [1 0 0 0 0] + [0 0 0 0 0] + s.t. < [0 0 0 0 0], X_i > = 1 i = 1, 2, 3 + [0 0 0 0 0] + [0 0 0 0 0] + + primary constraints per clique: + [0 0 0 0 0] + [0 1 0 0 0] + < [0 0-1 0 0], X_i > = 0 i = 1, 2, 3 + [0 0 0 0 0] + [0 0 0 0 0] + + consensus constraints: + [0 0 0 0 0] [0 0 0 0 0] + [0 0 0 0 0] [0-1 0 0 0] + < [0 0 0 0 0], X_1 > + < [0 0 0 0 0], X_2 > = 0 etc. + [0 0 0 1 0] [0 0 0 0 0] + [0 0 0 0 0] [0 0 0 0 0] + +Solution is: + h x_0 x_1 x_2 x_3 +[1 1 1 2 2 3 3 0 0] +""" + +import matplotlib.pylab as plt +import numpy as np +import scipy.sparse as sp +from poly_matrix import PolyMatrix + +from cert_tools.admm_clique import ADMMClique +from cert_tools.admm_solvers import solve_alternating +from cert_tools.hom_qcqp import HomQCQP +from cert_tools.linalg_tools import rank_project, svec +from cert_tools.sdp_solvers import solve_sdp +from cert_tools.test_tools import get_chain_rot_prob + + +def create_admm_test_problem(): + """ + [1-1 0 0 0] [4-2 0 0 0] [9-3 0 0 0] + [0 1 0 0 0] [0 1 0 0 0] [0 1 0 0 0] + min < [0 0 0 0 0], X_1 > + < [0 0 0 0 0], X_2 > + < [0 0 0 0 0], X_3 > + [0 0 0 0 0] [0 0 0 0 0] [0 0 0 1 0] + [0 0 0 0 0] [0 0 0 0 0] [0 0 0 0 0] + """ + Q = PolyMatrix() + # first block + Q["h", "h"] = 1.0 + Q["h", "x_0"] = np.array([[-1.0, 0]]) + Q["x_0", "x_0"] = np.array([[1.0, 0.0], [0.0, 0.0]]) + # second block + Q["h", "h"] += 4.0 + Q["h", "x_1"] = np.array([[-2.0, 0]]) + Q["x_1", "x_1"] = np.array([[1.0, 0.0], [0.0, 0.0]]) + # third block + Q["h", "h"] += 9.0 + Q["h", "x_2"] = np.array([[-3.0, 0]]) + Q["x_2", "x_2"] = np.array([[1.0, 0.0], [0.0, 0.0]]) + Q["x_3", "x_3"] = np.array([[1.0, 0.0], [0.0, 0.0]]) + + # constraints + """ + primary constraints per clique: + [0 0 0 0 0] + [0 1 0 0 0] + < [0 0-1 0 0], X_i > = 0 i = 1, 2, 3 + [0 0 0 0 0] + [0 0 0 0 0] + """ + A_1a = PolyMatrix() + A_1a["x_0", "x_0"] = np.array([[1.0, 0.0], [0.0, -1.0]]) + A_1b = PolyMatrix() + A_1b["h", "x_0"] = np.array([[1.0, -1.0]]) + A_2a = PolyMatrix() + A_2a["x_1", "x_1"] = np.array([[1.0, 0.0], [0.0, -1.0]]) + A_2b = PolyMatrix() + A_2b["h", "x_1"] = np.array([[1.0, -1.0]]) + A_3a = PolyMatrix() + A_3a["x_2", "x_2"] = np.array([[1.0, 0.0], [0.0, -1.0]]) + A_3b = PolyMatrix() + A_3b["h", "x_2"] = np.array([[1.0, -1.0]]) + A_4a = PolyMatrix() + A_4a["x_3", "x_3"] = np.array([[1.0, 0.0], [0.0, -1.0]]) + A_4b = PolyMatrix() + A_4b["h", "x_3"] = np.array([[1.0, -1.0]]) + + problem = HomQCQP() + problem.C = Q + problem.As = [A_1a, A_2a, A_3a, A_4a, A_1b, A_2b, A_3b, A_4b] + problem.var_sizes = {"h": 1, "x_0": 2, "x_1": 2, "x_2": 2, "x_3": 2} + return problem + + +def test_consistency(): + problem = create_admm_test_problem() + admm_cliques = ADMMClique.create_admm_cliques_from_problem(problem, variable=["x_"]) + + Q, Constraints = problem.get_problem_matrices() + X, *_ = solve_sdp(Q, Constraints) + X_poly, __ = PolyMatrix.init_from_sparse(X, var_dict=problem.var_sizes) + + for clique in admm_cliques: + clique.X = X_poly.get_matrix_dense(clique.var_size) + + # check that vectorized consistency constraints hold everywhere. + for clique in admm_cliques: + g = np.vstack( + [Gi @ admm_cliques[vi].X.reshape(-1, 1) for vi, Gi in clique.G_dict.items()] + ) + np.testing.assert_allclose( + clique.F @ clique.X.reshape(-1, 1), + -g, + ) + print("consistency tests passed") + + +def test_problem(): + + problem = create_admm_test_problem() + Q, Constraints = problem.get_problem_matrices() + X, *_ = solve_sdp(Q, Constraints) + print(X) + plt.matshow(X) + x, info_rank = rank_project(X) + np.testing.assert_allclose( + x.flatten(), [1.0, 1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 0.0, 0.0], atol=1e-5 + ) + print("done") + + +def plot_admm(): + problem = create_admm_test_problem() + + clique_data = [{"h", f"x_{i}", f"x_{i+1}"} for i in range(3)] + problem.clique_decomposition(clique_data=clique_data) + + # C_dict = problem.decompose_matrix(problem.C, method="greedy-cover") + # C_dict = problem.decompose_matrix(problem.C, method="first") + C_dict = problem.decompose_matrix(problem.C, method="split") + fig, axs = plt.subplots(1, len(C_dict), squeeze=False) + axs = {i: ax for i, ax in zip(C_dict, axs[0, :])} + for i, C in C_dict.items(): + C.matshow(ax=axs[i]) + axs[i].set_title(f"clique {i}") + # plt.show(block=False) + + A_dicts = [] + for A in problem.As: + A_dict = problem.decompose_matrix(A, method="first") + fig, axs = plt.subplots(1, len(A_dict), squeeze=False) + axs = {i: ax for i, ax in zip(A_dict, axs[0, :])} + for i, A in A_dict.items(): + A.matshow(ax=axs[i]) + axs[i].set_title(f"clique {i}") + A_dicts.append(A_dict) + plt.close("all") + + eq_list = problem.get_consistency_constraints() + assert len(eq_list) == 2 * 6 + counter = {(1, 0): 0, (2, 1): 0} + plots = {(1, 0): plt.subplots(2, 6)[1], (2, 1): plt.subplots(2, 6)[1]} + for k, l, Ak, Al in eq_list: + # Ak.matshow(ax=plots[(k, l)][0, counter[(k, l)]]) + # Al.matshow(ax=plots[(k, l)][1, counter[(k, l)]]) + plots[(k, l)][0, counter[(k, l)]].matshow(Ak.toarray()) + plots[(k, l)][1, counter[(k, l)]].matshow(Al.toarray()) + counter[(k, l)] += 1 + return + + +def test_admm(): + problem = create_admm_test_problem() + + Q, Constraints = problem.get_problem_matrices() + X, info_SDP = solve_sdp(Q, Constraints) + print("cost SDP", info_SDP["cost"]) + + admm_cliques = ADMMClique.create_admm_cliques_from_problem(problem, variable=["x_"]) + solution, info = solve_alternating(admm_cliques, verbose=True, maxiter=10) + print("cost ADMM", info["cost"]) + + +def notest_fusion(): + from cert_tools.fusion_tools import mat_fusion, svec_fusion + + problem = create_admm_test_problem() + Q = problem.C.get_matrix(problem.var_sizes) + + Q_fusion = mat_fusion(Q) + q_fusion = svec_fusion(Q_fusion) + + q = Q.toarray()[np.triu_indices[Q.shape[0]]] + np.testing.assert_allclose(q, q_fusion) + + +if __name__ == "__main__": + # plot_admm() + # test_consistency() + test_admm() + # test_problem() diff --git a/_test/test_cliques.py b/_test/test_cliques.py index 9c01651..68e17fc 100644 --- a/_test/test_cliques.py +++ b/_test/test_cliques.py @@ -4,8 +4,8 @@ from poly_matrix import PolyMatrix from cert_tools import HomQCQP -from cert_tools.test_tools import (constraints_test, cost_test, - get_chain_rot_prob) +from cert_tools.base_clique import get_chain_clique_data +from cert_tools.test_tools import constraints_test, cost_test, get_chain_rot_prob root_dir = os.path.abspath(os.path.dirname(__file__) + "/../") @@ -48,7 +48,6 @@ def test_symmetric(): def test_constraint_decomposition(): - problem = get_chain_rot_prob() problem.clique_decomposition() constraints_test(problem) @@ -86,26 +85,8 @@ def test_cost_decomposition(): assert set(clique.var_list).difference(var_list) == set() -def get_chain_clique_data(var_sizes, fixed=["h"], variable=["x_", "z_"]): - clique_data = [] - indices = [ - int(v.split(variable[0])[1].strip("_")) for v in var_sizes if variable[0] in v - ] - # debug start - if len(variable) > 1: - for v in variable: - for i in indices: - assert f"{v}{i}" in var_sizes - # debug end - for i, j in zip(indices[:-1], indices[1:]): - clique_data.append( - set(fixed + [f"{v}{i}" for v in variable] + [f"{v}{j}" for v in variable]) - ) - return clique_data - - def test_fixed_decomposition(): - """Example of how to do a clique decomposition keeping the order of variables within + """Example of how to do a clique decomposition keeping the order of variables within each clique.""" problem = HomQCQP() problem.C, var_sizes = generate_random_matrix() diff --git a/cert_tools/admm_clique.py b/cert_tools/admm_clique.py index 91ba909..fbb761a 100644 --- a/cert_tools/admm_clique.py +++ b/cert_tools/admm_clique.py @@ -5,6 +5,8 @@ import scipy.sparse as sp from cert_tools.base_clique import BaseClique +from cert_tools.hom_qcqp import HomQCQP +from cert_tools.linalg_tools import svec from cert_tools.sdp_solvers import adjust_Q CONSTRAIN_ALL_OVERLAP = False @@ -46,34 +48,20 @@ class ADMMClique(BaseClique): def __init__( self, Q, - A_list: list, - b_list: list, + Constraints: list, var_dict: dict, index, X: np.ndarray = None, - N: int = 0, - hom="l", - x_dim: int = 0, - base_size: int = 1, + hom="h", ): - super().__init__( - Q=Q, - A_list=A_list, - b_list=b_list, - var_size=var_dict, - X=X, - index=index, - hom=hom, - ) - self.x_dim = x_dim - # usually, this corresponds to just the homogenization, which is shared. - # for the robust lifters and stereo, it also comprises the pose. - self.base_size = base_size - self.constrain_all = CONSTRAIN_ALL_OVERLAP - if self.constrain_all: - self.num_overlap = x_dim**2 + x_dim - else: - self.num_overlap = x_dim + self.Q = Q + self.Constraints = Constraints + + self.X_dim = Q.get_shape()[0] + self.var_size = var_dict + self.X = X + self.index = index + self.hom = hom self.status = 0 self.X_var = cp.Variable((self.X_dim, self.X_dim), PSD=True) @@ -81,171 +69,81 @@ def __init__( self.z_new = None self.z_prev = None - self.g = None - - assert N > 0, "must give total number of nodes N" - try: - self.F = self.generate_F(N=N) - self.E = self.get_E(N=N) - except: - self.E = None - - def get_E(self, N): - """Create the indexing matrix that picks the k-th out of N cliques. - - For our example, this matrix is of the form: - - k-th block - | - v - | 1 0 0 | - | 0 ... 1 0 ... | - | 0 ... 0 1 ... | - + @staticmethod + def create_admm_cliques_from_problem(problem: HomQCQP, variable=["x_", "z_"]): """ - data = np.ones(self.X_dim) - rows = range(self.X_dim) - columns = [0] + list( - range( - self.base_size + self.index * self.x_dim, - self.base_size + self.index * self.x_dim + 2 * self.x_dim, - ) - ) - shape = self.X_dim, 1 + N * self.x_dim - return sp.coo_matrix((data, [rows, columns]), shape=shape) - - def get_B_list_right(self): - B_list = [] - B = np.zeros((self.X_dim, self.X_dim)) - B[0, 0] = 1.0 - # B_list.append(B) - for i, j in itertools.combinations_with_replacement(range(self.x_dim), 2): - B = np.zeros((self.X_dim, self.X_dim)) - B[self.base_size + self.x_dim + i, self.base_size + self.x_dim + j] = 1.0 - B[self.base_size + self.x_dim + j, self.base_size + self.x_dim + i] = 1.0 - B_list.append(B) - for i in range(self.x_dim): - B = np.zeros((self.X_dim, self.X_dim)) - B[0, self.base_size + self.x_dim + i] = 1.0 - B[self.base_size + self.x_dim + i, 0] = 1.0 - B_list.append(B) - return B_list + Generate the cliques to be used in ADMM. - def get_B_list_left(self): - B_list = [] - B = np.zeros((self.X_dim, self.X_dim)) - B[0, 0] = -1.0 - # B_list.append(B) - for i, j in itertools.combinations_with_replacement(range(self.x_dim), 2): - B = np.zeros((self.X_dim, self.X_dim)) - B[self.base_size + i, self.base_size + j] = -1.0 - B[self.base_size + j, self.base_size + i] = -1.0 - B_list.append(B) - for i in range(self.x_dim): - B = np.zeros((self.X_dim, self.X_dim)) - B[0, self.base_size + i] = -1.0 - B[self.base_size + i, 0] = -1.0 - B_list.append(B) - return B_list - - def generate_g(self, left=None, right=None): - """Generate vector for overlap constraints: F @ X.flatten() - g = 0""" - vec_all = None - if left is not None: - vec_all = self.F_right @ left.flatten() - if right is not None: - vec_new = self.F_left @ right.flatten() - if isinstance(right, np.ndarray): - vec_all = ( - np.hstack([vec_all, vec_new]) if vec_all is not None else vec_new - ) - elif isinstance(right, cp.Variable): - vec_all = ( - cp.hstack([vec_all, vec_new]) if vec_all is not None else vec_new - ) - else: - raise TypeError("unexpected type") - return vec_all - - def generate_F(self, N): - """Generate matrix for overlap constraints: F @ X.flatten() - g = 0""" - self.F_right = self.F_oneside(*self.get_slices_right()) - self.F_left = self.F_oneside(*self.get_slices_left()) - if self.index == 0: - self.F = self.F_right - elif self.index == N - 1: - self.F = self.F_left - else: - self.F = sp.vstack([self.F_left, self.F_right]) - self.sigmas = np.zeros(self.F.shape[0]) + The generated F and G matrices are the vectorized versions of the consistency constraints. + They are such that F @ this.flatten() + G @ other.flatten() == 0, where other is the stack + of all flattened cliques at the indices given in variables_FG. + """ + from cert_tools.base_clique import get_chain_clique_data - def F_oneside(self, starts, ends): - """Picks the right node of the clique.""" - n_constraints = sum( - (end[0] - start[0]) * (end[1] - start[1]) - for start, end in zip(starts, ends) - ) - assert n_constraints == self.num_overlap - counter = 0 - i_list = [] - j_list = [] - data = [] # for testing only - for start, end in zip(starts, ends): - for i in range(start[0], end[0]): - for j in range(start[1], end[1]): - i_list.append(counter) - j_list.append(i * self.X_dim + j) - counter += 1 - if self.X is not None: - data.append(self.X[i, j]) - assert counter == n_constraints - F_oneside = sp.csr_array( - ([1] * len(i_list), (i_list, j_list)), - shape=(n_constraints, self.X_var.size), + clique_data = get_chain_clique_data( + problem.var_sizes, fixed=["h"], variable=variable ) - if self.X is not None: - np.testing.assert_allclose(F_oneside @ self.X.flatten(), data) - return F_oneside - - def generate_overlap_slices(self, N): - if self.index > 0: # the RIGHT side of the left node is overlapping - self.left_slice_start, self.left_slice_end = self.get_slices_right() - else: - self.left_slice_start = self.left_slice_end = [[0, 0]] - if self.index < N - 1: # the LEFT side of the right node is overlapping - self.right_slice_start, self.right_slice_end = self.get_slices_left() - else: - self.right_slice_start = self.right_slice_end = [[0, 0]] - - def get_slices_right(self): - """Picks the right part of a node""" - left_slice_start = [[0, self.base_size + self.x_dim]] # i_start, i_end - left_slice_end = [[1, self.base_size + 2 * self.x_dim]] - if self.constrain_all: - left_slice_start += [ - [self.base_size + self.x_dim, self.base_size + self.x_dim] - ] - left_slice_end += [ - [self.base_size + 2 * self.x_dim, self.base_size + 2 * self.x_dim] - ] - return left_slice_start, left_slice_end + problem.clique_decomposition(clique_data=clique_data) + + eq_list = problem.get_consistency_constraints() + + Q_dict = problem.decompose_matrix(problem.C, method="split") + A_dict_list = [problem.decompose_matrix(A, method="first") for A in problem.As] + admm_cliques = [] + for clique in problem.cliques: + Constraints = [problem.get_homog_constraint(clique.var_sizes)] + for A_dict in A_dict_list: + if clique.index in A_dict.keys(): + Constraints.append( + (A_dict[clique.index].get_matrix(clique.var_sizes), 0.0) + ) + admm_clique = ADMMClique( + Q=Q_dict[clique.index].get_matrix(clique.var_sizes), + Constraints=Constraints, + var_dict=clique.var_sizes, + index=clique.index, + hom="h", + N=len(clique_data), + ) - def get_slices_left(self): - """Picks the left part of a node""" - right_slice_start = [[0, self.base_size]] - right_slice_end = [[1, self.base_size + self.x_dim]] - if self.constrain_all: - right_slice_start += [[self.base_size, self.base_size]] - right_slice_end += [ - [self.base_size + self.x_dim, self.base_size + self.x_dim] - ] - return right_slice_start, right_slice_end + # find all overlapping constraints involving this clique + # + = 0 + # F @ vech(Xk) + G @ vech(Xl) = 0 + F = dict() + G = dict() + for k, l, Ak, Al in eq_list: + if k == clique.index: + if l in F: + # TODO(FD) we currently need to use the full matrix and not just the upper half + # because it's not trivial to extract the upper half of a matrix in the Fusion API. + # We might change that later. + F[l] = sp.vstack([F[l], Ak.reshape(1, -1)]) + G[l] = sp.vstack([G[l], Al.reshape(1, -1)]) + else: + F[l] = Ak.reshape(1, -1) + G[l] = Al.reshape(1, -1) + # TODO(FD) I am not 100% this is needed. For dSDP this would be counting constraints double. + # But it seems like for ADMM it is required, because for example, the first clique in a chain + # would otherwise not be linked to any other cliques through consensus constraints. + elif l == clique.index: + if k in F: + F[k] = sp.vstack([F[k], Al.reshape(1, -1)]) + G[k] = sp.vstack([G[k], Ak.reshape(1, -1)]) + else: + F[k] = Al.reshape(1, -1) + G[k] = Ak.reshape(1, -1) + + admm_clique.F = sp.vstack(F.values()) if len(F) else None + admm_clique.G_dict = G + admm_clique.sigmas = np.zeros(admm_clique.F.shape[0]) if len(F) else None + admm_cliques.append(admm_clique) + return admm_cliques def current_error(self): - return np.sum(np.abs(self.F @ self.X_new - self.g)) + return np.sum(np.abs(self.F @ self.X_new - self.z_new)) def get_constraints_cvxpy(self, X): - return [cp.trace(A_k @ X) == b_k for A_k, b_k in zip(self.A_list, self.b_list)] + return [cp.trace(A_k @ X) == b_k for A_k, b_k in zip(self.Constraints)] def get_objective_cvxpy(self, X, rho_k, adjust=False, scale_method="fro"): if adjust: diff --git a/cert_tools/admm_solvers.py b/cert_tools/admm_solvers.py index 2ed5b29..5c5fcf8 100644 --- a/cert_tools/admm_solvers.py +++ b/cert_tools/admm_solvers.py @@ -6,6 +6,7 @@ import cvxpy as cp import mosek.fusion as fu import numpy as np + from cert_tools.admm_clique import ADMMClique, update_rho from cert_tools.fusion_tools import mat_fusion, read_costs_from_mosek from cert_tools.sdp_solvers import ( @@ -28,8 +29,6 @@ TAU_RHO = 2.0 # how much to change rho in each iteration. EPS_ABS = 0.0 # set to 0 to use relative only EPS_REL = 1e-10 -INDIVIDUAL_RHO = False # use different rho for each clique -VECTOR_RHO = False # use different rho for each error term. UPDATE_RHO = True # if False, never update rho at all # Stop ADMM if the last N_ADMM iterations don't have significant change in cost. @@ -53,10 +52,14 @@ def initialize_admm(clique_list, X0=None, rho_start=None): clique.rho_k = rho_start clique.counter = 0 clique.status = 0 - if X0 is not None: - clique.z_new = deepcopy(clique.F @ X0[k].flatten()) - else: - clique.z_new = np.zeros(clique.F.shape[0]) + if clique.G_dict is not None: + if X0 is not None: + other = np.vstack( + [Gi @ X0[vi].reshape(-1, 1) for vi, Gi in clique.G_dict.items()] + ) + clique.z_new = clique.G @ other + else: + clique.z_new = np.zeros(clique.F.shape[0]) def check_convergence(clique_list, primal_err, dual_err): @@ -78,24 +81,26 @@ def check_convergence(clique_list, primal_err, dual_err): def update_z(clique_list): """Average the overlapping areas of X_new for consensus (stored in Z_new).""" - for i, c in enumerate(clique_list): - this = c.X_new.flatten() - left = clique_list[i - 1].X_new.flatten() if i > 0 else None - right = clique_list[i + 1].X_new.flatten() if i < len(clique_list) - 1 else None + for c in clique_list: c.z_prev = deepcopy(c.z_new) + other_contributions = np.hstack( + [Gi @ clique_list[vi].X_new.flatten() for vi, Gi in c.G_dict.items()] + ) + c.z_new = 0.5 * (c.F @ c.X_new.flatten() - other_contributions) + # for each Z, average over the neighboring cliques. - if i == 0: - c.z_new = 0.5 * (c.F_right @ this + c.F_left @ right) - elif i == len(clique_list) - 1: - c.z_new = 0.5 * (c.F_right @ left + c.F_left @ this) - else: - c.z_new = 0.5 * np.hstack( - [ - c.F_right @ left + c.F_left @ this, - c.F_right @ this + c.F_left @ right, - ] - ) + # if i == 0: + # c.z_new = 0.5 * (c.F_right @ this + c.F_left @ right) + # elif i == len(clique_list) - 1: + # c.z_new = 0.5 * (c.F_right @ left + c.F_left @ this) + # else: + # c.z_new = 0.5 * np.hstack( + # [ + # c.F_right @ left + c.F_left @ this, + # c.F_right @ this + c.F_left @ right, + # ] + # ) assert c.z_new.shape == c.z_prev.shape @@ -103,8 +108,7 @@ def update_sigmas(clique_list): """Update sigmas (dual variables) and residual terms.""" for clique in clique_list: assert isinstance(clique, ADMMClique) - g = clique.z_new - clique.primal_res_k = clique.F @ clique.X_new.flatten() - g + clique.primal_res_k = clique.F @ clique.X_new.flatten() - clique.z_new clique.dual_res_k = clique.rho_k * (clique.z_new - clique.z_prev) clique.sigmas += clique.rho_k * clique.primal_res_k @@ -176,14 +180,8 @@ def solve_inner_sdp_fusion( scale = 1.0 if F is not None: assert g is not None - if F.shape[1] == Q.shape[0]: - err = fu.Expr.sub( - fu.Expr.mul(F, X.slice([0, 0], [Q.shape[0], 1])), - fu.Matrix.dense(g.value[:, None]), - ) - else: - F_fu = mat_fusion(F) - err = fu.Expr.sub(fu.Expr.mul(F_fu, fu.Expr.flatten(X)), g) + F_fu = mat_fusion(F) + err = fu.Expr.sub(fu.Expr.mul(F_fu, fu.Expr.flatten(X)), g) # doesn't work unforuntately: # Expr.mul(0.5 * rho, Expr.sum(Expr.mulElm(err, err))), @@ -218,7 +216,7 @@ def solve_inner_sdp_fusion( fu.Domain.equalsTo(0.0), ) else: - M.objective(fu.ObjectiveSense.Minimize, fu.Expr.dot(Q_here, X)) + M.objective(fu.ObjectiveSense.Minimize, fu.Expr.dot(mat_fusion(Q_here), X)) adjust_tol_fusion(options_fusion, tol) options_fusion["intpntCoTolRelGap"] = tol * 10 @@ -253,16 +251,15 @@ def solve_inner_sdp( s.t. = bi X >= 0 - where e(X) = F @ vec(X) - b + where e(X) = F @ vec(X) - g """ if adjust: print("Warning: adjusting Q is currently not fully working for inner sdp") if use_fusion: - Constraints = list(zip(clique.A_list, clique.b_list)) return solve_inner_sdp_fusion( clique.Q, - Constraints, + clique.Constraints, clique.F, clique.g, clique.sigmas, @@ -305,20 +302,14 @@ def solve_alternating( maxiter=MAXITER, mu_rho=MU_RHO, tau_rho=TAU_RHO, - individual_rho=INDIVIDUAL_RHO, - vector_rho=VECTOR_RHO, ): """Use ADMM to solve decomposed SDP, but without using parallelism.""" - assert not (vector_rho and not individual_rho), "incompatible combination" if sigmas is not None: for k, sigma in sigmas.items(): clique_list[k].sigmas = sigma for c in clique_list: - if vector_rho: - c.rho_k = np.full(c.F.shape[0], rho_start).astype(float) - else: - c.rho_k = rho_start + c.rho_k = rho_start # rho_k = rho_start info_here = {"success": False, "msg": "did not converge.", "stop": False} @@ -333,9 +324,7 @@ def solve_alternating( # ADMM step 1: update X for k, clique in enumerate(clique_list): assert isinstance(clique, ADMMClique) # for debugging only - # update g with solved value from previous iteration - clique.g = clique_list[k].z_new - assert clique.g is not None + clique.g = clique.z_new X, info = solve_inner_sdp( clique, @@ -366,15 +355,11 @@ def solve_alternating( # update rho if UPDATE_RHO: - if individual_rho: - for c in clique_list: - c.update_rho(mu_rho=mu_rho, tau_rho=tau_rho) - else: - rho_new = update_rho( - clique_list[0].rho_k, dual_err, primal_err, mu=MU_RHO, tau=TAU_RHO - ) - for c in clique_list: - c.rho_k = rho_new + rho_new = update_rho( + clique_list[0].rho_k, dual_err, primal_err, mu=mu_rho, tau=tau_rho + ) + for c in clique_list: + c.rho_k = rho_new info_here["cost"] = cost_original cost_history.append(cost_original) @@ -407,6 +392,8 @@ def solve_parallel( maxiter=MAXITER, use_fusion=False, verbose=False, + mu_rho=MU_RHO, + tau_rho=TAU_RHO, ): """Use ADMM to solve decomposed SDP, with simple parallelism.""" @@ -496,17 +483,11 @@ def run_worker(cliques_per_pipe, pipe): # Intermediate steps: update rho and check convergence if UPDATE_RHO: - if INDIVIDUAL_RHO: - for c in clique_list: - c.update_rho( - mu_rho=MU_RHO, tau_rho=TAU_RHO, individual_rho=INDIVIDUAL_RHO - ) - else: - rho_new = update_rho( - clique_list[0].rho_k, dual_err, primal_err, mu=MU_RHO, tau=TAU_RHO - ) - for c in clique_list: - c.rho_k = rho_new + rho_new = update_rho( + clique_list[0].rho_k, dual_err, primal_err, mu=mu_rho, tau=tau_rho + ) + for c in clique_list: + c.rho_k = rho_new cost_original = np.sum( [np.trace(clique.X_new @ clique.Q) for clique in clique_list] ) diff --git a/cert_tools/base_clique.py b/cert_tools/base_clique.py index 2a50144..63b1fe4 100644 --- a/cert_tools/base_clique.py +++ b/cert_tools/base_clique.py @@ -4,6 +4,24 @@ OVERLAP_ALL = False +def get_chain_clique_data(var_sizes, fixed=["h"], variable=["x_", "z_"]): + clique_data = [] + indices = [ + int(v.split(variable[0])[1].strip("_")) for v in var_sizes if variable[0] in v + ] + # debug start + if len(variable) > 1: + for v in variable: + for i in indices: + assert f"{v}{i}" in var_sizes + # debug end + for i, j in zip(indices[:-1], indices[1:]): + clique_data.append( + set(fixed + [f"{v}{i}" for v in variable] + [f"{v}{j}" for v in variable]) + ) + return clique_data + + class BaseClique(object): """Base class used to represent a clique in the aggregate sparsity graph (ASG). Used to store useful information about the clique, namely diff --git a/cert_tools/fusion_tools.py b/cert_tools/fusion_tools.py index 4ba61cc..6aa0915 100644 --- a/cert_tools/fusion_tools.py +++ b/cert_tools/fusion_tools.py @@ -21,6 +21,13 @@ def get_slice(X: Matrix, i: int): return X.slice([i, 0, 0], [i + 1, X_dim, X_dim]).reshape([X_dim, X_dim]) +def svec_fusion(X): + """Not working""" + N = X.numRows() + assert isinstance(X, Matrix) + return [X.index(i, j) for (i, j) in zip(*np.triu_indices(N))] + + # TODO(FD) not used anymore as now we are setting accSolutionStatus to Anything. # Before, this was used to read from UNKNOWN problem status. def read_costs_from_mosek(fname): diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index 05e8168..b280262 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -8,13 +8,13 @@ import plotly.graph_objects as go import plotly.io as pio import scipy.sparse as sp -from cert_tools.base_clique import BaseClique -from cert_tools.linalg_tools import find_dependent_columns, svec from cvxopt import amd, spmatrix from igraph import Graph +from poly_matrix import PolyMatrix from scipy.linalg import polar -from poly_matrix import PolyMatrix +from cert_tools.base_clique import BaseClique +from cert_tools.linalg_tools import find_dependent_columns, svec class HomQCQP(object): @@ -322,6 +322,20 @@ def process_clique_data(clique_data): return clique_list, separators, parents + def get_admm_cliques(self): + from cert_tools.admm_clique import ADMMClique + + admm_cliques = [] + for clique in self.cliques: + admm_cliques.append(ADMMClique.init_from_clique(clique)) + + def get_homog_constraint(self, var_sizes=None): + if var_sizes is None: + var_sizes = self.var_sizes + Ah = PolyMatrix() + Ah[self.h, self.h] = 1 + return (Ah.get_matrix(var_sizes), 1.0) + def get_problem_matrices(self): """Get sparse, numerical form of objective and constraint matrices for use in optimization""" @@ -330,10 +344,7 @@ def get_problem_matrices(self): # Define other constraints constraints = [(A.get_matrix(self.var_sizes), 0.0) for A in self.As] # define homogenizing constraint - Ah = PolyMatrix() - Ah[self.h, self.h] = 1 - homog_constraint = (Ah.get_matrix(self.var_sizes), 1.0) - constraints.append(homog_constraint) + constraints.append(self.get_homog_constraint()) return cost, constraints def get_standard_form(self, vec_order="C"): @@ -417,7 +428,14 @@ def get_consistency_constraints(self): return eq_list def decompose_matrix(self, pmat: PolyMatrix, method="split"): - """Decompose a matrix according to clique decomposition. Returns a dictionary with the key being the clique number and the value being a PolyMatrix that contains decomposed matrix on that clique.""" + """Decompose a matrix according to clique decomposition. + + Returns a dictionary with the key being the clique number and the value being a + PolyMatrix that contains decomposed matrix on that clique. + + Args: + method (str): "split" means equal split between overlapping, "first" means first takes all, "greedy-cover" uses a smart algorithm to split. + """ assert isinstance(pmat, PolyMatrix), TypeError("Input should be a PolyMatrix") assert pmat.symmetric, ValueError("PolyMatrix input should be symmetric") diff --git a/cert_tools/test_tools.py b/cert_tools/test_tools.py index e06d856..3584855 100644 --- a/cert_tools/test_tools.py +++ b/cert_tools/test_tools.py @@ -1,14 +1,14 @@ import numpy as np -from cert_tools import HomQCQP +from poly_matrix import PolyMatrix from pylgmath import so3op -from poly_matrix import PolyMatrix +from cert_tools import HomQCQP # Global Defaults ER_MIN = 1e6 -def cost_test(problem): +def cost_test(problem: HomQCQP): variables = problem.C.get_variables() C = PolyMatrix() mat_decomp = problem.decompose_matrix(problem.C, method="split") @@ -19,8 +19,9 @@ def cost_test(problem): ) -def constraints_test(problem): +def constraints_test(problem: HomQCQP): for A_gt in problem.As: + assert isinstance(A_gt, PolyMatrix) variables = A_gt.get_variables() A = PolyMatrix() mat_decomp = problem.decompose_matrix(A_gt, method="split") From 2cce5b144b1e04d1ae0e02738c9491fa9694707e Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 4 Dec 2024 19:14:44 +0100 Subject: [PATCH 08/15] Change ADMM from z to g, remove unnecessary code. --- _test/test_admm.py | 54 ++++++++++++++++++----- cert_tools/admm_clique.py | 36 ++++++++------- cert_tools/admm_solvers.py | 89 +++++++++++++++++--------------------- 3 files changed, 99 insertions(+), 80 deletions(-) diff --git a/_test/test_admm.py b/_test/test_admm.py index 7e3c764..c91dd0e 100644 --- a/_test/test_admm.py +++ b/_test/test_admm.py @@ -41,9 +41,8 @@ from cert_tools.admm_clique import ADMMClique from cert_tools.admm_solvers import solve_alternating from cert_tools.hom_qcqp import HomQCQP -from cert_tools.linalg_tools import rank_project, svec +from cert_tools.linalg_tools import rank_project from cert_tools.sdp_solvers import solve_sdp -from cert_tools.test_tools import get_chain_rot_prob def create_admm_test_problem(): @@ -77,6 +76,12 @@ def create_admm_test_problem(): < [0 0-1 0 0], X_i > = 0 i = 1, 2, 3 [0 0 0 0 0] [0 0 0 0 0] + + [0 1-1 0 0] + [0 0 0 0 0] + < [0 0 0 0 0], X_i > = 0 i = 1, 2, 3 + [0 0 0 0 0] + [0 0 0 0 0] """ A_1a = PolyMatrix() A_1a["x_0", "x_0"] = np.array([[1.0, 0.0], [0.0, -1.0]]) @@ -109,7 +114,6 @@ def test_consistency(): Q, Constraints = problem.get_problem_matrices() X, *_ = solve_sdp(Q, Constraints) X_poly, __ = PolyMatrix.init_from_sparse(X, var_dict=problem.var_sizes) - for clique in admm_cliques: clique.X = X_poly.get_matrix_dense(clique.var_size) @@ -126,12 +130,9 @@ def test_consistency(): def test_problem(): - problem = create_admm_test_problem() Q, Constraints = problem.get_problem_matrices() X, *_ = solve_sdp(Q, Constraints) - print(X) - plt.matshow(X) x, info_rank = rank_project(X) np.testing.assert_allclose( x.flatten(), [1.0, 1.0, 1.0, 2.0, 2.0, 3.0, 3.0, 0.0, 0.0], atol=1e-5 @@ -179,16 +180,46 @@ def plot_admm(): return +def test_update_z(): + from cert_tools.admm_solvers import initialize_admm, update_g + + """ If the variables are already consistent, then update_z should have no effect. """ + problem = create_admm_test_problem() + admm_cliques = ADMMClique.create_admm_cliques_from_problem(problem, variable=["x_"]) + + x = np.array([1.0, 1, 1, 2, 2, 3, 3, 0, 0]) + X = np.outer(x, x) + X0 = problem.get_X0(X) + for clique in admm_cliques: + clique.X_new = X0[clique.index] + + initialize_admm(admm_cliques, X0=X0) + update_g(admm_cliques) + for clique in admm_cliques: + np.testing.assert_allclose(clique.g_prev, clique.g) + print("test passed") + + def test_admm(): problem = create_admm_test_problem() + admm_cliques = ADMMClique.create_admm_cliques_from_problem(problem, variable=["x_"]) + # get the one-shot SDP solution Q, Constraints = problem.get_problem_matrices() X, info_SDP = solve_sdp(Q, Constraints) print("cost SDP", info_SDP["cost"]) - admm_cliques = ADMMClique.create_admm_cliques_from_problem(problem, variable=["x_"]) - solution, info = solve_alternating(admm_cliques, verbose=True, maxiter=10) - print("cost ADMM", info["cost"]) + # create initialization + X0 = problem.get_X0(X) + + # get the ADMM solution + X_list, info = solve_alternating( + admm_cliques, verbose=True, maxiter=100, X0=X0, adjust=False + ) + + # TODO(FD): very inaccurate -- is this normal? + x_minrank, *_ = problem.get_mr_completion(X_list, rank_tol=10) + np.testing.assert_allclose(x_minrank[:, 0], [1, 1, 1, 2, 2, 3, 3, 0, 0], atol=0.1) def notest_fusion(): @@ -206,6 +237,7 @@ def notest_fusion(): if __name__ == "__main__": # plot_admm() - # test_consistency() + test_problem() + test_consistency() + test_update_z() test_admm() - # test_problem() diff --git a/cert_tools/admm_clique.py b/cert_tools/admm_clique.py index fbb761a..cdd4656 100644 --- a/cert_tools/admm_clique.py +++ b/cert_tools/admm_clique.py @@ -1,12 +1,9 @@ -import itertools - import cvxpy as cp import numpy as np import scipy.sparse as sp from cert_tools.base_clique import BaseClique from cert_tools.hom_qcqp import HomQCQP -from cert_tools.linalg_tools import svec from cert_tools.sdp_solvers import adjust_Q CONSTRAIN_ALL_OVERLAP = False @@ -103,39 +100,40 @@ def create_admm_cliques_from_problem(problem: HomQCQP, variable=["x_", "z_"]): var_dict=clique.var_sizes, index=clique.index, hom="h", - N=len(clique_data), ) # find all overlapping constraints involving this clique # + = 0 # F @ vech(Xk) + G @ vech(Xl) = 0 - F = dict() - G = dict() + F_dict = dict() + G_dict = dict() for k, l, Ak, Al in eq_list: if k == clique.index: - if l in F: + if l in F_dict: # TODO(FD) we currently need to use the full matrix and not just the upper half # because it's not trivial to extract the upper half of a matrix in the Fusion API. # We might change that later. - F[l] = sp.vstack([F[l], Ak.reshape(1, -1)]) - G[l] = sp.vstack([G[l], Al.reshape(1, -1)]) + F_dict[l] = sp.vstack([F_dict[l], Ak.reshape(1, -1)]) + G_dict[l] = sp.vstack([G_dict[l], Al.reshape(1, -1)]) else: - F[l] = Ak.reshape(1, -1) - G[l] = Al.reshape(1, -1) + F_dict[l] = Ak.reshape(1, -1) + G_dict[l] = Al.reshape(1, -1) # TODO(FD) I am not 100% this is needed. For dSDP this would be counting constraints double. # But it seems like for ADMM it is required, because for example, the first clique in a chain # would otherwise not be linked to any other cliques through consensus constraints. elif l == clique.index: - if k in F: - F[k] = sp.vstack([F[k], Al.reshape(1, -1)]) - G[k] = sp.vstack([G[k], Ak.reshape(1, -1)]) + if k in F_dict: + F_dict[k] = sp.vstack([F_dict[k], Al.reshape(1, -1)]) + G_dict[k] = sp.vstack([G_dict[k], Ak.reshape(1, -1)]) else: - F[k] = Al.reshape(1, -1) - G[k] = Ak.reshape(1, -1) + F_dict[k] = Al.reshape(1, -1) + G_dict[k] = Ak.reshape(1, -1) - admm_clique.F = sp.vstack(F.values()) if len(F) else None - admm_clique.G_dict = G - admm_clique.sigmas = np.zeros(admm_clique.F.shape[0]) if len(F) else None + admm_clique.F = sp.vstack(F_dict.values()) if len(F_dict) else None + admm_clique.G_dict = G_dict + admm_clique.sigmas = ( + np.zeros(admm_clique.F.shape[0]) if len(F_dict) else None + ) admm_cliques.append(admm_clique) return admm_cliques diff --git a/cert_tools/admm_solvers.py b/cert_tools/admm_solvers.py index 5c5fcf8..7800477 100644 --- a/cert_tools/admm_solvers.py +++ b/cert_tools/admm_solvers.py @@ -46,20 +46,24 @@ N_THREADS = 4 # np.inf -def initialize_admm(clique_list, X0=None, rho_start=None): - """Initialize Z (consensus variable) based on contents of X0 (initial feasible points)""" +def initialize_admm(clique_list, X0=None): + """Initialize g based on contents of X0 (initial feasible points)""" for k, clique in enumerate(clique_list): - clique.rho_k = rho_start clique.counter = 0 clique.status = 0 if clique.G_dict is not None: if X0 is not None: + # for debugging only + clique.X = X0[clique.index] + + # just a sanity chak that X0 is actually valid. other = np.vstack( [Gi @ X0[vi].reshape(-1, 1) for vi, Gi in clique.G_dict.items()] ) - clique.z_new = clique.G @ other + clique.g = -clique.F @ X0[clique.index].flatten() + np.testing.assert_allclose(other.flatten(), clique.g) else: - clique.z_new = np.zeros(clique.F.shape[0]) + clique.g = np.zeros(clique.F.shape[0]) def check_convergence(clique_list, primal_err, dual_err): @@ -68,7 +72,7 @@ def check_convergence(clique_list, primal_err, dual_err): dim_dual = len(clique_list) * clique_list[0].X_dim eps_max = np.max( [ - np.max(np.hstack([clique.X_new.flatten(), clique.z_new])) + np.max(np.hstack([clique.X_new.flatten(), clique.g])) for clique in clique_list[:-1] ] ) @@ -79,38 +83,24 @@ def check_convergence(clique_list, primal_err, dual_err): return (primal_err < eps_pri) and (dual_err < eps_dual) -def update_z(clique_list): +def update_g(clique_list): """Average the overlapping areas of X_new for consensus (stored in Z_new).""" for c in clique_list: - - c.z_prev = deepcopy(c.z_new) + c.g_prev = deepcopy(c.g) other_contributions = np.hstack( [Gi @ clique_list[vi].X_new.flatten() for vi, Gi in c.G_dict.items()] ) - c.z_new = 0.5 * (c.F @ c.X_new.flatten() - other_contributions) - - # for each Z, average over the neighboring cliques. - # if i == 0: - # c.z_new = 0.5 * (c.F_right @ this + c.F_left @ right) - # elif i == len(clique_list) - 1: - # c.z_new = 0.5 * (c.F_right @ left + c.F_left @ this) - # else: - # c.z_new = 0.5 * np.hstack( - # [ - # c.F_right @ left + c.F_left @ this, - # c.F_right @ this + c.F_left @ right, - # ] - # ) - assert c.z_new.shape == c.z_prev.shape - - -def update_sigmas(clique_list): + c.g = 0.5 * (-c.F @ c.X_new.flatten() + other_contributions) + assert c.g.shape == c.g_prev.shape + + +def update_sigmas(clique_list, rho_k): """Update sigmas (dual variables) and residual terms.""" for clique in clique_list: assert isinstance(clique, ADMMClique) - clique.primal_res_k = clique.F @ clique.X_new.flatten() - clique.z_new - clique.dual_res_k = clique.rho_k * (clique.z_new - clique.z_prev) - clique.sigmas += clique.rho_k * clique.primal_res_k + clique.primal_res_k = clique.F @ clique.X_new.flatten() + clique.g + clique.dual_res_k = rho_k * (clique.g - clique.g_prev) + clique.sigmas += rho_k * clique.primal_res_k # TODO(FD): this function is hideous. Can we simplify / remove it somehow? @@ -181,7 +171,7 @@ def solve_inner_sdp_fusion( if F is not None: assert g is not None F_fu = mat_fusion(F) - err = fu.Expr.sub(fu.Expr.mul(F_fu, fu.Expr.flatten(X)), g) + err = fu.Expr.add(fu.Expr.mul(F_fu, fu.Expr.flatten(X)), g) # doesn't work unforuntately: # Expr.mul(0.5 * rho, Expr.sum(Expr.mulElm(err, err))), @@ -247,16 +237,23 @@ def solve_inner_sdp( ): """Solve the inner SDP of the ADMM algorithm, similar to [Dall'Anese 2013] - min + y'e(X) + rho/2*||e(X)||^2 - s.t. = bi + Each clique j keeps track of g_j = [G_1 @ Z_1; ...; G_N @ Z_N] where the cliques Z_i, G_i + designate clique that have overlap with j, such that F @ X_i = g_j. + + min + y'e(X_j) + rho/2*||e(X_j)||^2 + s.t. = bi # primary X >= 0 - where e(X) = F @ vec(X) - g + where e(X_j) = F @ vec(X_j) + g_j """ if adjust: print("Warning: adjusting Q is currently not fully working for inner sdp") if use_fusion: + # for debugging only + err = clique.F @ clique.X.flatten() + clique.g + # print(f"current error: {err}") + return solve_inner_sdp_fusion( clique.Q, clique.Constraints, @@ -307,16 +304,14 @@ def solve_alternating( if sigmas is not None: for k, sigma in sigmas.items(): clique_list[k].sigmas = sigma - - for c in clique_list: - c.rho_k = rho_start + rho_k = rho_start # rho_k = rho_start info_here = {"success": False, "msg": "did not converge.", "stop": False} cost_history = [] - initialize_admm(clique_list, X0, rho_start=rho_start) # fill Z_new + initialize_admm(clique_list, X0) # fill Z_new for iter in range(maxiter): cost_lagrangian = 0 cost_original = 0 @@ -324,11 +319,9 @@ def solve_alternating( # ADMM step 1: update X for k, clique in enumerate(clique_list): assert isinstance(clique, ADMMClique) # for debugging only - clique.g = clique.z_new - X, info = solve_inner_sdp( clique, - clique.rho_k, + rho_k, verbose=False, use_fusion=use_fusion, tol=tol_inner, @@ -346,20 +339,16 @@ def solve_alternating( cost_lagrangian += cost # ADMM step 2: update Z - update_z(clique_list) + update_g(clique_list) # ADMM step 3: update Lagrange multipliers - update_sigmas(clique_list) + update_sigmas(clique_list, rho_k) primal_err = np.linalg.norm(np.hstack([c.primal_res_k for c in clique_list])) dual_err = np.linalg.norm(np.hstack([c.dual_res_k for c in clique_list])) # update rho if UPDATE_RHO: - rho_new = update_rho( - clique_list[0].rho_k, dual_err, primal_err, mu=mu_rho, tau=tau_rho - ) - for c in clique_list: - c.rho_k = rho_new + rho_k = update_rho(rho_k, dual_err, primal_err, mu=mu_rho, tau=tau_rho) info_here["cost"] = cost_original cost_history.append(cost_original) @@ -435,7 +424,7 @@ def run_worker(cliques_per_pipe, pipe): indices_per_pipe[k].append(i) # Initialize z of all cliques - initialize_admm(clique_list, X0, rho_start=rho_start) + initialize_admm(clique_list, X0) pipes = [] procs = [] @@ -474,7 +463,7 @@ def run_worker(cliques_per_pipe, pipe): clique_list[i].X_new = X_new_list[count] # ADMM step 2: update Z variables - update_z(clique_list) + update_g(clique_list) # ADMM step 3: update Lagrange multipliers update_sigmas(clique_list) From 7e4fa32dd30be039e073d5488e60cf73984270e0 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 4 Dec 2024 19:15:25 +0100 Subject: [PATCH 09/15] Fix homogeneous constraint and verbose kwarg --- _test/test_homqcqp.py | 8 ++++---- cert_tools/eopt_solvers_qp.py | 7 +++++-- cert_tools/hom_qcqp.py | 11 +++++++++-- cert_tools/test_tools.py | 7 ------- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/_test/test_homqcqp.py b/_test/test_homqcqp.py index 50ef7cb..9dd04e7 100644 --- a/_test/test_homqcqp.py +++ b/_test/test_homqcqp.py @@ -2,7 +2,7 @@ import unittest import numpy as np -#import pytest +from poly_matrix import PolyMatrix from cert_tools import HomQCQP from cert_tools.hom_qcqp import greedy_cover @@ -11,7 +11,7 @@ from cert_tools.sparse_solvers import solve_clarabel, solve_dsdp from cert_tools.test_tools import get_chain_rot_prob, get_loop_rot_prob -from poly_matrix import PolyMatrix +# import pytest class TestHomQCQP(unittest.TestCase): @@ -398,11 +398,11 @@ def test_solve_dual_dsdp(self, rank1=False): test = TestHomQCQP() # test.test_solve() # test.test_get_asg(plot=True) - test.test_clique_decomp(plot=False) + # test.test_clique_decomp(plot=False) # test.test_consistency_constraints() # test.test_greedy_cover() # test.test_decompose_matrix() # test.test_solve_primal_dsdp() # test.test_solve_dual_dsdp() - # test.test_standard_form() + test.test_standard_form() # test.test_clarabel() diff --git a/cert_tools/eopt_solvers_qp.py b/cert_tools/eopt_solvers_qp.py index c420383..2ab6ff5 100644 --- a/cert_tools/eopt_solvers_qp.py +++ b/cert_tools/eopt_solvers_qp.py @@ -1,6 +1,7 @@ import cvxpy as cp import mosek import numpy as np + from cert_tools.eig_tools import get_min_eigpairs from cert_tools.eopt_solvers import ( backtrack_cutoff, @@ -55,7 +56,8 @@ def solve_d_from_indefinite_U(U, Q_1, A_vec, verbose=False): prob = cp.Problem(cp.Minimize(1), constraints=constraint) try: - prob.solve(solver="MOSEK", verbose=verbose > 2, **options_cvxpy) + options_cvxpy["verbose"] = verbose > 2 + prob.solve(solver="MOSEK", **options_cvxpy) except mosek.Error: print("Did not find MOSEK, using different solver.") prob.solve(solver="CVXOPT", verbose=verbose > 2) @@ -111,7 +113,8 @@ def solve_inner_QP(vecs, eigs, A_vec, t, rho, W, verbose=False, lmin=False): prob = cp.Problem(cp.Minimize(delta + d.T @ W @ d), constraints=constraints) try: - prob.solve(solver="MOSEK", verbose=verbose > 2, **options_cvxpy) + options_cvxpy["verbose"] = verbose > 2 + prob.solve(solver="MOSEK", **options_cvxpy) except mosek.Error: print("Did not find MOSEK, using different solver.") prob.solve(solver="CVXOPT", verbose=verbose > 2) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index b280262..2916105 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -322,6 +322,13 @@ def process_clique_data(clique_data): return clique_list, separators, parents + def get_X0(self, X): + X0 = {} + X_poly, __ = PolyMatrix.init_from_sparse(X, var_dict=self.var_sizes) + for clique in self.cliques: + X0[clique.index] = X_poly.get_matrix_dense(clique.var_sizes) + return X0 + def get_admm_cliques(self): from cert_tools.admm_clique import ADMMClique @@ -334,7 +341,7 @@ def get_homog_constraint(self, var_sizes=None): var_sizes = self.var_sizes Ah = PolyMatrix() Ah[self.h, self.h] = 1 - return (Ah.get_matrix(var_sizes), 1.0) + return (Ah.get_matrix_sparse(var_sizes), 1.0) def get_problem_matrices(self): """Get sparse, numerical form of objective and constraint matrices @@ -434,7 +441,7 @@ def decompose_matrix(self, pmat: PolyMatrix, method="split"): PolyMatrix that contains decomposed matrix on that clique. Args: - method (str): "split" means equal split between overlapping, "first" means first takes all, "greedy-cover" uses a smart algorithm to split. + method (str): "split" means equal split between overlapping, "first" means first takes all, "greedy-cover" uses a smart algorithm to split. """ assert isinstance(pmat, PolyMatrix), TypeError("Input should be a PolyMatrix") assert pmat.symmetric, ValueError("PolyMatrix input should be symmetric") diff --git a/cert_tools/test_tools.py b/cert_tools/test_tools.py index 3584855..b19d396 100644 --- a/cert_tools/test_tools.py +++ b/cert_tools/test_tools.py @@ -218,13 +218,6 @@ def get_row_col_constraints(index): constraints.append(A) return constraints - @staticmethod - def get_homog_constraint(): - """generate homogenizing constraint""" - A = PolyMatrix() - A["h", "h"] = 1 - return [(A, 1.0)] - def convert_sdp_to_rot(self, X, er_min=ER_MIN): """ Converts a solution matrix to a list of rotations. From 256b258de4c80b0cdaf385b8b6fe6212a34bd715 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 4 Dec 2024 19:31:49 +0100 Subject: [PATCH 10/15] Enable option to add inequality constraints --- cert_tools/sdp_solvers.py | 66 ++++++++++++++++++++++++++++++--------- 1 file changed, 52 insertions(+), 14 deletions(-) diff --git a/cert_tools/sdp_solvers.py b/cert_tools/sdp_solvers.py index bff13c2..33d4980 100644 --- a/cert_tools/sdp_solvers.py +++ b/cert_tools/sdp_solvers.py @@ -118,6 +118,7 @@ def get_subgradient(Q, A_list, a): def solve_low_rank_sdp( Q, Constraints, + IneqConstraints=[], rank=1, x_cand=None, adjust=(1, 0), @@ -127,6 +128,9 @@ def solve_low_rank_sdp( ): """Use the factorization proposed by Burer and Monteiro to solve a fixed rank SDP. + + IneqConstraints: (A, bl, br) s.t. bl <= Y'A'Y <= br + """ # Get problem dimensions n = Q.shape[0] @@ -134,26 +138,37 @@ def solve_low_rank_sdp( Y = cas.SX.sym("Y", n, rank) # Define cost f = cas.trace(Y.T @ Q @ Y) - # Define constraints - g_lhs = [] + + # Define equality constraints + g = [] g_rhs = [] - for A, b in Constraints: - g_lhs += [cas.trace(Y.T @ A @ Y)] + g_lhs = [] + for i, (A, b) in enumerate(Constraints): + # Limit the number of constraints used to the degrees of freedom + if limit_constraints and i > rank * n: + continue + g += [cas.trace(Y.T @ A @ Y)] + g_lhs += [b] g_rhs += [b] - # Limit the number of constraints used to the degrees of freedom - if limit_constraints and len(g_lhs) > rank * n: - g_lhs = g_lhs[: rank * n] - g_rhs = g_rhs[: rank * n] + + # Define inequality constraints + for i, (A, bl, br) in enumerate(IneqConstraints): + g += [cas.trace(Y.T @ A @ Y)] + g_lhs += [bl] + g_rhs += [br] + # Concatenate + g = cas.vertcat(*g) g_lhs = cas.vertcat(*g_lhs) g_rhs = cas.vertcat(*g_rhs) + # Define Low Rank NLP - nlp = {"x": Y.reshape((-1, 1)), "f": f, "g": g_lhs} + nlp = {"x": Y.reshape((-1, 1)), "f": f, "g": g} options["ipopt.print_level"] = int(verbose) options["print_time"] = int(verbose) S = cas.nlpsol("S", "ipopt", nlp, options) # Run Program - sol_input = dict(lbg=g_rhs, ubg=g_rhs) + sol_input = dict(lbg=g_lhs, ubg=g_rhs) if x_cand is not None: sol_input["x0"] = x_cand.reshape((-1, 1)) r = S(**sol_input) @@ -173,13 +188,14 @@ def solve_low_rank_sdp( H = H + A * mults[i, 0] # Return - info = {"X": X_opt, "H": H, "cost": cost} + info = {"X": X_opt, "H": H, "cost": cost, "Y": Y_opt} return Y_opt, info def solve_sdp_mosek( Q, Constraints, + IneqConstraints=[], adjust=ADJUST, primal=PRIMAL, tol=TOL, @@ -192,6 +208,8 @@ def solve_sdp_mosek( Q: Cost matrix Constraints: List of tuples representing constraints. Each tuple, (A,b) is such that tr(A @ X) == b + IneqConstraints: List of tuples representing inequality constraints. Each tuple, (A,bl, br) is such that + bl <= tr(A @ X) <= br adjust (bool, optional): Whether or not to rescale and shift Q for better conditioning. verbose (bool, optional): If true, prints output to screen. Defaults to True. @@ -231,12 +249,16 @@ def streamprinter(text): ) # problem params dim = Q_here.shape[0] - numcon = len(Constraints) + numcon = len(Constraints) + len(IneqConstraints) + # append vars,constr task.appendbarvars([dim]) task.appendcons(numcon) + # bound keys bkc = mosek.boundkey.fx + bkr = mosek.boundkey.ra + # Cost matrix Q_l = sp.tril(Q_here, format="csr") rows, cols = Q_l.nonzero() @@ -244,23 +266,39 @@ def streamprinter(text): assert not np.any(np.isinf(vals)), ValueError("Cost matrix has inf vals") symq = task.appendsparsesymmat(dim, rows.astype(int), cols.astype(int), vals) task.putbarcj(0, [symq], [1.0]) + # Input the objective sense (minimize/maximize) task.putobjsense(mosek.objsense.minimize) - # Add constraints + + # Add equality constraints cnt = 0 for A, b in Constraints: # Generate matrix A_l = sp.tril(A, format="csr") rows, cols = A_l.nonzero() - vals = A_l[rows, cols].tolist()[0] + vals = np.array(A_l[rows, cols]).flatten() syma = task.appendsparsesymmat(dim, rows, cols, vals) # Add constraint matrix task.putbaraij(cnt, 0, [syma], [1.0]) # Set bound (equality) task.putconbound(cnt, bkc, b, b) cnt += 1 + + # Add inequality constraints + for A, bl, br in IneqConstraints: + A_l = sp.tril(A, format="csr") + rows, cols = A_l.nonzero() + vals = np.array(A_l[rows, cols]).flatten() + syma = task.appendsparsesymmat(dim, rows, cols, vals) + # Add constraint matrix + task.putbaraij(cnt, 0, [syma], [1.0]) + # Set bound (equality) + task.putconbound(cnt, bkr, bl, br) + cnt += 1 + # Store problem task.writedata("solve_mosek.ptf") + # Solve the problem and print summary task.optimize() task.solutionsummary(mosek.streamtype.msg) From e96ec2d2ecf6a27e8a0f0164ebb4a632a7d0baac Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 9 Dec 2024 20:06:34 +0100 Subject: [PATCH 11/15] Remove first option for decompose_matrix, use correct variables in init_from_lifter --- cert_tools/hom_qcqp.py | 41 ++++++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index 2916105..8eec9ce 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -8,13 +8,15 @@ import plotly.graph_objects as go import plotly.io as pio import scipy.sparse as sp +from cert_tools.base_clique import BaseClique, get_chain_clique_data +from cert_tools.linalg_tools import find_dependent_columns, svec from cvxopt import amd, spmatrix from igraph import Graph -from poly_matrix import PolyMatrix from scipy.linalg import polar -from cert_tools.base_clique import BaseClique -from cert_tools.linalg_tools import find_dependent_columns, svec +from poly_matrix import PolyMatrix + +CONSTRAIN_ONLY_H_ROW = True class HomQCQP(object): @@ -45,22 +47,31 @@ def __init__(self, homog_var="h"): self.h = homog_var # Homogenizing variable name @staticmethod - def init_from_lifter(lifter): + def init_from_lifter(lifter, learned=False): + from lifters.state_lifter import StateLifter + + assert isinstance(lifter, StateLifter) problem = HomQCQP() problem.C = lifter.get_Q_from_y(lifter.y_, output_poly=True) problem.var_sizes = lifter.var_dict - A_sparse_list = lifter.get_A_learned_simple() - A_poly_list = [] - for A in A_sparse_list: - A_poly, __ = PolyMatrix.init_from_sparse(A, lifter.var_dict) - A_poly.symmetric = True - A_poly_list.append(A_poly) + if learned: + A_poly_list = lifter.get_A_learned_simple(output_poly=True) + else: + # does not output the homogeneous constraint. + A_poly_list = lifter.get_A_known(output_poly=True) + lifter.test_constraints( + [A.get_matrix_sparse(problem.var_sizes) for A in A_poly_list] + ) + problem.As = A_poly_list + problem.get_asg(lifter.var_dict) # to suppress warning in clique_decomposition # TODO(FD) not sure if we should do this here or wait. - # problem.get_asg(lifter.var_dict) # to suppress warning in clique_decomposition - # problem.clique_decomposition() + clique_data = get_chain_clique_data( + problem.var_sizes, variable=lifter.VARIABLES + ) + problem.clique_decomposition(clique_data=clique_data) return problem def define_objective(self, *args, **kwargs) -> PolyMatrix: @@ -441,7 +452,7 @@ def decompose_matrix(self, pmat: PolyMatrix, method="split"): PolyMatrix that contains decomposed matrix on that clique. Args: - method (str): "split" means equal split between overlapping, "first" means first takes all, "greedy-cover" uses a smart algorithm to split. + method (str): "split" means equal split between overlapping, "greedy-cover" uses a smart algorithm to split. """ assert isinstance(pmat, PolyMatrix), TypeError("Input should be a PolyMatrix") assert pmat.symmetric, ValueError("PolyMatrix input should be symmetric") @@ -477,9 +488,9 @@ def decompose_matrix(self, pmat: PolyMatrix, method="split"): for edge in edges: cliques = edge_clique_map[edge] & valid_cliques # Define weighting based on method - if method in ["split"]: + if method == "split": alpha = np.ones(len(cliques)) / len(cliques) - elif method in ["first", "greedy-cover"]: + elif method == "greedy-cover": alpha = np.zeros(len(cliques)) alpha[0] = 1.0 else: From 8e2df580aeb26162935b55bc92e67fcdda2ab427 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 9 Dec 2024 20:07:00 +0100 Subject: [PATCH 12/15] Add option to constrain only one row of overlap --- cert_tools/hom_qcqp.py | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index 8eec9ce..bc7cfb9 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -416,6 +416,45 @@ def get_consistency_constraints(self): indices_l = clique_l._get_indices(var_list=sepset) size_k = clique_k.size size_l = clique_l.size + + # Define constraint matrices only in one row + if CONSTRAIN_ONLY_H_ROW: + assert "h" in sepset + + hom_k = int(clique_k._get_indices(var_list="h")) + hom_l = int(clique_l._get_indices(var_list="h")) + vals = np.ones(2) + for i_k, i_l in zip(indices_k, indices_l): + if i_k == hom_k: + assert i_l == hom_l + continue + + rows_k = np.r_[hom_k, i_k] + cols_k = np.r_[i_k, hom_k] + A_k = sp.coo_matrix( + (vals, (rows_k, cols_k)), + (size_k, size_k), + ) + + rows_l = np.r_[hom_l, i_l] + cols_l = np.r_[i_l, hom_l] + A_l = sp.coo_matrix( + (-vals, (rows_l, cols_l)), + (size_l, size_l), + ) + eq_list.append((k, l, A_k, A_l)) + + A_k = sp.coo_matrix( + ([1.0], ([hom_k], [hom_k])), + (size_k, size_k), + ) + A_l = sp.coo_matrix( + ([-1.0], ([hom_l], [hom_l])), + (size_l, size_l), + ) + eq_list.append((k, l, A_k, A_l)) + continue + # Define sparse constraint matrices for each element in the seperator overlap for i in range(len(indices_k)): for j in range(i, len(indices_k)): From a8db5cf2b7f055fdd0ba48731045df7ca46c3930 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 9 Dec 2024 20:07:46 +0100 Subject: [PATCH 13/15] New function to assign constraint matrices to cliques --- cert_tools/hom_qcqp.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index bc7cfb9..3820943 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -484,6 +484,31 @@ def get_consistency_constraints(self): return eq_list + def assign_matrix(self, pmat: PolyMatrix): + """Assign a matrix to the clique that it corresponds to. + + Returns the index of the clique that this matrix applies to. + """ + assert isinstance(pmat, PolyMatrix), TypeError("Input should be a PolyMatrix") + assert pmat.symmetric, ValueError("PolyMatrix input should be symmetric") + + if not len(self.var_clique_map): + raise ValueError( + "var_clique_map is empty. Did you run clique_decomposition?" + ) + + involved_variables = set(pmat.get_variables()) + valid_cliques = [] + for clique in self.cliques: + if involved_variables.issubset(clique.var_list): + valid_cliques.append(clique.index) + + if not len(valid_cliques): + raise ValueError( + f"Error assigning constraint to a single clique: {len(valid_cliques)} matches" + ) + return valid_cliques + def decompose_matrix(self, pmat: PolyMatrix, method="split"): """Decompose a matrix according to clique decomposition. From 8048f56f3f9ea06836f037c6d2a76429fde5ea34 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 9 Dec 2024 20:21:20 +0100 Subject: [PATCH 14/15] Create consistency constraints member variable --- _test/test_admm.py | 6 +++--- _test/test_homqcqp.py | 15 ++++++++------- _test/test_sdp_solvers.py | 5 ++--- cert_tools/admm_clique.py | 17 +++++++---------- cert_tools/admm_solvers.py | 7 +++++-- cert_tools/hom_qcqp.py | 15 +++++++-------- cert_tools/sparse_solvers.py | 31 +++++++++++++++---------------- 7 files changed, 47 insertions(+), 49 deletions(-) diff --git a/_test/test_admm.py b/_test/test_admm.py index c91dd0e..0d5bc2a 100644 --- a/_test/test_admm.py +++ b/_test/test_admm.py @@ -167,11 +167,11 @@ def plot_admm(): A_dicts.append(A_dict) plt.close("all") - eq_list = problem.get_consistency_constraints() - assert len(eq_list) == 2 * 6 + problem.consistency_constraints() + assert len(problem.Es) == 2 * 6 counter = {(1, 0): 0, (2, 1): 0} plots = {(1, 0): plt.subplots(2, 6)[1], (2, 1): plt.subplots(2, 6)[1]} - for k, l, Ak, Al in eq_list: + for k, l, Ak, Al in problem.Es: # Ak.matshow(ax=plots[(k, l)][0, counter[(k, l)]]) # Al.matshow(ax=plots[(k, l)][1, counter[(k, l)]]) plots[(k, l)][0, counter[(k, l)]].matshow(Ak.toarray()) diff --git a/_test/test_homqcqp.py b/_test/test_homqcqp.py index 9dd04e7..8139fa4 100644 --- a/_test/test_homqcqp.py +++ b/_test/test_homqcqp.py @@ -2,8 +2,6 @@ import unittest import numpy as np -from poly_matrix import PolyMatrix - from cert_tools import HomQCQP from cert_tools.hom_qcqp import greedy_cover from cert_tools.linalg_tools import svec @@ -11,6 +9,8 @@ from cert_tools.sparse_solvers import solve_clarabel, solve_dsdp from cert_tools.test_tools import get_chain_rot_prob, get_loop_rot_prob +from poly_matrix import PolyMatrix + # import pytest @@ -151,12 +151,12 @@ def test_consistency_constraints(self): nvars = 5 problem = get_chain_rot_prob(N=nvars) problem.clique_decomposition() # Run clique decomposition - eq_list = problem.get_consistency_constraints() + problem.consistency_constraints(constrain_only_h_row=False) # check the number of constraints generated clq_dim = 10 # homogenizing var plus rotation n_cons_per_sep = round(clq_dim * (clq_dim + 1) / 2) - assert len(eq_list) == (nvars - 2) * n_cons_per_sep, ValueError( + assert len(problem.Es) == (nvars - 2) * n_cons_per_sep, ValueError( "Wrong number of equality constraints" ) @@ -215,7 +215,7 @@ def test_decompose_matrix(self): # problem.decompose_matrix(A) # functionality - for method in ["split", "first", "greedy-cover"]: + for method in ["split", "greedy-cover"]: C_d = problem.decompose_matrix(C, method=method) assert len(C_d.keys()) == nvars - 1, ValueError( f"{method} Method: Wrong number of cliques in decomposed matrix" @@ -250,6 +250,7 @@ def test_solve_primal_dsdp(self, rank1=False): problem = get_chain_rot_prob(N=nvars, locked_pose=locked_pose) problem.clique_decomposition() # get cliques # Solve decomposed problem (Interior Point Version) + problem.consistency_constraints(constrain_only_h_row=False) c_list, info = solve_dsdp(problem, verbose=True, tol=1e-8) # check solutions # Solve non-decomposed problem @@ -402,7 +403,7 @@ def test_solve_dual_dsdp(self, rank1=False): # test.test_consistency_constraints() # test.test_greedy_cover() # test.test_decompose_matrix() - # test.test_solve_primal_dsdp() + test.test_solve_primal_dsdp() # test.test_solve_dual_dsdp() - test.test_standard_form() + # test.test_standard_form() # test.test_clarabel() diff --git a/_test/test_sdp_solvers.py b/_test/test_sdp_solvers.py index acddb44..3dd7a75 100644 --- a/_test/test_sdp_solvers.py +++ b/_test/test_sdp_solvers.py @@ -4,12 +4,11 @@ import mosek import numpy as np - from cert_tools import ( - solve_sdp_mosek, solve_low_rank_sdp, - solve_sdp_fusion, solve_sdp_cvxpy, + solve_sdp_fusion, + solve_sdp_mosek, ) root_dir = os.path.abspath(os.path.dirname(__file__) + "/../") diff --git a/cert_tools/admm_clique.py b/cert_tools/admm_clique.py index cdd4656..267e188 100644 --- a/cert_tools/admm_clique.py +++ b/cert_tools/admm_clique.py @@ -1,7 +1,6 @@ import cvxpy as cp import numpy as np import scipy.sparse as sp - from cert_tools.base_clique import BaseClique from cert_tools.hom_qcqp import HomQCQP from cert_tools.sdp_solvers import adjust_Q @@ -81,19 +80,17 @@ def create_admm_cliques_from_problem(problem: HomQCQP, variable=["x_", "z_"]): problem.var_sizes, fixed=["h"], variable=variable ) problem.clique_decomposition(clique_data=clique_data) - - eq_list = problem.get_consistency_constraints() + problem.consistency_constraints() Q_dict = problem.decompose_matrix(problem.C, method="split") - A_dict_list = [problem.decompose_matrix(A, method="first") for A in problem.As] + A_dict_list = [(problem.assign_matrix(A), A) for A in problem.As] + # A_dict_list = [problem.decompose_matrix(A, method="first") for A in problem.As] admm_cliques = [] for clique in problem.cliques: Constraints = [problem.get_homog_constraint(clique.var_sizes)] - for A_dict in A_dict_list: - if clique.index in A_dict.keys(): - Constraints.append( - (A_dict[clique.index].get_matrix(clique.var_sizes), 0.0) - ) + for idx, A in A_dict_list: + if clique.index in idx: + Constraints.append((A.get_matrix(clique.var_sizes), 0.0)) admm_clique = ADMMClique( Q=Q_dict[clique.index].get_matrix(clique.var_sizes), Constraints=Constraints, @@ -107,7 +104,7 @@ def create_admm_cliques_from_problem(problem: HomQCQP, variable=["x_", "z_"]): # F @ vech(Xk) + G @ vech(Xl) = 0 F_dict = dict() G_dict = dict() - for k, l, Ak, Al in eq_list: + for k, l, Ak, Al in problem.Es: if k == clique.index: if l in F_dict: # TODO(FD) we currently need to use the full matrix and not just the upper half diff --git a/cert_tools/admm_solvers.py b/cert_tools/admm_solvers.py index 7800477..7942daa 100644 --- a/cert_tools/admm_solvers.py +++ b/cert_tools/admm_solvers.py @@ -4,9 +4,9 @@ from multiprocessing import Pipe, Process import cvxpy as cp +import matplotlib.pylab as plt # for debugging import mosek.fusion as fu import numpy as np - from cert_tools.admm_clique import ADMMClique, update_rho from cert_tools.fusion_tools import mat_fusion, read_costs_from_mosek from cert_tools.sdp_solvers import ( @@ -229,6 +229,9 @@ def solve_inner_sdp_fusion( "cost": cost, "msg": f"solved with status {M.getProblemStatus()}", } + else: + raise ValueError(f"Problem status is: {M.getProblemStatus()}") + return X, info @@ -251,7 +254,7 @@ def solve_inner_sdp( if use_fusion: # for debugging only - err = clique.F @ clique.X.flatten() + clique.g + # err = clique.F @ clique.X.flatten() + clique.g # print(f"current error: {err}") return solve_inner_sdp_fusion( diff --git a/cert_tools/hom_qcqp.py b/cert_tools/hom_qcqp.py index 3820943..7044f5e 100644 --- a/cert_tools/hom_qcqp.py +++ b/cert_tools/hom_qcqp.py @@ -40,6 +40,7 @@ def __init__(self, homog_var="h"): self.dim = 0 # total size of variables self.C = None # cost matrix self.As = None # list of constraints + self.Es = None # list of consistency constraints self.asg = Graph() # Aggregate sparsity graph self.cliques = [] # List of clique objects self.order = [] # Elimination ordering @@ -391,7 +392,7 @@ def get_standard_form(self, vec_order="C"): b = svec(C.toarray(), vec_order) return P, q, A, b - def get_consistency_constraints(self): + def consistency_constraints(self, constrain_only_h_row=CONSTRAIN_ONLY_H_ROW): """Return a list of constraints that enforce equalities between clique variables. List consist of 4-tuples: (k, l, A_k, A_l) where k and l are the indices of the cliques for which the equality is @@ -403,7 +404,7 @@ def get_consistency_constraints(self): PolyMatrix module. """ # Lopp through edges in the junction tree - eq_list = [] + self.Es = [] for l, clique_l in enumerate(self.cliques): # Get parent clique object and separator set k = clique_l.parent @@ -418,7 +419,7 @@ def get_consistency_constraints(self): size_l = clique_l.size # Define constraint matrices only in one row - if CONSTRAIN_ONLY_H_ROW: + if constrain_only_h_row: assert "h" in sepset hom_k = int(clique_k._get_indices(var_list="h")) @@ -442,7 +443,7 @@ def get_consistency_constraints(self): (-vals, (rows_l, cols_l)), (size_l, size_l), ) - eq_list.append((k, l, A_k, A_l)) + self.Es.append((k, l, A_k, A_l)) A_k = sp.coo_matrix( ([1.0], ([hom_k], [hom_k])), @@ -452,7 +453,7 @@ def get_consistency_constraints(self): ([-1.0], ([hom_l], [hom_l])), (size_l, size_l), ) - eq_list.append((k, l, A_k, A_l)) + self.Es.append((k, l, A_k, A_l)) continue # Define sparse constraint matrices for each element in the seperator overlap @@ -480,9 +481,7 @@ def get_consistency_constraints(self): (vals_l, (rows_l, cols_l)), (size_l, size_l), ) - eq_list.append((k, l, A_k, A_l)) - - return eq_list + self.Es.append((k, l, A_k, A_l)) def assign_matrix(self, pmat: PolyMatrix): """Assign a matrix to the clique that it corresponds to. diff --git a/cert_tools/sparse_solvers.py b/cert_tools/sparse_solvers.py index 99bc5d7..d40a4d8 100644 --- a/cert_tools/sparse_solvers.py +++ b/cert_tools/sparse_solvers.py @@ -23,8 +23,6 @@ from poly_matrix import PolyMatrix -CONSTRAIN_ALL_OVERLAP = False - TOL = 1e-5 @@ -428,15 +426,17 @@ def get_decomp_fusion_expr(pmat_in, decomp_method="split"): # CLIQUE CONSISTENCY EQUALITIES if verbose: print("Generating overlap consistency constraints") - clq_constrs = problem.get_consistency_constraints() + + if problem.Es is None: + problem.consistency_constraints() # TEST reduce number of clique if reduce_constrs is not None: - n_constrs = int(reduce_constrs * len(clq_constrs)) - clq_constrs = random.sample(clq_constrs, n_constrs) + n_constrs = int(reduce_constrs * len(problem.Es)) + clq_constrs = random.sample(problem.Es, n_constrs) if verbose: print("Adding overlap consistency constraints to problem") cnt = 0 - for k, l, A_k, A_l in clq_constrs: + for k, l, A_k, A_l in problem.Es: # Convert sparse array to fusion sparse matrix A_k_fusion = sparse_to_fusion(A_k) A_l_fusion = sparse_to_fusion(A_l) @@ -484,16 +484,15 @@ def get_decomp_fusion_expr(pmat_in, decomp_method="split"): # EXTRACT SOLN status = M.getProblemStatus() - if status == fu.ProblemStatus.PrimalAndDualFeasible: - # Get MOSEK cost - cost = M.primalObjValue() - clq_list = [cvar.level().reshape(cvar.shape) for cvar in cvars] - dual = [cvar.dual().reshape(cvar.shape) for cvar in cvars] - info["success"] = True - info["dual"] = dual - info["cost"] = cost - else: - print("Solve Failed - Mosek Status: " + str(status)) + if status != fu.ProblemStatus.PrimalAndDualFeasible: + print("Warning: solve failed -- mosek status: " + str(status)) + + cost = M.primalObjValue() + clq_list = [cvar.level().reshape(cvar.shape) for cvar in cvars] + dual = [cvar.dual().reshape(cvar.shape) for cvar in cvars] + info["success"] = True + info["dual"] = dual + info["cost"] = cost return clq_list, info From ff589b1c3ee5ce016941b8cfe164d204c2265b20 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 21 Jan 2025 21:24:11 +0100 Subject: [PATCH 15/15] Add argument --- cert_tools/admm_clique.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cert_tools/admm_clique.py b/cert_tools/admm_clique.py index 267e188..6113291 100644 --- a/cert_tools/admm_clique.py +++ b/cert_tools/admm_clique.py @@ -80,7 +80,7 @@ def create_admm_cliques_from_problem(problem: HomQCQP, variable=["x_", "z_"]): problem.var_sizes, fixed=["h"], variable=variable ) problem.clique_decomposition(clique_data=clique_data) - problem.consistency_constraints() + problem.consistency_constraints(constrain_only_h_row=False) Q_dict = problem.decompose_matrix(problem.C, method="split") A_dict_list = [(problem.assign_matrix(A), A) for A in problem.As]