From e81911b6a1dcc4a11a7c60699b0ef375cb6c07e6 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 14 Sep 2023 16:17:59 -0400 Subject: [PATCH 01/13] Start moving code to cert_tools --- .gitmodules | 3 + _scripts/generate_all_results.py | 13 +- _scripts/run_all_study.py | 2 +- _scripts/run_datasets_ro.py | 5 +- _scripts/run_datasets_stereo.py | 11 +- _scripts/run_other_study.py | 12 +- _scripts/run_range_only_study.py | 12 +- _scripts/run_stereo_study.py | 13 +- auto_template/__init__.py | 0 {lifters => auto_template}/learner.py | 50 +--- {utils => auto_template}/real_experiments.py | 2 +- {utils => auto_template}/sim_experiments.py | 4 +- centering/certificate.py | 54 ++++ certifiable-tools | 1 + environment.yml | 1 + lifters/base_class.py | 15 +- lifters/plotting_tools.py | 255 ---------------- lifters/poly_lifters.py | 40 ++- lifters/state_lifter.py | 173 +---------- solve_mosek.ptf | 161 +++++++++++ solvers/common.py | 288 ++----------------- solvers/sparse.py | 20 +- utils/common.py | 20 +- utils/plotting_tools.py | 161 +++++++++-- 24 files changed, 483 insertions(+), 833 deletions(-) create mode 100644 auto_template/__init__.py rename {lifters => auto_template}/learner.py (95%) rename {utils => auto_template}/real_experiments.py (99%) rename {utils => auto_template}/sim_experiments.py (99%) create mode 100644 centering/certificate.py create mode 160000 certifiable-tools delete mode 100644 lifters/plotting_tools.py create mode 100644 solve_mosek.ptf diff --git a/.gitmodules b/.gitmodules index fd610ca..37a7936 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "starloc"] path = starloc url = git@github.com:duembgen/starloc +[submodule "certifiable-tools"] + path = certifiable-tools + url = git@github.com:utiasASRL/certifiable-tools diff --git a/_scripts/generate_all_results.py b/_scripts/generate_all_results.py index ea4f4bd..72be0b6 100755 --- a/_scripts/generate_all_results.py +++ b/_scripts/generate_all_results.py @@ -8,12 +8,13 @@ import matplotlib try: - matplotlib.use("TkAgg") + # matplotlib.use("TkAgg") + matplotlib.use("Agg") except: pass if __name__ == "__main__": - n_seeds = 1 # was 10 + n_seeds = 1 # was 10 recompute = False tightness = True scalability = True @@ -38,12 +39,10 @@ scalability=scalability, ) - - - n_successful = 3 # was 100 - run_datasets_stereo(recompute=False, n_successful=n_successful) + n_successful = 3 # was 100 run_datasets_ro(recompute=False, n_successful=n_successful) + run_datasets_stereo(recompute=False, n_successful=n_successful) - n_successful = 3 # was 10 + n_successful = 3 # was 10 run_datasets_ro(recompute=True, n_successful=n_successful) run_datasets_stereo(recompute=True, n_successful=n_successful) diff --git a/_scripts/run_all_study.py b/_scripts/run_all_study.py index 8f2bc8f..3aa0ab7 100644 --- a/_scripts/run_all_study.py +++ b/_scripts/run_all_study.py @@ -3,7 +3,7 @@ import pandas as pd import numpy as np -from lifters.learner import Learner +from auto_template.learner import Learner from lifters.mono_lifter import MonoLifter from lifters.wahba_lifter import WahbaLifter from lifters.stereo2d_lifter import Stereo2DLifter diff --git a/_scripts/run_datasets_ro.py b/_scripts/run_datasets_ro.py index a0e5aff..c724f58 100644 --- a/_scripts/run_datasets_ro.py +++ b/_scripts/run_datasets_ro.py @@ -10,12 +10,13 @@ import pandas as pd -from utils.real_experiments import ( +from auto_template.real_experiments import ( Experiment, run_experiments, plot_results, plot_local_vs_global, ) +from auto_template.learner import TOL_RANK_ONE, TOL_REL_GAP DATASET_ROOT = str(Path(__file__).parent.parent) @@ -74,8 +75,6 @@ def run_all(recompute=RECOMPUTE, n_successful=100, plot_poses=False): plot_local_vs_global(df, fname_root=fname_root) - from lifters.learner import TOL_RANK_ONE, TOL_REL_GAP - plot_results( df, ylabel="SVR", diff --git a/_scripts/run_datasets_stereo.py b/_scripts/run_datasets_stereo.py index bfd1e4e..70c643e 100644 --- a/_scripts/run_datasets_stereo.py +++ b/_scripts/run_datasets_stereo.py @@ -10,8 +10,13 @@ except: pass -from utils.real_experiments import Experiment -from utils.real_experiments import run_experiments, plot_results, plot_local_vs_global +from auto_template.learner import TOL_RANK_ONE, TOL_REL_GAP +from auto_template.real_experiments import Experiment +from auto_template.real_experiments import ( + run_experiments, + plot_results, + plot_local_vs_global, +) DATASET_ROOT = str(Path(__file__).parent.parent) MAX_N_LANDMARKS = 8 # 10 @@ -80,8 +85,6 @@ def run_all(recompute=RECOMPUTE, n_successful=10): # cost below is found empirically plot_local_vs_global(df, fname_root=fname_root, cost_thresh=1e3) - from lifters.learner import TOL_RANK_ONE, TOL_REL_GAP - plot_results( df, ylabel="RDG", diff --git a/_scripts/run_other_study.py b/_scripts/run_other_study.py index 3172e6b..7d6d5d7 100644 --- a/_scripts/run_other_study.py +++ b/_scripts/run_other_study.py @@ -1,14 +1,14 @@ import numpy as np -from utils.sim_experiments import plot_scalability, save_table -from lifters.learner import Learner -from lifters.mono_lifter import MonoLifter -from utils.plotting_tools import savefig - -from utils.sim_experiments import ( +from auto_template.learner import Learner +from auto_template.sim_experiments import ( + plot_scalability, run_oneshot_experiment, run_scalability_new, ) +from lifters.mono_lifter import MonoLifter +from utils.plotting_tools import savefig + DEBUG = False diff --git a/_scripts/run_range_only_study.py b/_scripts/run_range_only_study.py index 67d1d6b..fd2fc8c 100644 --- a/_scripts/run_range_only_study.py +++ b/_scripts/run_range_only_study.py @@ -1,11 +1,11 @@ import numpy as np -from lifters.learner import Learner +from auto_template.learner import Learner from lifters.range_only_lifters import RangeOnlyLocLifter from utils.plotting_tools import savefig -from utils.sim_experiments import plot_scalability, save_table -from utils.sim_experiments import run_oneshot_experiment, run_scalability_new +from auto_template.sim_experiments import plot_scalability +from auto_template.sim_experiments import run_oneshot_experiment, run_scalability_new n_positions = 3 @@ -15,7 +15,7 @@ DEBUG = False RESULTS_DIR = "_results" -#RESULTS_DIR = "_results_server" +# RESULTS_DIR = "_results_server" def range_only_tightness(): @@ -86,9 +86,7 @@ def range_only_scalability_new(n_seeds, recompute): fname_root = f"{RESULTS_DIR}/scalability_{learner.lifter}" df_sub = df[df.type != "from scratch"]["t solve SDP"] - fig, axs = plot_scalability( - df, log=True, start="t ", legend_idx=1 - ) + fig, axs = plot_scalability(df, log=True, start="t ", legend_idx=1) # [ax.set_ylim(10, 1000) for ax in axs.values()] diff --git a/_scripts/run_stereo_study.py b/_scripts/run_stereo_study.py index b8aa522..d3eb5d5 100644 --- a/_scripts/run_stereo_study.py +++ b/_scripts/run_stereo_study.py @@ -6,15 +6,13 @@ # matplotlib.use('Agg') # non-interactive # plt.ioff() -from utils.sim_experiments import ( +from auto_template.learner import Learner +from auto_template.sim_experiments import ( + plot_scalability, run_scalability_new, run_oneshot_experiment, run_scalability_plot, ) -from utils.sim_experiments import plot_scalability, save_table -from utils.plotting_tools import savefig - -from lifters.learner import Learner from lifters.stereo2d_lifter import Stereo2DLifter from lifters.stereo3d_lifter import Stereo3DLifter from utils.plotting_tools import savefig @@ -22,7 +20,8 @@ DEBUG = False RESULTS_DIR = "_results" -#RESULTS_DIR = "_results_server" +# RESULTS_DIR = "_results_server" + def stereo_tightness(d=2, n_landmarks=None): """ @@ -106,7 +105,7 @@ def stereo_scalability_new(n_seeds, recompute, d=2): param_list=n_landmarks_list, n_seeds=n_seeds, recompute=recompute, - results_folder=RESULTS_DIR + results_folder=RESULTS_DIR, ) if df is None: return diff --git a/auto_template/__init__.py b/auto_template/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lifters/learner.py b/auto_template/learner.py similarity index 95% rename from lifters/learner.py rename to auto_template/learner.py index e54c2fa..e52726d 100644 --- a/lifters/learner.py +++ b/auto_template/learner.py @@ -4,23 +4,21 @@ import numpy as np import pandas as pd import scipy.sparse as sp -import sparseqr as sqr -from utils.plotting_tools import import_plt, add_rectangles, add_colorbar -from utils.common import rank_project - -plt = import_plt() +from cert_tools.linalg_tools import find_dependent_columns, rank_project +from poly_matrix.poly_matrix import PolyMatrix from lifters.state_lifter import StateLifter -from poly_matrix.poly_matrix import PolyMatrix +from utils.constraint import Constraint +from utils.plotting_tools import import_plt, add_rectangles, add_colorbar +from utils.plotting_tools import savefig, plot_singular_values from solvers.common import find_local_minimum from solvers.common import solve_certificate from solvers.common import solve_sdp_cvxpy from solvers.sparse import solve_lambda from solvers.sparse import bisection, brute_force -from utils.plotting_tools import savefig -from utils.constraint import Constraint +plt = import_plt() # parameter of SDP solver TOL = 1e-10 @@ -501,8 +499,6 @@ def learn_templates(self, plot=False, data_dict=None): bad_idx = self.lifter.clean_Y(basis_new, Y, S, plot=False) if plot: - from lifters.plotting_tools import plot_singular_values - if len(bad_idx): plot_singular_values( S, eps=self.lifter.EPS_SVD, label=f"run {i}", ax=ax @@ -614,42 +610,10 @@ def clean_constraints( if A_vec.shape[0] < A_vec.shape[1]: print("Warning: fat matrix.") - # Use sparse rank revealing QR - # We "solve" a least squares problem to get the rank and permutations - # This is the cheapest way to use sparse QR, since it does not require - # explicit construction of the Q matrix. We can't do this with qr function - # because the "just return R" option is not exposed. - Z, R, E, rank = sqr.rz( - A_vec, np.zeros((A_vec.shape[0], 1)), tolerance=1e-10 - ) - # Sort the diagonal values. Note that SuiteSparse uses AMD/METIS ordering - # to acheive sparsity. - r_vals = np.abs(R.diagonal()) - sort_inds = np.argsort(r_vals)[::-1] - if rank < A_vec.shape[1]: - print(f"clean_constraints: keeping {rank}/{A_vec.shape[1]} independent") - - bad_idx = list(range(A_vec.shape[1])) - keep_idx = sorted(E[sort_inds[:rank]])[::-1] - for good_idx in keep_idx: - del bad_idx[good_idx] - # bad_idx = list(E[sort_inds[rank:]]) - - # Sanity check, removed because too expensive. It almost always passed anyways. - Z, R, E, rank_full = sqr.rz( - A_vec.tocsc()[:, keep_idx], - np.zeros((A_vec.shape[0], 1)), - tolerance=1e-10, - ) - if rank_full != rank: - print( - f"Warning: selected constraints did not pass lin. independence check. Rank is {rank_full}, should be {rank}." - ) - + bad_idx = find_dependent_columns(A_vec) if len(bad_idx): for idx in sorted(bad_idx)[::-1]: del constraints[idx] - assert len(constraints) == rank if remove_imprecise: error, bad_idx = self.lifter.test_constraints( diff --git a/utils/real_experiments.py b/auto_template/real_experiments.py similarity index 99% rename from utils/real_experiments.py rename to auto_template/real_experiments.py index 88490b2..3a8dd0a 100644 --- a/utils/real_experiments.py +++ b/auto_template/real_experiments.py @@ -12,7 +12,7 @@ from starloc.reader import read_landmarks, read_data, read_calib -from lifters.learner import Learner +from auto_template.learner import Learner from lifters.range_only_lifters import RangeOnlyLocLifter from lifters.stereo3d_lifter import Stereo3DLifter from utils.plotting_tools import savefig diff --git a/utils/sim_experiments.py b/auto_template/sim_experiments.py similarity index 99% rename from utils/sim_experiments.py rename to auto_template/sim_experiments.py index 8c758fd..72147da 100644 --- a/utils/sim_experiments.py +++ b/auto_template/sim_experiments.py @@ -6,7 +6,7 @@ import numpy as np import matplotlib.pylab as plt -from lifters.learner import Learner +from auto_template.learner import Learner from lifters.mono_lifter import MonoLifter from lifters.wahba_lifter import WahbaLifter from lifters.stereo2d_lifter import Stereo2DLifter @@ -348,7 +348,7 @@ def run_scalability_new( param_list: list, results_folder: str, n_seeds: int = 1, - recompute: bool =False, + recompute: bool = False, ): fname_root = f"{results_folder}/scalability_{learner.lifter}" fname_all = fname_root + "_complete.pkl" diff --git a/centering/certificate.py b/centering/certificate.py new file mode 100644 index 0000000..8b911d5 --- /dev/null +++ b/centering/certificate.py @@ -0,0 +1,54 @@ +import numpy as np +import scipy.linalg as la +import matplotlib + +matplotlib.use("TkAgg") + +from cert_tools.linalg_tools import get_nullspace +from cert_tools.eig_tools import solve_Eopt + + +def get_certificate(Q, A_list, x_hat): + """ + Compute non-centered certificate. + """ + L = np.concatenate([(A @ x_hat)[:, None] for A in A_list], axis=1) + b = -Q @ x_hat + + null_basis, info = get_nullspace(L, method="qr") + y, *_ = la.lstsq(L, b) + + Q_bar = Q + np.sum([Ai * yi for Ai, yi in zip(A_list, y)]) + A_list_bar = [] + for j in range(null_basis.shape[1]): + A_bar = np.sum([Ai * ni for Ai, ni in zip(A_list, null_basis[:, j])]) + A_list_bar.append(A_bar) + + alphas, info = solve_Eopt(Q_bar, A_list_bar) + return info["success"] + + +def get_centered_certificate(Q_bar, A_list): + x_hat = np.zeros(Q_bar.shape[0]) + x_hat[0] = 1.0 + return get_certificate(Q_bar, A_list, x_hat) + + +if __name__ == "__main__": + from lifters.poly_lifters import Poly6Lifter + + lifter = Poly6Lifter() + Q, __ = lifter.get_Q() + A_list = [lifter.get_A0()] + lifter.get_A_known() + # A_b_list = lifter.get_A_b_list(A_list) + + t_init = -1 + t_hat, info, cost = lifter.local_solver(t_init) + x_hat = lifter.get_x(t_hat) + + for A_i in A_list[1:]: + assert abs(x_hat @ A_i @ x_hat) <= 1e-10 + + success = get_certificate(Q, A_list, x_hat) + + success = get_centered_certificate(Q, A_list, x_hat) diff --git a/certifiable-tools b/certifiable-tools new file mode 160000 index 0000000..6a02f66 --- /dev/null +++ b/certifiable-tools @@ -0,0 +1 @@ +Subproject commit 6a02f66cffa766450c51adb847dad6e2db60e64d diff --git a/environment.yml b/environment.yml index fca2380..5eb1318 100644 --- a/environment.yml +++ b/environment.yml @@ -27,4 +27,5 @@ dependencies: - sparseqr - asrl-pylgmath>=1.0.3 - -e poly_matrix # build local poly_matrix submodule + - -e certifiable-tools - -e . # build local lifting package diff --git a/lifters/base_class.py b/lifters/base_class.py index ef4b0a3..e425064 100644 --- a/lifters/base_class.py +++ b/lifters/base_class.py @@ -65,19 +65,15 @@ def get_level_dims(self, n=1): def generate_random_setup(self): return - @abstractmethod + # @abstractmethod def generate_random_theta(self): pass - @abstractmethod + # @abstractmethod def sample_theta(self): return - @abstractmethod - def sample_theta(self): - return - - @abstractmethod + # @abstractmethod def sample_parameters(self, x=None): return @@ -89,7 +85,7 @@ def get_x(self, theta, var_subset=None) -> np.ndarray: def get_p(self, parameters=None, var_subset=None) -> np.ndarray: return - @abstractmethod + # @abstractmethod def get_parameters(self, var_subset=None) -> list: return @@ -100,3 +96,6 @@ def get_grad(self, t, y): def get_Q(self, noise=1e-3): Warning("get_Q not implemented yet") return None, None + + def get_A_known(self): + return [] diff --git a/lifters/plotting_tools.py b/lifters/plotting_tools.py deleted file mode 100644 index 6333892..0000000 --- a/lifters/plotting_tools.py +++ /dev/null @@ -1,255 +0,0 @@ -def import_plt(): - import matplotlib.pylab as plt - import shutil - - usetex = True if shutil.which("latex") else False - plt.rcParams.update( - { - "text.usetex": usetex, - "font.family": "DejaVu Sans", - "font.size": 12, - } - ) - plt.rc("text.latex", preamble=r"\usepackage{bm}") - return plt - - -import os - -import numpy as np - -plt = import_plt() - - -def partial_plot_and_save(lifter, Q, A_list, fname_root="", appendix="", title="A"): - # plot resulting matrices - from math import ceil - - from lifters.plotting_tools import plot_matrices - - n = 10 # how many plots per figure - chunks = min(ceil(len(A_list) / n), 20) # how many figures to create - # make sure it's symmetric - vmin = np.min([np.min(A) for A in A_list]) - vmax = np.max([np.max(A) for A in A_list]) - vmin = min(vmin, -vmax) - vmax = max(vmax, -vmin) - plot_count = 0 - for k in np.arange(chunks): - fig, axs = plot_matrices( - A_list, - n_matrices=n, - start_idx=k * n, - colorbar=False, - vmin=vmin, - vmax=vmax, - nticks=3, - lines=[1 + lifter.N, 1 + lifter.N + lifter.M], - title=title, - ) - # if k in [0, 1, chunks - 2, chunks - 1]: - if fname_root != "": - savefig(fig, f"{fname_root}/{title}{plot_count}_{lifter}{appendix}.png") - plot_count += 1 - - if Q is not None: - fig, ax = plt.subplots() - fig.set_size_inches(3, 3) - try: - ax.matshow(np.abs(Q) > 1e-10) - except: - ax.matshow(np.abs(Q.toarray() > 1e-10)) - ax.set_title("Q mask") - if fname_root != "": - savefig(fig, f"{fname_root}/Q_{lifter}{appendix}.png") - - -def add_colorbar(fig, ax, im, title=None, nticks=None, visible=True, size=None): - from mpl_toolkits.axes_grid1 import make_axes_locatable - - divider = make_axes_locatable(ax) - if size is None: - w, h = fig.get_size_inches() - size = f"{5*h/w}%" - cax = divider.append_axes("right", size=size, pad=0.05) - if title is not None: - cax.set_ylabel(title) - - if not visible: - cax.axis("off") - return - - fig.colorbar(im, cax=cax, orientation="vertical") - - # add symmetric nticks ticks: min and max, and equally spaced in between - if nticks is not None: - from math import floor - - ticks = cax.get_yticks() - new_ticks = [ticks[0]] - step = floor(len(ticks) / (nticks - 1)) - new_ticks += list(ticks[step + 1 :: step]) - new_ticks += [ticks[-1]] - # print(f"reduce {ticks} to {new_ticks}") - assert len(new_ticks) == nticks - cax.set_yticks(ticks[::step]) - return cax - - -def make_dirs_safe(path): - """Make directory of input path, if it does not exist yet.""" - dirname = os.path.dirname(path) - if not os.path.exists(dirname): - os.makedirs(dirname) - - -def savefig(fig, name, verbose=True): - make_dirs_safe(name) - fig.savefig(name, bbox_inches="tight", pad_inches=0, dpi=200) - if verbose: - print(f"saved plot as {name}") - - -def plot_matrix( - Ai, vmin=None, vmax=None, nticks=None, title="", colorbar=True, fig=None, ax=None -): - import matplotlib - - if ax is None: - fig, ax = plt.subplots() - if fig is None: - fig = plt.gcf() - - norm = matplotlib.colors.SymLogNorm(10**-5, vmin=vmin, vmax=vmax) - if type(Ai) == np.ndarray: - im = ax.matshow(Ai, norm=norm) - else: - im = ax.matshow(Ai.toarray(), norm=norm) - ax.axis("off") - ax.set_title(title, y=1.0) - if colorbar: - add_colorbar(fig, ax, im, nticks=nticks) - else: - add_colorbar(fig, ax, im, nticks=nticks, visible=False) - return fig, ax, im - - -def plot_matrices( - A_list, - n_matrices=15, - start_idx=0, - colorbar=True, - vmin=None, - vmax=None, - nticks=None, - lines=[], - names=None, - title="", -): - num_plots = min(len(A_list), n_matrices) - - fig, axs = plt.subplots(1, num_plots, squeeze=False) - axs = axs.flatten() - fig.set_size_inches(num_plots, 2) - for i, (ax, Ai) in enumerate(zip(axs, A_list[start_idx:])): - title = names[i] if names else f"${title}_{{{start_idx+i+1}}}$" - fig, ax, im = plot_matrix( - Ai, - vmin, - vmax, - title=title, - colorbar=colorbar, - nticks=nticks, - fig=fig, - ax=axs[i], - ) - - try: - for n in lines: - for ax in axs[: i + 1]: - ax.plot([-0.5, n - 0.5], [n - 0.5, n - 0.5], color="red") - ax.plot([n - 0.5, n - 0.5], [-0.5, n - 0.5], color="red") - except: - pass - - if not colorbar: - add_colorbar(fig, ax, im, nticks=nticks, visible=True) - # add_colorbar(fig, ax, im, nticks=nticks) - [ax.axis("off") for ax in axs[i:]] - # fig.suptitle( - # f"{title} {start_idx+1}:{start_idx+i+1} of {len(A_list)} constraints", y=0.9 - # ) - return fig, axs - - -def plot_singular_values(S, eps=None, label="singular values", ax=None): - if ax is None: - fig, ax = plt.subplots() - else: - fig = plt.gcf() - fig.set_size_inches(4, 2) - ax.semilogy(S, marker="o", label=label) - if eps is not None: - ax.axhline(eps, color="C1") - ax.grid() - ax.set_xlabel("index") - ax.set_ylabel("magnitude of singular values") - ax.legend(loc="upper right") - return fig, ax - - -def plot_tightness(df, ax=None): - import seaborn as sns - - palette = "viridis" - if ax is None: - fig, ax = plt.subplots() - else: - fig = plt.gcf() - - try: - sns.lineplot( - data=df[df.shuffle == 0], - x="n", - y="local cost", - style="seed", - hue="shuffle", - ax=ax, - palette=palette, - legend=False, - ) - sns.lineplot( - data=df, - x="n", - y="dual cost", - style="seed", - hue="shuffle", - ax=ax, - palette=palette, - markers="o", - markeredgecolor=None, - ) - except KeyError as e: - print("Error, no valid data for dual cost?") - except AttributeError as e: - print("Error, empty dataframe?") - ax.set_ylabel("cost") - ax.set_yscale("log") - return fig, ax - - -if __name__ == "__main__": - import numpy as np - - vmin = -1 - vmax = 1 - A_list = np.random.rand(4, 4, 3) - fig, axs = plot_matrices(A_list, colorbar=False, vmin=vmin, vmax=vmax, nticks=4) - - dim_x = 2 - for ax in axs: - ax.plot([-0.5, dim_x + 0.5], [dim_x + 0.5, dim_x + 0.5], color="red") - ax.plot([dim_x + 0.5, dim_x + 0.5], [-0.5, dim_x + 0.5], color="red") - plt.show() - - print("done") diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index f817380..2d640d4 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -8,7 +8,14 @@ class PolyLifter(StateLifter): def __init__(self, degree): self.degree = degree - super().__init__(theta_shape=(1,), M=self.M) + super().__init__(d=1) + + @property + def var_dict(self): + if self.var_dict_ is None: + self.var_dict_ = {"h": 1, "t": 1} + self.var_dict_.update({f"z_{i}": 1 for i in range(self.M)}) + return self.var_dict_ @property def M(self): @@ -43,7 +50,8 @@ def local_solver(self, t0, *args, **kwargs): from scipy.optimize import minimize sol = minimize(self.get_cost, t0) - return sol.x[0], sol.success, sol.fun + info = {"success": sol.success} + return sol.x[0], info, sol.fun def __repr__(self): return f"poly{self.degree}" @@ -58,6 +66,15 @@ def get_Q_mat(self): Q = np.r_[np.c_[2, 1, 0], np.c_[1, -1 / 2, -1 / 3], np.c_[0, -1 / 3, 1 / 4]] return Q + def get_A_known(self): + from poly_matrix import PolyMatrix + + # z_0 = t^2 + A_1 = PolyMatrix(symmetric=True) + A_1["h", "z_0"] = -1 + A_1["t", "t"] = 2 + return A_1.get_matrix(self.var_dict) + def generate_random_setup(self): self.theta_ = np.array([-1]) @@ -77,5 +94,24 @@ def get_Q_mat(self): / 10 ) + def get_A_known(self): + from poly_matrix import PolyMatrix + + # z_0 = t^2 + A_1 = PolyMatrix(symmetric=True) + A_1["h", "z_0"] = -1 + A_1["t", "t"] = 2 + + # z_1 = t^3 = t z_0 + A_2 = PolyMatrix(symmetric=True) + A_2["h", "z_1"] = -1 + A_2["t", "z_0"] = 1 + + # t^4 = z_1 t = z_0 z_0 + B_0 = PolyMatrix(symmetric=True) + B_0["z_0", "z_0"] = 2 + B_0["t", "z_1"] = -1 + return [A_i.get_matrix(self.var_dict) for A_i in [A_1, A_2, B_0]] + def generate_random_setup(self): self.theta_ = np.array([-1]) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 2eb175a..0162e71 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -2,12 +2,13 @@ import numpy as np import matplotlib.pylab as plt -import scipy.linalg as la import scipy.sparse as sp + from utils.common import upper_triangular from lifters.base_class import BaseClass +from cert_tools.linalg_tools import get_nullspace from poly_matrix import PolyMatrix, unroll @@ -155,7 +156,7 @@ def get_level_dims(self, n=1): assert ( self.level == "no" ), "Need to overwrite get_level_dims to use level different than 'no'" - return 0 + return {"no": 0} @property def theta(self): @@ -449,37 +450,6 @@ def get_vector_dense(self, poly_row_sub): vector = np.r_[vector, mat[np.triu_indices(mat.shape[0])]] return np.array(vector) - def get_A_learned( - self, - A_known: list = [], - plot: bool = False, - incremental: bool = False, - normalize: bool = NORMALIZE, - method: str = METHOD, - ): - if incremental: - assert len(A_known) == 0 - var_subset = ["h", "x", "z_0"] - basis_list = self.get_basis_list( - var_subset=var_subset, - A_known=A_known, - eps_svd=self.EPS_SVD, - plot=plot, - method=method, - ) - basis_list_all = self.apply_templates(basis_list, normalize=normalize) - basis_learned = self.get_basis_from_poly_rows(basis_list_all) - A_learned = self.generate_matrices(basis_learned, normalize=normalize) - else: - basis_list = self.get_basis_list( - var_subset=self.var_dict, - A_known=A_known, - plot=plot, - method=method, - ) - A_learned = self.generate_matrices(basis_list, normalize=normalize) - return A_learned - def convert_polyrow_to_a(self, poly_row, var_subset, correct=True): """Convert poly-row to reduced a. @@ -629,95 +599,6 @@ def zero_pad_subvector(self, b, var_subset, target_subset=None): else: return None - def zero_pad_subvector_old(self, b, var_subset, target_subset=None): - """Add zero padding to b vector learned for a subset of variables(e.g. as obtained from learning method)""" - var_dict = self.get_var_dict(var_subset) - if target_subset is None: - target_subset = self.var_dict - dim_X = self.get_dim_X(var_subset) - param_dict = self.get_param_idx_dict(var_subset) - - # default is that b is an augmented vector (with parameters) - if len(b) != dim_X * len(param_dict): - # can call this function with "a" vector (equivalent to b without parameters) - b = self.augment_using_zero_padding(b) - - param_dict_target = self.get_param_idx_dict(target_subset) - dim_X_target = self.get_dim_X(target_subset) - bi_all = np.zeros(self.get_dim_Y(target_subset)) - for p, key in enumerate(param_dict.keys()): - block = b[p * dim_X : (p + 1) * (dim_X)] - mat_small = self.create_symmetric(block, eps_sparse=self.EPS_SPARSE) - poly_mat, __ = PolyMatrix.init_from_sparse(mat_small, var_dict) - mat_target = poly_mat.get_matrix(target_subset).toarray() - - pt = param_dict_target[key] - bi_all[pt * dim_X_target : (pt + 1) * dim_X_target] = mat_target[ - np.triu_indices(mat_target.shape[0]) - ] - - # below doesn't pass. this would make for a cleaner implementation, consider fixing. - # poly_row = self.convert_b_to_polyrow(b, var_subset) - # row_target_dict = self.var_dict_all(target_subset) - # bi_all_test = poly_row.get_matrix( - # (["h"], row_target_dict), output_type="dense" - # ).flatten() - # np.testing.assert_allclose(bi_all, bi_all_test) - return bi_all - - def get_basis_list( - self, - var_subset, - A_known: list = [], - plot: bool = False, - method: str = METHOD, - ): - print("Warning: do not use get_basis_list anymore, it is not efficient.") - """Generate list of PolyRow basis vectors""" - basis_list = [] - - Y = self.generate_Y(var_subset=var_subset) - if len(A_known): - A_known = self.extract_A_known(A_known, var_subset, output_type="dense") - Y = np.r_[Y, A_known] - for i in range(A_known.shape[0]): - basis_list.append(self.convert_b_to_polyrow(A_known[i], var_subset)) - - for i in range(self.N_CLEANING_STEPS + 1): - # TODO(FD) can e enforce lin. independance to previously found - # matrices at this point? - basis_new, S = self.get_basis(Y, method=method) - corank = basis_new.shape[0] - - if corank == 0: - print(f"{var_subset}: no new learned matrices found") - return basis_list - print(f"{var_subset}: {corank} learned matrices found") - self.test_S_cutoff(S, corank) - bad_bins = self.clean_Y(basis_new, Y, S[-corank:], plot) - if len(bad_bins) > 0: - print(f"deleting {len(bad_bins)}") - Y = np.delete(Y, bad_bins, axis=0) - else: - break - - if plot: - from lifters.plotting_tools import plot_singular_values - - plot_singular_values(S, eps=self.EPS_SVD) - - # find out which of the constraints are linearly dependant of the others. - current_basis = None - for i, bi_sub in enumerate(basis_new): - bi_poly = self.convert_b_to_polyrow(bi_sub, var_subset, tol=self.EPS_SPARSE) - ai = self.get_reduced_a(bi_sub, var_subset) - basis_list.append(bi_poly) - if current_basis is None: - current_basis = ai[None, :] - else: - current_basis = np.r_[current_basis, ai[None, :]] - return basis_list - def apply_templates(self, basis_list, n_landmarks=None, verbose=False): """ Apply the learned patterns in basis_list to all landmarks. @@ -863,8 +744,9 @@ def get_basis( :param A_known: if given, will generate basis that is orthogonal to these given constraints. - :return: basis + :return: basis, S """ + # if there is a known list of constraints, add them to the Y so that resulting nullspace is orthogonal to them if basis_known is not None: if len(A_known): @@ -881,45 +763,10 @@ def get_basis( if method != "qrp": print("using a method other than qrp is not recommended.") - if method == "svd": - U, S, Vh = np.linalg.svd( - Y - ) # nullspace of Y is in last columns of V / last rows of Vh - rank = np.sum(np.abs(S) > self.EPS_SVD) - basis = Vh[rank:, :] - - # test that it is indeed a null space - np.testing.assert_allclose(Y @ basis.T, 0.0, atol=1e-5) - elif method == "qr": - # if Y.T = QR, the last n-r columns - # of R make up the nullspace of Y. - Q, R = np.linalg.qr(Y.T) - S = np.abs(np.diag(R)) - sorted_idx = np.argsort(S)[::-1] - S = S[sorted_idx] - rank = np.where(S < self.EPS_SVD)[0][0] - # decreasing order - basis = Q[:, sorted_idx[rank:]].T - elif method == "qrp": - # Based on Section 5.5.5 "Basic Solutions via QR with Column Pivoting" from Golub and Van Loan. - - assert Y.shape[0] >= Y.shape[1], "only tall matrices supported" - - Q, R, p = la.qr(Y, pivoting=True, mode="economic") - S = np.abs(np.diag(R)) - rank = np.sum(S > self.EPS_SVD) - R1, R2 = R[:rank, :rank], R[:rank, rank:] - # [R1 R2] @ [R1^-1 @ R2] = [R2 - R2] - # [0 0 ] [ -I ] [0] - N = np.vstack([la.solve_triangular(R1, R2), -np.eye(R2.shape[1])]) - - basis = np.zeros(N.T.shape) - basis[:, p] = N.T - else: - raise ValueError(method) + basis, info = get_nullspace(Y, method=method, tolerance=self.EPS_SVD) basis[np.abs(basis) < self.EPS_SPARSE] = 0.0 - return basis, S + return basis, info["values"] def get_reduced_a(self, bi, var_subset=None, sparse=False): """ @@ -1072,9 +919,3 @@ def get_param_idx_dict(self, var_subset=None): param_dict[f"{pi}.{pj}"] = i i += 1 return param_dict - - def get_hess(self, t, y): - raise NotImplementedError("hessian not implemented") - - def get_grad(self, t, y): - raise NotImplementedError("grad not implemented") diff --git a/solve_mosek.ptf b/solve_mosek.ptf new file mode 100644 index 0000000..aa3dba4 --- /dev/null +++ b/solve_mosek.ptf @@ -0,0 +1,161 @@ +Task + # Written by MOSEK v10.0.36 + # problemtype: Conic Problem + # number of linear variables: 0 + # number of linear constraints: 59 + # number of old-style cones: 0 + # number of positive semidefinite variables: 1 + # number of positive semidefinite matrixes: 60 + # number of affine conic constraints: 0 + # number of disjunctive constraints: 0 + # number of old-style A nonzeros: 0 +Objective + Minimize + +Constraints + @c0 [1] + + @c1 [0] + + @c2 [0] + + @c3 [0] + + @c4 [0] + + @c5 [0] + + @c6 [0] + + @c7 [0] + + @c8 [0] + + @c9 [0] + + @c10 [0] + + @c11 [0] + + @c12 [0] + + @c13 [0] + + @c14 [0] + + @c15 [0] + + @c16 [0] + + @c17 [0] + + @c18 [0] + + @c19 [0] + + @c20 [0] + + @c21 [0] + + @c22 [0] + + @c23 [0] + + @c24 [0] + + @c25 [0] + + @c26 [0] + + @c27 [0] + + @c28 [0] + + @c29 [0] + + @c30 [0] + + @c31 [0] + + @c32 [0] + + @c33 [0] + + @c34 [0] + + @c35 [0] + + @c36 [0] + + @c37 [0] + + @c38 [0] + + @c39 [0] + + @c40 [0] + + @c41 [0] + + @c42 [0] + + @c43 [0] + + @c44 [0] + + @c45 [0] + + @c46 [0] + + @c47 [0] + + @c48 [0] + + @c49 [0] + + @c50 [0] + + @c51 [0] + + @c52 [0] + + @c53 [0] + + @c54 [0] + + @c55 [0] + + @c56 [0] + + @c57 [0] + + @c58 [0] + +Variables + @X0 [PSD(28)] +SymmetricMatrixes + M0 SYMMAT(28) (1,0,-0.4785055549304569) (1,1,0.7393994836597444) (2,0,-0.6207149615967967) + (2,1,0.7484258356920022) (2,2,1) (3,0,-0.47963203640487045) (3,1,0.6954010959595047) + (3,2,0.7756856732458306) (3,3,0.9620679271922155) (4,0,-0.6478065587977859) + (4,4,0.7393994836597444) (5,0,-0.6621089719013669) (5,4,0.7484258356920022) + (5,5,1) (6,0,-0.6664766738219702) (6,4,0.6954010959595047) (6,5,0.7756856732458306) + (6,6,0.9620679271922155) (7,0,-0.6271421783627252) (7,7,0.7393994836597444) + (8,0,-0.6795565004215671) (8,7,0.7484258356920022) (8,8,1) (9,0,-0.6102804227123851) + (9,7,0.6954010959595047) (9,8,0.7756856732458306) (9,9,0.9620679271922155) + (10,0,0.3884472423672094) (10,1,-0.5636364384637816) (10,2,-0.6773620756979517) + (10,3,-0.6449745956113119) (10,10,0.5409014903184014) (13,0,0.3884472423672094) + (13,1,-0.5636364384637816) (13,2,-0.6773620756979517) (13,3,-0.6449745956113119) + (13,10,0.5409014903184014) (13,13,0.5409014903184014) (15,0,0.3884472423672094) + (15,1,-0.5636364384637816) (15,2,-0.6773620756979517) (15,3,-0.6449745956113119) + (15,10,0.5409014903184014) (15,13,0.5409014903184014) (15,15,0.5409014903184014) + (16,0,0.45507632937870307) (16,4,-0.5636364384637816) (16,5,-0.6773620756979517) + (16,6,-0.6449745956113119) (16,16,0.5409014903184014) (19,0,0.45507632937870307) + (19,4,-0.5636364384637816) (19,5,-0.6773620756979517) (19,6,-0.6449745956113119) + (19,16,0.5409014903184014) (19,19,0.5409014903184014) (21,0,0.45507632937870307) + (21,4,-0.5636364384637816) (21,5,-0.6773620756979517) (21,6,-0.6449745956113119) + (21,16,0.5409014903184014) (21,19,0.5409014903184014) (21,21,0.5409014903184014) + (22,0,0.46175428328363605) (22,7,-0.5636364384637816) (22,8,-0.6773620756979517) + (22,9,-0.6449745956113119) (22,22,0.5409014903184014) (25,0,0.46175428328363605) + (25,7,-0.5636364384637816) (25,8,-0.6773620756979517) (25,9,-0.6449745956113119) + (25,22,0.5409014903184014) (25,25,0.5409014903184014) (27,0,0.46175428328363605) + (27,7,-0.5636364384637816) (27,8,-0.6773620756979517) (27,9,-0.6449745956113119) + (27,22,0.5409014903184014) (27,25,0.5409014903184014) (27,27,0.5409014903184014) + M1 SYMMAT(28) (0,0,1) + M2 SYMMAT(28) (1,1,1) (2,2,1) (3,3,1) (10,0,-0.5) (13,0,-0.5) (15,0,-0.5) + M3 SYMMAT(28) (4,4,1) (5,5,1) (6,6,1) (16,0,-0.5) (19,0,-0.5) (21,0,-0.5) + M4 SYMMAT(28) (7,7,1) (8,8,1) (9,9,1) (22,0,-0.5) (25,0,-0.5) (27,0,-0.5) + M5 SYMMAT(28) (4,4,0.999999999999995) (5,5,-1) (16,0,-0.5000000000000066) (19,0,0.5000000000000049) + M6 SYMMAT(28) (1,1,-1) (3,3,0.9999999999999976) (10,0,0.5000000000000001) (15,0,-0.4999999999999977) + M7 SYMMAT(28) (7,7,0.9999999999999977) (8,8,-1) (22,0,-0.500000000000002) (25,0,0.49999999999999545) + M8 SYMMAT(28) (7,7,1.0000000000000016) (9,9,-1) (22,0,-0.499999999999997) (27,0,0.5) + M9 SYMMAT(28) (4,4,0.999999999999996) (6,6,-1) (16,0,-0.5) (21,0,0.49999999999999495) + M10 SYMMAT(28) (2,2,-1) (3,3,1.0000000000000009) (13,0,0.4999999999999978) (15,0,-0.4999999999999954) + M11 SYMMAT(28) (9,7,-0.7071067811865475) (24,0,0.7071067811865505) + M12 SYMMAT(28) (13,3,-0.7071067811865475) (14,2,0.7071067811865478) + M13 SYMMAT(28) (22,9,0.7071067811865459) (24,7,-0.7071067811865475) + M14 SYMMAT(28) (12,12,-1) (15,10,0.49999999999999767) + M15 SYMMAT(28) (17,5,0.7071067811865512) (19,4,-0.7071067811865475) + M16 SYMMAT(28) (17,17,-1) (19,16,0.5000000000000009) + M17 SYMMAT(28) (18,18,-1) (21,16,0.5000000000000004) + M18 SYMMAT(28) (18,6,0.7071067811865466) (21,4,-0.7071067811865475) + M19 SYMMAT(28) (23,8,-0.7071067811865475) (25,7,0.7071067811865481) + M20 SYMMAT(28) (14,14,-1) (15,13,0.4999999999999992) + M21 SYMMAT(28) (16,5,0.7071067811865467) (17,4,-0.7071067811865475) + M22 SYMMAT(28) (10,3,-0.7071067811865475) (12,1,0.7071067811865465) + M23 SYMMAT(28) (26,9,-0.7071067811865475) (27,8,0.7071067811865482) + M24 SYMMAT(28) (11,2,-0.7071067811865475) (13,1,0.7071067811865501) + M25 SYMMAT(28) (25,9,0.7071067811865468) (26,8,-0.7071067811865475) + M26 SYMMAT(28) (10,2,0.7071067811865477) (11,1,-0.7071067811865475) + M27 SYMMAT(28) (5,4,0.7071067811865475) (17,0,-0.7071067811865475) + M28 SYMMAT(28) (6,5,-0.7071067811865475) (20,0,0.707106781186549) + M29 SYMMAT(28) (19,6,0.7071067811865471) (20,5,-0.7071067811865475) + M30 SYMMAT(28) (20,20,-1) (21,19,0.49999999999999956) + M31 SYMMAT(28) (24,9,-0.7071067811865475) (27,7,0.7071067811865468) + M32 SYMMAT(28) (22,8,-0.7071067811865475) (23,7,0.7071067811865468) + M33 SYMMAT(28) (23,23,-1) (25,22,0.4999999999999994) + M34 SYMMAT(28) (26,26,-1) (27,25,0.5000000000000006) + M35 SYMMAT(28) (20,6,-0.7071067811865475) (21,5,0.7071067811865459) + M36 SYMMAT(28) (11,11,-1) (13,10,0.5000000000000001) + M37 SYMMAT(28) (16,6,-0.7071067811865475) (18,4,0.7071067811865467) + M38 SYMMAT(28) (17,6,0.7071067811865481) (20,4,-0.7071067811865475) + M39 SYMMAT(28) (14,3,0.7071067811865471) (15,2,-0.7071067811865475) + M40 SYMMAT(28) (12,2,-0.7071067811865475) (14,1,0.7071067811865465) + M41 SYMMAT(28) (18,17,-0.7071067811865475) (20,16,0.7071067811865475) + M42 SYMMAT(28) (12,3,-0.7071067811865475) (15,1,0.7071067811865497) + M43 SYMMAT(28) (24,8,0.7071067811865454) (26,7,-0.7071067811865475) + M44 SYMMAT(28) (24,23,0.7071067811865481) (26,22,-0.7071067811865475) + M45 SYMMAT(28) (9,8,0.707106781186547) (26,0,-0.7071067811865475) + M46 SYMMAT(28) (20,18,0.7071067811865475) (21,17,-0.7071067811865475) + M47 SYMMAT(28) (25,24,-0.7071067811865475) (26,23,0.7071067811865475) + M48 SYMMAT(28) (14,12,-0.7071067811865475) (15,11,0.7071067811865477) + M49 SYMMAT(28) (19,18,0.7071067811865477) (20,17,-0.7071067811865475) + M50 SYMMAT(28) (24,24,-1) (27,22,0.49999999999999906) + M51 SYMMAT(28) (26,24,0.7071067811865468) (27,23,-0.7071067811865475) + M52 SYMMAT(28) (12,11,-0.7071067811865475) (14,10,0.7071067811865472) + M53 SYMMAT(28) (13,12,-0.7071067811865475) (14,11,0.7071067811865481) + M54 SYMMAT(28) (23,9,-0.7071067811865475) (24,8,0.7071067811865479) + M55 SYMMAT(28) (11,3,-0.7071067811865475) (14,1,0.7071067811865472) + M56 SYMMAT(28) (17,6,0.7071067811865482) (18,5,-0.7071067811865475) + M57 SYMMAT(28) (6,4,-0.7071067811865475) (18,0,0.7071067811865479) + M58 SYMMAT(28) (3,2,-0.7071067811865475) (14,0,0.7071067811865469) + M59 SYMMAT(28) (3,1,0.7071067811865471) (12,0,-0.7071067811865475) diff --git a/solvers/common.py b/solvers/common.py index f44a490..1feef0c 100644 --- a/solvers/common.py +++ b/solvers/common.py @@ -6,86 +6,37 @@ from lifters.range_only_lifters import RangeOnlyLocLifter from lifters.stereo_lifter import StereoLifter +from cert_tools import solve_sdp_mosek +from cert_tools import solve_feasibility_sdp + TOL = 1e-10 # can be overwritten by a parameter. # Reference for MOSEK parameters explanations: # https://docs.mosek.com/latest/pythonapi/parameters.html#doc-all-parameter-list - VERBOSE = False -SOLVER = "MOSEK" # first choice solver_options = { - None: {}, - "CVXOPT": { - "verbose": VERBOSE, - "refinement": 1, - "kktsolver": "qr", # important so that we can solve with redundant constraints - "abstol": 1e-7, # will be changed according to primal - "reltol": 1e-6, # will be changed according to primal - "feastol": 1e-9, - }, - "MOSEK": { - "verbose": VERBOSE, - "mosek_params": { - "MSK_IPAR_INTPNT_MAX_ITERATIONS": 500, - "MSK_DPAR_INTPNT_CO_TOL_PFEAS": TOL, # was 1e-8 - "MSK_DPAR_INTPNT_CO_TOL_DFEAS": TOL, # was 1e-8 - # "MSK_DPAR_INTPNT_CO_TOL_REL_GAP": 1e-7, - # "MSK_DPAR_INTPNT_CO_TOL_REL_GAP": 1e-10, # this made the problem infeasible sometimes - "MSK_DPAR_INTPNT_CO_TOL_MU_RED": TOL, - "MSK_DPAR_INTPNT_CO_TOL_INFEAS": 1e-12, - "MSK_IPAR_INTPNT_SOLVE_FORM": "MSK_SOLVE_DUAL", - }, + "verbose": VERBOSE, + "mosek_params": { + "MSK_IPAR_INTPNT_MAX_ITERATIONS": 500, + "MSK_DPAR_INTPNT_CO_TOL_PFEAS": TOL, # was 1e-8 + "MSK_DPAR_INTPNT_CO_TOL_DFEAS": TOL, # was 1e-8 + # "MSK_DPAR_INTPNT_CO_TOL_REL_GAP": 1e-7, + # "MSK_DPAR_INTPNT_CO_TOL_REL_GAP": 1e-10, # this made the problem infeasible sometimes + "MSK_DPAR_INTPNT_CO_TOL_MU_RED": TOL, + "MSK_DPAR_INTPNT_CO_TOL_INFEAS": 1e-12, + "MSK_IPAR_INTPNT_SOLVE_FORM": "MSK_SOLVE_DUAL", }, } def adjust_tol(options, tol): - for opt in options: - if "mosek_params" in opt: - opt["mosek_params"].update( - { - "MSK_DPAR_INTPNT_CO_TOL_PFEAS": tol, - "MSK_DPAR_INTPNT_CO_TOL_DFEAS": tol, - "MSK_DPAR_INTPNT_CO_TOL_MU_RED": tol, - } - ) - else: - opt.update( - { - "abstol": tol, - "reltol": tol, - "feastol": tol, - } - ) - - -def adjust_Q(Q, offset=True, scale=True, plot=False): - from copy import deepcopy - - ii, jj = (Q == Q.max()).nonzero() - if (ii[0], jj[0]) != (0, 0) or (len(ii) > 1): - pass - # print("Warning: largest element of Q is not unique or not in top-left. Check ordering?") - - Q_mat = deepcopy(Q) - if offset: - Q_offset = Q_mat[0, 0] - else: - Q_offset = 0 - Q_mat[0, 0] -= Q_offset - - if scale: - # Q_scale = spl.norm(Q_mat, "fro") - Q_scale = Q_mat.max() - else: - Q_scale = 1.0 - Q_mat /= Q_scale - if plot: - fig, axs = plt.subplots(1, 2) - axs[0].matshow(np.log10(np.abs(Q.toarray()))) - axs[1].matshow(np.log10(np.abs(Q_mat.toarray()))) - plt.show() - return Q_mat, Q_scale, Q_offset + options["mosek_params"].update( + { + "MSK_DPAR_INTPNT_CO_TOL_PFEAS": tol, + "MSK_DPAR_INTPNT_CO_TOL_DFEAS": tol, + "MSK_DPAR_INTPNT_CO_TOL_MU_RED": tol, + } + ) def solve_sdp_cvxpy( @@ -93,147 +44,19 @@ def solve_sdp_cvxpy( A_b_list, B_list=[], adjust=True, - solver=SOLVER, primal=False, verbose=True, tol=None, ): - """Run CVXPY to solve a semidefinite program. - - Args: - Q (_type_): Cost matrix - A_b_list (_type_): constraint tuple, (A,b) such that trace(A @ X) == b - adjust (bool, optional): Choose to normalize Q matrix. - primal (bool, optional): Choose to solve primal or dual. Defaults to False (dual). - verbose (bool, optional): Print verbose ouptut. Defaults to True. + assert primal is False, "Option primal not supported anymore, it is less efficient." + assert len(B_list) == 0, "Inequality constraints not supported anymore." - Returns: - _type_: (X, cost_out): solution matrix and output cost. - """ - opts = solver_options[solver] + opts = solver_options opts["verbose"] = verbose - if tol: - adjust_tol([opts], tol) - - Q_here, scale, offset = adjust_Q(Q) if adjust else (Q, 1.0, 0.0) - if primal: - """ - min < Q, X > - s.t. trace(Ai @ X) == bi, for all i. - """ - X = cp.Variable(Q.shape, symmetric=True) - constraints = [X >> 0] - constraints += [cp.trace(A @ X) == b for A, b in A_b_list] - constraints += [cp.trace(B @ X) <= 0 for B in B_list] - cprob = cp.Problem(cp.Minimize(cp.trace(Q_here @ X)), constraints) - try: - cprob.solve( - solver=solver, - # save_file="solve_cvxpy_primal.ptf", - **opts, - ) - except Exception as e: - print(e) - cost = None - X = None - H = None - yvals = None - msg = "infeasible / unknown" - else: - if np.isfinite(cprob.value): - cost = cprob.value - X = X.value - H = constraints[0].dual_value - yvals = [c.dual_value for c in constraints[1:]] - msg = "converged" - else: - cost = None - X = None - H = None - yvals = None - msg = "unbounded" - else: # Dual - """ - max < y, b > - s.t. sum(Ai * yi for all i) << Q - """ - m = len(A_b_list) - y = cp.Variable(shape=(m,)) + adjust_tol(opts, tol) - k = len(B_list) - if k > 0: - u = cp.Variable(shape=(k,)) - - As, b = zip(*A_b_list) - b = np.concatenate([np.atleast_1d(bi) for bi in b]) - objective = cp.Maximize(b @ y) - - # We want the lagrangian to be H := Q - sum l_i * A_i + sum u_i * B_i. - # With this choice, l_0 will be negative - LHS = cp.sum( - [y[i] * Ai for (i, Ai) in enumerate(As)] - + [-u[i] * Bi for (i, Bi) in enumerate(B_list)] - ) - # this does not include symmetry of Q!! - constraints = [LHS << Q_here] - constraints += [LHS == LHS.T] - if k > 0: - constraints.append(u >= 0) - - cprob = cp.Problem(objective, constraints) - try: - cprob.solve( - solver=solver, - **opts, - ) - except Exception as e: - print(e) - cost = None - X = None - H = None - yvals = None - msg = "infeasible / unknown" - else: - if np.isfinite(cprob.value): - cost = cprob.value - X = constraints[0].dual_value - H = Q_here - LHS.value - yvals = [x.value for x in y] - - # sanity check for inequality constraints. - # we want them to be inactive!!! - if len(B_list): - mu = np.array([ui.value for ui in u]) - i_nnz = np.where(mu > 1e-10)[0] - if len(i_nnz): - for i in i_nnz: - print( - f"Warning: is constraint {i} active? (mu={mu[i]:.4e}):" - ) - print(np.trace(B_list[i] @ X)) - msg = "converged" - else: - cost = None - X = None - H = None - yvals = None - msg = "unbounded" - - # reverse Q adjustment - if cost: - cost = cost * scale + offset - - H = Q_here - cp.sum( - [yvals[i] * Ai for (i, Ai) in enumerate(As)] - + [-u[i] * Bi for (i, Bi) in enumerate(B_list)] - ) - yvals[0] = yvals[0] * scale + offset - # H *= scale - # H[0, 0] += offset - - info = {"H": H, "yvals": yvals, "cost": cost, "msg": msg} - return X, info + return solve_sdp_mosek(Q, A_b_list, adjust=adjust, verbose=verbose) def find_local_minimum( @@ -356,67 +179,12 @@ def solve_certificate( A_b_list, xhat, adjust=True, - solver=SOLVER, verbose=False, tol=None, ): """Solve certificate.""" - opts = solver_options[solver] + opts = solver_options opts["verbose"] = verbose - if tol: - adjust_tol([opts], tol) - - Q_here, scale, offset = adjust_Q(Q) if adjust else (Q, 1.0, 0.0) - - m = len(A_b_list) - y = cp.Variable(shape=(m,)) - - As, b = zip(*A_b_list) - b = np.concatenate([np.atleast_1d(bi) for bi in b]) - - H = cp.sum([Q_here] + [y[i] * Ai for (i, Ai) in enumerate(As)]) - constraints = [H >> 0] - eps = cp.Variable() - constraints += [H @ xhat <= eps] - constraints += [H @ xhat >= -eps] - - objective = cp.Minimize(eps) - - cprob = cp.Problem(objective, constraints) - try: - cprob.solve( - solver=solver, - **opts, - ) - except Exception as e: - eps = None - cost = None - X = None - H = None - yvals = None - msg = "infeasible / unknown" - else: - if np.isfinite(cprob.value): - cost = cprob.value - X = constraints[0].dual_value - H = H.value - yvals = [x.value for x in y] - msg = "converged" - else: - eps = None - cost = None - X = None - H = None - yvals = None - msg = "unbounded" - - # reverse Q adjustment - if cost: - eps = eps.value - cost = cost * scale + offset - H = Q_here + cp.sum([yvals[i] * Ai for (i, Ai) in enumerate(As)]) - yvals[0] = yvals[0] * scale + offset - - info = {"X": X, "yvals": yvals, "cost": cost, "msg": msg, "eps": eps} - return H, info + adjust_tol(opts, tol) + return solve_feasibility_sdp(Q, A_b_list, xhat, adjust=adjust, sdp_opts=opts) diff --git a/solvers/sparse.py b/solvers/sparse.py index ae3fe6e..50a4ae2 100644 --- a/solvers/sparse.py +++ b/solvers/sparse.py @@ -1,17 +1,16 @@ import cvxpy as cp -import numpy as np -from .common import adjust_Q, adjust_tol, solver_options +from solvers.common import adjust_tol, solver_options + +from cert_tools import adjust_Q VERBOSE = False -SOLVER = "MOSEK" # for computing the lambda parameter, we are adding all possible constraints # and therefore we might run into numerical problems. Setting below to a high value # was found to lead to less cases where the solver terminates with "UNKNOWN" status. # see https://docs.mosek.com/latest/pythonapi/parameters.html#doc-all-parameter-list LAMBDA_REL_GAP = 0.1 - EPSILON = 1e-4 # set to None to minimize epsilon @@ -73,8 +72,7 @@ def solve_lambda( B_list=[], force_first=1, adjust=False, - solver=SOLVER, - opts=solver_options[SOLVER], + opts=solver_options, primal=False, verbose=False, tol=None, @@ -83,7 +81,7 @@ def solve_lambda( :param force_first: number of constraints on which we do not put a L1 cost, effectively encouraging the problem to use them. """ - adjust_tol([opts], tol) if tol else None + adjust_tol(opts, tol) if tol else None Q_here, scale, offset = adjust_Q(Q) if adjust else (Q, 1.0, 0.0) if primal: @@ -133,13 +131,9 @@ def solve_lambda( cprob = cp.Problem(objective, constraints) opts["verbose"] = verbose - if solver == "MOSEK": - opts["mosek_params"]["MSK_DPAR_INTPNT_CO_TOL_REL_GAP"] = LAMBDA_REL_GAP + opts["mosek_params"]["MSK_DPAR_INTPNT_CO_TOL_REL_GAP"] = LAMBDA_REL_GAP try: - cprob.solve( - solver=solver, - **opts, - ) + cprob.solve(solver="MOSEK", **opts) except: lamda = None X = None diff --git a/utils/common.py b/utils/common.py index b553aaf..f752343 100644 --- a/utils/common.py +++ b/utils/common.py @@ -2,28 +2,12 @@ def upper_triangular(p): - """ Given vector, get the half kronecker product. """ + """Given vector, get the half kronecker product.""" return np.outer(p, p)[np.triu_indices(len(p))] def diag_indices(n): - """ Given the half kronecker product, return diagonal elements """ + """Given the half kronecker product, return diagonal elements""" z = np.empty((n, n)) z[np.triu_indices(n)] = range(int(n * (n + 1) / 2)) return np.diag(z).astype(int) - - -def rank_project(X, p=1, tol=1e-10): - """ Project X to matrices of rank p. """ - E, V = np.linalg.eigh(X) - if p is None: - p = np.sum(np.abs(E) > tol) - x = V[:, -p:] * np.sqrt(E[-p:]) - - X_hat = np.outer(x, x) - info = { - "error X": np.linalg.norm(X_hat - X), - "error eigs": np.sum(np.abs(E[:p])) - } - return x, info - \ No newline at end of file diff --git a/utils/plotting_tools.py b/utils/plotting_tools.py index 6721e35..86361c2 100644 --- a/utils/plotting_tools.py +++ b/utils/plotting_tools.py @@ -1,12 +1,102 @@ -from lifters.plotting_tools import import_plt -import numpy as np +def import_plt(): + import matplotlib.pylab as plt + import shutil + + usetex = True if shutil.which("latex") else False + plt.rcParams.update( + { + "text.usetex": usetex, + "font.family": "DejaVu Sans", + "font.size": 12, + } + ) + plt.rc("text.latex", preamble=r"\usepackage{bm}") + return plt + -from lifters.plotting_tools import savefig, add_colorbar +import numpy as np from poly_matrix.poly_matrix import PolyMatrix +import os +import numpy as np + plt = import_plt() +def add_colorbar(fig, ax, im, title=None, nticks=None, visible=True, size=None): + from mpl_toolkits.axes_grid1 import make_axes_locatable + + divider = make_axes_locatable(ax) + if size is None: + w, h = fig.get_size_inches() + size = f"{5*h/w}%" + cax = divider.append_axes("right", size=size, pad=0.05) + if title is not None: + cax.set_ylabel(title) + + if not visible: + cax.axis("off") + return + + fig.colorbar(im, cax=cax, orientation="vertical") + + # add symmetric nticks ticks: min and max, and equally spaced in between + if nticks is not None: + from math import floor + + ticks = cax.get_yticks() + new_ticks = [ticks[0]] + step = floor(len(ticks) / (nticks - 1)) + new_ticks += list(ticks[step + 1 :: step]) + new_ticks += [ticks[-1]] + # print(f"reduce {ticks} to {new_ticks}") + assert len(new_ticks) == nticks + cax.set_yticks(ticks[::step]) + return cax + + +def add_scalebar( + ax, size=5, size_vertical=1, loc="lower left", fontsize=8, color="black", pad=0.1 +): + """Add a scale bar to the plot. + + :param ax: axis to use. + :param size: size of scale bar. + :param size_vertical: height (thckness) of the bar + :param loc: location (same syntax as for matplotlib legend) + """ + from mpl_toolkits.axes_grid1.anchored_artists import AnchoredSizeBar + import matplotlib.font_manager as fm + + fontprops = fm.FontProperties(size=fontsize) + scalebar = AnchoredSizeBar( + ax.transData, + size, + "{} m".format(size), + loc, + pad=pad, + color=color, + frameon=False, + size_vertical=size_vertical, + fontproperties=fontprops, + ) + ax.add_artist(scalebar) + + +def make_dirs_safe(path): + """Make directory of input path, if it does not exist yet.""" + dirname = os.path.dirname(path) + if not os.path.exists(dirname): + os.makedirs(dirname) + + +def savefig(fig, name, verbose=True): + make_dirs_safe(name) + fig.savefig(name, bbox_inches="tight", pad_inches=0, dpi=200) + if verbose: + print(f"saved plot as {name}") + + def plot_frame( lifter, ax, @@ -131,8 +221,31 @@ def plot_tightness(df_tight, qcqp_cost, fname_root): savefig(fig, fname_root + f"_tightness.png") +def plot_matrix( + Ai, vmin=None, vmax=None, nticks=None, title="", colorbar=True, fig=None, ax=None +): + import matplotlib + + if ax is None: + fig, ax = plt.subplots() + if fig is None: + fig = plt.gcf() + + norm = matplotlib.colors.SymLogNorm(10**-5, vmin=vmin, vmax=vmax) + if type(Ai) == np.ndarray: + im = ax.matshow(Ai, norm=norm) + else: + im = ax.matshow(Ai.toarray(), norm=norm) + ax.axis("off") + ax.set_title(title, y=1.0) + if colorbar: + add_colorbar(fig, ax, im, nticks=nticks) + else: + add_colorbar(fig, ax, im, nticks=nticks, visible=False) + return fig, ax, im + + def plot_matrices(df_tight, fname_root): - from lifters.plotting_tools import plot_matrix import itertools from math import ceil @@ -220,29 +333,17 @@ def plot_matrices(df_tight, fname_root): savefig(fig, fname) -def add_scalebar( - ax, size=5, size_vertical=1, loc="lower left", fontsize=8, color="black", pad=0.1 -): - """Add a scale bar to the plot. - - :param ax: axis to use. - :param size: size of scale bar. - :param size_vertical: height (thckness) of the bar - :param loc: location (same syntax as for matplotlib legend) - """ - from mpl_toolkits.axes_grid1.anchored_artists import AnchoredSizeBar - import matplotlib.font_manager as fm - - fontprops = fm.FontProperties(size=fontsize) - scalebar = AnchoredSizeBar( - ax.transData, - size, - "{} m".format(size), - loc, - pad=pad, - color=color, - frameon=False, - size_vertical=size_vertical, - fontproperties=fontprops, - ) - ax.add_artist(scalebar) +def plot_singular_values(S, eps=None, label="singular values", ax=None): + if ax is None: + fig, ax = plt.subplots() + else: + fig = plt.gcf() + fig.set_size_inches(4, 2) + ax.semilogy(S, marker="o", label=label) + if eps is not None: + ax.axhline(eps, color="C1") + ax.grid() + ax.set_xlabel("index") + ax.set_ylabel("magnitude of singular values") + ax.legend(loc="upper right") + return fig, ax From 388ed1a5a543cfe8ac969946ffa8d541e238877b Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 19 Sep 2023 21:20:13 -0400 Subject: [PATCH 02/13] Generate centered certificate --- centering/certificate.py | 49 +++++++++++++++++++++++++++++----------- certifiable-tools | 2 +- lifters/poly_lifters.py | 10 ++++++++ 3 files changed, 47 insertions(+), 14 deletions(-) diff --git a/centering/certificate.py b/centering/certificate.py index 8b911d5..13accf2 100644 --- a/centering/certificate.py +++ b/centering/certificate.py @@ -5,33 +5,55 @@ matplotlib.use("TkAgg") from cert_tools.linalg_tools import get_nullspace -from cert_tools.eig_tools import solve_Eopt +from cert_tools.eopt_solvers import solve_Eopt_QP -def get_certificate(Q, A_list, x_hat): +def get_certificate(Q, A_list, x_hat, centered=False): """ Compute non-centered certificate. """ L = np.concatenate([(A @ x_hat)[:, None] for A in A_list], axis=1) b = -Q @ x_hat - null_basis, info = get_nullspace(L, method="qr") - y, *_ = la.lstsq(L, b) + if centered: + redundant_idx = np.where(np.all(L == 0, axis=0))[0] + A_list_bar = [A_list[i] for i in redundant_idx] + else: + null_basis, info = get_nullspace(L, method="qr") # (n_null x N) + A_list_bar = [] + for j in range(null_basis.shape[0]): + # doesn't work with sparse: + # np.sum([Ai * ni for Ai, ni in zip(A_list, null_basis[:, j])]) + A_bar = A_list[0] * null_basis[j, 0] + for i in range(1, null_basis.shape[1]): + A_bar += A_list[i] * null_basis[j, i] + A_list_bar.append(A_bar) + y, *_ = la.lstsq(L, b) Q_bar = Q + np.sum([Ai * yi for Ai, yi in zip(A_list, y)]) - A_list_bar = [] - for j in range(null_basis.shape[1]): - A_bar = np.sum([Ai * ni for Ai, ni in zip(A_list, null_basis[:, j])]) - A_list_bar.append(A_bar) - alphas, info = solve_Eopt(Q_bar, A_list_bar) + x_init = np.zeros(len(A_list_bar)) + + if centered: + np.testing.assert_allclose(Q_bar[0, :], 0.0, atol=1e-8) + Q_bar = Q_bar[1:, 1:] + A_list_bar = [Ai[1:, 1:] for Ai in A_list_bar] + alphas, info = solve_Eopt_QP( + Q_bar, A_list_bar, x_init, verbose=2, lmin=True, l_threshold=1e-5 + ) + else: + alphas, info = solve_Eopt_QP( + Q_bar, A_list_bar, x_init, verbose=2, lmin=True, l_threshold=-1e-15 + ) return info["success"] -def get_centered_certificate(Q_bar, A_list): - x_hat = np.zeros(Q_bar.shape[0]) +def get_centered_certificate(Q, A_list, D): + x_hat = np.zeros(Q.shape[0]) x_hat[0] = 1.0 - return get_certificate(Q_bar, A_list, x_hat) + + Q_bar = D.T @ Q @ D + return get_certificate(Q_bar, A_list, x_hat, centered=True) if __name__ == "__main__": @@ -51,4 +73,5 @@ def get_centered_certificate(Q_bar, A_list): success = get_certificate(Q, A_list, x_hat) - success = get_centered_certificate(Q, A_list, x_hat) + D = lifter.get_D(t_hat) + success = get_centered_certificate(Q, A_list, D) diff --git a/certifiable-tools b/certifiable-tools index 6a02f66..080ffb8 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit 6a02f66cffa766450c51adb847dad6e2db60e64d +Subproject commit 080ffb8f4141eef57cb9d7b1de1e64ce3caeb0b1 diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index 2d640d4..a122cd3 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -113,5 +113,15 @@ def get_A_known(self): B_0["t", "z_1"] = -1 return [A_i.get_matrix(self.var_dict) for A_i in [A_1, A_2, B_0]] + def get_D(self, that): + # TODO(FD) generalize and move to PolyLifter + D = np.r_[ + np.c_[1.0, 0.0, 0.0, 0.0], + np.c_[that, 1.0, 0.0, 0.0], + np.c_[that**2, 2 * that, 1.0, 0.0], + np.c_[that**3, 3 * that**2, 3 * that, 1.0], + ] + return D + def generate_random_setup(self): self.theta_ = np.array([-1]) From c23f3a5d7de328dc4f399e30d04194d03b266b58 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 24 Oct 2023 22:05:46 -0400 Subject: [PATCH 03/13] Add example problems --- auto_template/real_experiments.py | 1 - centering/certificate.py | 49 ++++------ certifiable-tools | 2 +- examples/poly6.py | 67 +++++++++++++ examples/poly6_lifter.py | 89 +++++++++++++++++ examples/problem_utils.py | 21 ++++ examples/ro_lifter.py | 85 +++++++++++++++++ lifters/poly_lifters.py | 45 +++++---- lifters/range_only_lifters.py | 50 +++++++--- solve_mosek.ptf | 154 ++---------------------------- starloc | 2 +- 11 files changed, 358 insertions(+), 207 deletions(-) create mode 100644 examples/poly6.py create mode 100644 examples/poly6_lifter.py create mode 100644 examples/problem_utils.py create mode 100644 examples/ro_lifter.py diff --git a/auto_template/real_experiments.py b/auto_template/real_experiments.py index 3a8dd0a..2597430 100644 --- a/auto_template/real_experiments.py +++ b/auto_template/real_experiments.py @@ -3,7 +3,6 @@ import pickle import time - import matplotlib.pylab as plt import numpy as np import pandas as pd diff --git a/centering/certificate.py b/centering/certificate.py index 13accf2..c7621ae 100644 --- a/centering/certificate.py +++ b/centering/certificate.py @@ -1,3 +1,5 @@ +import pickle + import numpy as np import scipy.linalg as la import matplotlib @@ -5,32 +7,32 @@ matplotlib.use("TkAgg") from cert_tools.linalg_tools import get_nullspace -from cert_tools.eopt_solvers import solve_Eopt_QP +from cert_tools.eopt_solvers_qp import solve_Eopt_QP -def get_certificate(Q, A_list, x_hat, centered=False): +def get_certificate(Q, Constraints, x_hat, centered=False): """ Compute non-centered certificate. """ - L = np.concatenate([(A @ x_hat)[:, None] for A in A_list], axis=1) + L = np.concatenate([(A @ x_hat)[:, None] for A, b in Constraints], axis=1) b = -Q @ x_hat if centered: redundant_idx = np.where(np.all(L == 0, axis=0))[0] - A_list_bar = [A_list[i] for i in redundant_idx] + A_list_bar = [Constraints[i][0] for i in redundant_idx] else: null_basis, info = get_nullspace(L, method="qr") # (n_null x N) A_list_bar = [] for j in range(null_basis.shape[0]): # doesn't work with sparse: # np.sum([Ai * ni for Ai, ni in zip(A_list, null_basis[:, j])]) - A_bar = A_list[0] * null_basis[j, 0] + A_bar = Constraints[0][0] * null_basis[j, 0] for i in range(1, null_basis.shape[1]): - A_bar += A_list[i] * null_basis[j, i] + A_bar += Constraints[i][0] * null_basis[j, i] A_list_bar.append(A_bar) y, *_ = la.lstsq(L, b) - Q_bar = Q + np.sum([Ai * yi for Ai, yi in zip(A_list, y)]) + Q_bar = Q + np.sum([Ab[0] * yi for Ab, yi in zip(Constraints, y)]) x_init = np.zeros(len(A_list_bar)) @@ -48,30 +50,15 @@ def get_certificate(Q, A_list, x_hat, centered=False): return info["success"] -def get_centered_certificate(Q, A_list, D): - x_hat = np.zeros(Q.shape[0]) - x_hat[0] = 1.0 - - Q_bar = D.T @ Q @ D - return get_certificate(Q_bar, A_list, x_hat, centered=True) - - if __name__ == "__main__": - from lifters.poly_lifters import Poly6Lifter - - lifter = Poly6Lifter() - Q, __ = lifter.get_Q() - A_list = [lifter.get_A0()] + lifter.get_A_known() - # A_b_list = lifter.get_A_b_list(A_list) - - t_init = -1 - t_hat, info, cost = lifter.local_solver(t_init) - x_hat = lifter.get_x(t_hat) - - for A_i in A_list[1:]: - assert abs(x_hat @ A_i @ x_hat) <= 1e-10 + from examples.poly6_lifter import get_problem - success = get_certificate(Q, A_list, x_hat) + print("Original certificate...") + prob = get_problem() + success = get_certificate(prob["C"], prob["Constraints"], prob["x_cand"]) - D = lifter.get_D(t_hat) - success = get_centered_certificate(Q, A_list, D) + print("Centered certificate...") + prob = get_problem(centered=True) + success = get_certificate( + prob["C"], prob["Constraints"], prob["x_cand"], centered=True + ) diff --git a/certifiable-tools b/certifiable-tools index 080ffb8..b1bc105 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit 080ffb8f4141eef57cb9d7b1de1e64ce3caeb0b1 +Subproject commit b1bc105e174bafe8b8ba5398f3fafc3039dc1c56 diff --git a/examples/poly6.py b/examples/poly6.py new file mode 100644 index 0000000..4fa0ff5 --- /dev/null +++ b/examples/poly6.py @@ -0,0 +1,67 @@ +import numpy as np +import scipy.sparse as sp + +import matplotlib.pylab as plt + + +def plot_problem(problem, t_lims=[-1, 1]): + Q = problem["C"] + + ts = np.linspace(*t_lims, 20) + ys = [(t ** np.arange(4)).T @ Q @ t ** np.arange(4) for t in ts] + + fig, ax = plt.subplots() + ax.plot(ts, ys) + t = problem["x_cand"][1] + ax.scatter([t], t ** np.arange(4) @ Q @ t ** np.arange(4)) + plt.show() + + +def get_problem(): + # Sixth order polynomial that requires redundant constraints to solve + Q = np.array( + [ + [5.0000, 1.3167, -1.4481, 0], + [1.3167, -1.4481, 0, 0.2685], + [-1.4481, 0, 0.2685, -0.0667], + [0, 0.2685, -0.0667, 0.0389], + ] + ) + Constraints = [] + A = sp.csc_array((4, 4)) # w^2 = 1 + A[0, 0] = 1 + Constraints += [(A, 1.0)] + A = sp.csc_array((4, 4)) # x^2 = x*x + A[2, 0] = 1 / 2 + A[0, 2] = 1 / 2 + A[1, 1] = -1 + Constraints += [(A, 0.0)] + A = sp.csc_array((4, 4)) # x^3 = x^2*x + A[3, 0] = 1 + A[0, 3] = 1 + A[1, 2] = -1 + A[2, 1] = -1 + Constraints += [(A, 0.0)] + A = sp.csc_array((4, 4)) # x^3*x = x^2*x^2 + A[3, 1] = 1 / 2 + A[1, 3] = 1 / 2 + A[2, 2] = -1 + Constraints += [(A, 0.0)] + + # Candidate solution + x_cand = np.array([[1.0000, -1.4871, 2.2115, -3.2888]]).T + + # Dual optimal + mults = -np.array([[-3.1937], [2.5759], [-0.0562], [0.8318]]) + + return dict(Constraints=Constraints, Q=Q, x_cand=x_cand, opt_mults=mults) + + +if __name__ == "__main__": + fname = "certifiable-tools/_test/test_prob_9.pkl" + from problem_utils import save_test_problem + + prob = get_problem() + save_test_problem(**prob, fname=fname) + + plot_problem(prob, t_lims=[-3, 3]) diff --git a/examples/poly6_lifter.py b/examples/poly6_lifter.py new file mode 100644 index 0000000..1f690ae --- /dev/null +++ b/examples/poly6_lifter.py @@ -0,0 +1,89 @@ +import numpy as np +import scipy.sparse as sp + +import matplotlib.pylab as plt + +from lifters.poly_lifters import Poly6Lifter as Lifter +from utils.plotting_tools import savefig + + +def plot_problem(problem, t_lims=[-2, 5], fname=""): + lifter = Lifter() + Q = problem["Q"] + + ts = np.arange(*t_lims, 0.1) + ys = [lifter.get_x(t).T @ Q @ lifter.get_x(t) for t in ts] + + fig, ax = plt.subplots() + ax.plot(ts, ys) + t = problem["x_cand"][1] + ax.scatter([t], problem["x_cand"].T @ Q @ problem["x_cand"]) + ax.set_yscale("symlog") + if fname != "": + savefig(fig, fname) + + +def get_problem(poly_type="A", centered=False, t_init=-1): + lifter = Lifter(poly_type=poly_type) + Q, __ = lifter.get_Q() + A_list = lifter.get_A_known() + Constraints = lifter.get_A_b_list(A_list) + + t_hat, info, cost = lifter.local_solver(t_init) + + if centered: + D = lifter.get_D(t_hat) + Q = D.T @ Q @ D + + x_hat = np.zeros(Q.shape[0]) + x_hat[0] = 1.0 + else: + x_hat = lifter.get_x(t_hat) + for A_i in A_list[1:]: + assert abs(x_hat @ A_i @ x_hat) <= 1e-10 + + return dict(Q=Q, Constraints=Constraints, x_cand=x_hat) + + +if __name__ == "__main__": + from problem_utils import save_test_problem + + poly_type = "A" + number = 8 + + # poly_type = "B" + # number = 9 + + for centered in [False, True]: + appendix = "c" if centered else "" + + prob = get_problem(poly_type=poly_type, t_init=-1, centered=centered) + fname = f"certifiable-tools/_test/test_prob_{number}G{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-2, 5], fname=fname.replace(".pkl", ".png")) + + prob = get_problem(poly_type=poly_type, t_init=1, centered=centered) + fname = f"certifiable-tools/_test/test_prob_{number}L1{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-2, 5], fname=fname.replace(".pkl", ".png")) + + prob = get_problem(poly_type=poly_type, t_init=5, centered=centered) + fname = f"certifiable-tools/_test/test_prob_{number}L2{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-2, 5], fname=fname.replace(".pkl", ".png")) + + poly_type = "B" + number = 9 + + for centered in [False, True]: + appendix = "c" if centered else "" + + prob = get_problem(poly_type=poly_type, t_init=-1, centered=centered) + fname = f"certifiable-tools/_test/test_prob_{number}G{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-2, 5], fname=fname.replace(".pkl", ".png")) + + prob = get_problem(poly_type=poly_type, t_init=5, centered=centered) + fname = f"certifiable-tools/_test/test_prob_{number}L{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-2, 5], fname=fname.replace(".pkl", ".png")) diff --git a/examples/problem_utils.py b/examples/problem_utils.py new file mode 100644 index 0000000..9cb285a --- /dev/null +++ b/examples/problem_utils.py @@ -0,0 +1,21 @@ +import numpy as np +import pickle + + +def save_test_problem(Q, Constraints, x_cand=None, fname="", **kwargs): + test_problem_dict = {} + test_problem_dict["Q"] = Q + test_problem_dict["Constraints"] = Constraints + test_problem_dict.update(kwargs) + + if x_cand is not None: + test_problem_dict["x_cand"] = np.atleast_2d(x_cand).reshape((-1, 1)) + else: + x_cand = np.zeros((Q.shape[0], 1)) + x_cand[0, 0] = 1.0 + test_problem_dict["x_cand"] = x_cand + + with open(fname, "wb") as f: + pickle.dump(test_problem_dict, f, protocol=pickle.HIGHEST_PROTOCOL) + + print(f"saved testproblem as {fname}.") diff --git a/examples/ro_lifter.py b/examples/ro_lifter.py new file mode 100644 index 0000000..cd82bdb --- /dev/null +++ b/examples/ro_lifter.py @@ -0,0 +1,85 @@ +import numpy as np +import matplotlib.pylab as plt + +from lifters.range_only_lifters import RangeOnlyLocLifter as Lifter +from utils.plotting_tools import savefig + + +def get_problem(t_init=[0, 0], centered=False, level="no"): + # Create lifter + np.random.seed(0) + n_landmarks = 4 + d = 2 + lifter = Lifter(n_positions=1, n_landmarks=n_landmarks, d=d, level=level) + lifter.landmarks = np.c_[ + np.ones(n_landmarks) + np.random.rand(4) * 0.5, np.arange(n_landmarks) + ] + lifter.theta = np.array([0.3, 1.0]) + + Q, y = lifter.get_Q() + + from auto_template.learner import Learner + + learner = Learner(lifter=lifter, variable_list=[["h", "x_0", "z_0"]]) + learner.run() + Constraints = learner.get_A_b_list() + + t, *_ = lifter.local_solver(t_init=t_init, y=y) + + if centered: + D = lifter.get_D(t) + Q = D.T @ Q @ D + x_hat = np.zeros(lifter.get_dim_x()) + x_hat[0] = 1.0 + else: + x_hat = lifter.get_x(theta=t) + return dict(Q=Q, Constraints=Constraints, x_cand=x_hat), lifter + + +def plot_problem(prob, lifter, fname=""): + Q = prob["Q"] + + xx, yy = np.meshgrid(np.arange(-1, 4, 0.1), np.arange(-1, 4, 0.1)) + zz = np.empty(xx.shape) + for i in range(xx.shape[0]): + for j in range(yy.shape[1]): + x = lifter.get_x(theta=np.array([xx[i, j], yy[i, j]])) + zz[i, j] = x.T @ Q @ x + + fig, ax = plt.subplots() + ax.pcolormesh(xx, yy, np.log10(zz)) + ax.scatter( + lifter.landmarks[:, 0], lifter.landmarks[:, 1], color="white", marker="o" + ) + ax.scatter(lifter.theta[0], lifter.theta[1], color="white", marker="*") + ax.scatter(prob["x_cand"][1], prob["x_cand"][2], color="green", marker="*") + plt.show() + if fname != "": + savefig(fig, fname) + + +if __name__ == "__main__": + from problem_utils import save_test_problem + + number = 10 + level = "no" + + # number = 11 + # level = "quad" + + for centered in [False, True]: + appendix = "c" if centered else "" + + prob, lifter = get_problem( + t_init=np.array([0, 0]), centered=centered, level=level + ) + fname = f"certifiable-tools/_test/test_prob_{number}G{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, lifter, fname=fname.replace(".pkl", ".png")) + + prob, lifter = get_problem( + t_init=np.array([2, 0]), centered=centered, level=level + ) + fname = f"certifiable-tools/_test/test_prob_{number}L{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, lifter, fname=fname.replace(".pkl", ".png")) diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index a122cd3..c56cb3b 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -80,19 +80,30 @@ def generate_random_setup(self): class Poly6Lifter(PolyLifter): - def __init__(self): + def __init__(self, poly_type="A"): + assert poly_type in ["A", "B"] + self.poly_type = poly_type super().__init__(degree=6) def get_Q_mat(self): - return ( - np.r_[ - np.c_[25, 12, 0, 0], - np.c_[12, -13, -5 / 2, 0], - np.c_[0, -5 / 2, 25 / 4, -9 / 10], - np.c_[0, 0, -9 / 10, 1 / 6], - ] - / 10 - ) + if self.poly_type == "A": + return 0.1 * np.array( + [ + [25, 12, 0, 0], + [12, -13, -2.5, 0], + [0, -2.5, 6.25, -0.9], + [0, 0, -0.9, 1 / 6], + ] + ) + elif self.poly_type == "B": + return np.array( + [ + [5.0000, 1.3167, -1.4481, 0], + [1.3167, -1.4481, 0, 0.2685], + [-1.4481, 0, 0.2685, -0.0667], + [0, 0.2685, -0.0667, 0.0389], + ] + ) def get_A_known(self): from poly_matrix import PolyMatrix @@ -115,12 +126,14 @@ def get_A_known(self): def get_D(self, that): # TODO(FD) generalize and move to PolyLifter - D = np.r_[ - np.c_[1.0, 0.0, 0.0, 0.0], - np.c_[that, 1.0, 0.0, 0.0], - np.c_[that**2, 2 * that, 1.0, 0.0], - np.c_[that**3, 3 * that**2, 3 * that, 1.0], - ] + D = np.array( + [ + [1.0, 0.0, 0.0, 0.0], + [that, 1.0, 0.0, 0.0], + [that**2, 2 * that, 1.0, 0.0], + [that**3, 3 * that**2, 3 * that, 1.0], + ] + ) return D def generate_random_setup(self): diff --git a/lifters/range_only_lifters.py b/lifters/range_only_lifters.py index 086675b..7cfc934 100644 --- a/lifters/range_only_lifters.py +++ b/lifters/range_only_lifters.py @@ -15,7 +15,7 @@ # TODO(FD): parameters below are not all equivalent. SOLVER_KWARGS = { - "BFGS": dict(gtol=1e-6, xrtol=1e-10), # relative step size + "BFGS": dict(gtol=1e-6), # xtol=1e-10), # relative step size "Nelder-Mead": dict(xatol=1e-10), # absolute step size "Powell": dict(ftol=1e-6, xtol=1e-10), "TNC": dict(gtol=1e-6, xtol=1e-10), @@ -26,7 +26,7 @@ class RangeOnlyLocLifter(StateLifter): """Range-only localization - level "no" uses substitution z_i=||p_i||^2=x_i^2 + y_i^2 - - level "quad" uses substitution z_i=[x_i^2, y_i^2, x_iy_i] + - level "quad" uses substitution z_i=[x_i^2, x_iy_i, y_i^2] """ TIGHTNESS = "rank" @@ -127,18 +127,35 @@ def get_A_known(self, var_dict=None, output_poly=False): A_list = [] for n in positions: - A = PolyMatrix(symmetric=True) - A[f"x_{n}", f"x_{n}"] = np.eye(self.d) if self.level == "no": + A = PolyMatrix(symmetric=True) + A[f"x_{n}", f"x_{n}"] = np.eye(self.d) A["h", f"z_{n}"] = -0.5 + if output_poly: + A_list.append(A) + else: + A_list.append(A.get_matrix(self.var_dict)) + elif self.level == "quad": - mat = np.zeros((1, self.size_z)) - mat[0, diag_idx] = -0.5 - A["h", f"z_{n}"] = mat - if output_poly: - A_list.append(A) - else: - A_list.append(A.get_matrix(self.var_dict)) + count = 0 + for i in range(self.d): + for j in range(i, self.d): + A = PolyMatrix(symmetric=True) + mat_x = np.zeros((self.d, self.d)) + mat_z = np.zeros((1, self.size_z)) + if i == j: + mat_x[i, i] = 1.0 + else: + mat_x[i, j] = 0.5 + mat_x[j, i] = 0.5 + mat_z[0, count] = -0.5 + A[f"x_{n}", f"x_{n}"] = mat_x + A["h", f"z_{n}"] = mat_z + count += 1 + if output_poly: + A_list.append(A) + else: + A_list.append(A.get_matrix(self.var_dict)) return A_list def get_x(self, theta=None, parameters=None, var_subset=None): @@ -357,6 +374,17 @@ def get_Q(self, noise: float = None) -> tuple: assert abs(cost1 - cost3) < 1e-10 return Q, self.y_ + def get_D(self, that): + D = np.eye(1 + self.n_positions * self.d + self.size_z) + x = self.get_x(theta=that) + J = self.get_J_lifting(t=that) + + D = sp.lil_array((len(x), len(x))) + D[range(len(x)), range(len(x))] = 1.0 + D[:, 0] = x + D[-J.shape[0] :, 1 : 1 + J.shape[1]] = J + return D.tocsc() + def get_sub_idx_x(self, sub_idx, add_z=True): sub_idx_x = [0] for idx in sub_idx: diff --git a/solve_mosek.ptf b/solve_mosek.ptf index aa3dba4..8116623 100644 --- a/solve_mosek.ptf +++ b/solve_mosek.ptf @@ -1,11 +1,11 @@ Task - # Written by MOSEK v10.0.36 + # Written by MOSEK v10.1.15 # problemtype: Conic Problem # number of linear variables: 0 - # number of linear constraints: 59 + # number of linear constraints: 2 # number of old-style cones: 0 # number of positive semidefinite variables: 1 - # number of positive semidefinite matrixes: 60 + # number of positive semidefinite matrixes: 3 # number of affine conic constraints: 0 # number of disjunctive constraints: 0 # number of old-style A nonzeros: 0 @@ -14,148 +14,10 @@ Objective Constraints @c0 [1] + @c1 [0] + - @c2 [0] + - @c3 [0] + - @c4 [0] + - @c5 [0] + - @c6 [0] + - @c7 [0] + - @c8 [0] + - @c9 [0] + - @c10 [0] + - @c11 [0] + - @c12 [0] + - @c13 [0] + - @c14 [0] + - @c15 [0] + - @c16 [0] + - @c17 [0] + - @c18 [0] + - @c19 [0] + - @c20 [0] + - @c21 [0] + - @c22 [0] + - @c23 [0] + - @c24 [0] + - @c25 [0] + - @c26 [0] + - @c27 [0] + - @c28 [0] + - @c29 [0] + - @c30 [0] + - @c31 [0] + - @c32 [0] + - @c33 [0] + - @c34 [0] + - @c35 [0] + - @c36 [0] + - @c37 [0] + - @c38 [0] + - @c39 [0] + - @c40 [0] + - @c41 [0] + - @c42 [0] + - @c43 [0] + - @c44 [0] + - @c45 [0] + - @c46 [0] + - @c47 [0] + - @c48 [0] + - @c49 [0] + - @c50 [0] + - @c51 [0] + - @c52 [0] + - @c53 [0] + - @c54 [0] + - @c55 [0] + - @c56 [0] + - @c57 [0] + - @c58 [0] + Variables - @X0 [PSD(28)] + @X0 [PSD(4)] SymmetricMatrixes - M0 SYMMAT(28) (1,0,-0.4785055549304569) (1,1,0.7393994836597444) (2,0,-0.6207149615967967) - (2,1,0.7484258356920022) (2,2,1) (3,0,-0.47963203640487045) (3,1,0.6954010959595047) - (3,2,0.7756856732458306) (3,3,0.9620679271922155) (4,0,-0.6478065587977859) - (4,4,0.7393994836597444) (5,0,-0.6621089719013669) (5,4,0.7484258356920022) - (5,5,1) (6,0,-0.6664766738219702) (6,4,0.6954010959595047) (6,5,0.7756856732458306) - (6,6,0.9620679271922155) (7,0,-0.6271421783627252) (7,7,0.7393994836597444) - (8,0,-0.6795565004215671) (8,7,0.7484258356920022) (8,8,1) (9,0,-0.6102804227123851) - (9,7,0.6954010959595047) (9,8,0.7756856732458306) (9,9,0.9620679271922155) - (10,0,0.3884472423672094) (10,1,-0.5636364384637816) (10,2,-0.6773620756979517) - (10,3,-0.6449745956113119) (10,10,0.5409014903184014) (13,0,0.3884472423672094) - (13,1,-0.5636364384637816) (13,2,-0.6773620756979517) (13,3,-0.6449745956113119) - (13,10,0.5409014903184014) (13,13,0.5409014903184014) (15,0,0.3884472423672094) - (15,1,-0.5636364384637816) (15,2,-0.6773620756979517) (15,3,-0.6449745956113119) - (15,10,0.5409014903184014) (15,13,0.5409014903184014) (15,15,0.5409014903184014) - (16,0,0.45507632937870307) (16,4,-0.5636364384637816) (16,5,-0.6773620756979517) - (16,6,-0.6449745956113119) (16,16,0.5409014903184014) (19,0,0.45507632937870307) - (19,4,-0.5636364384637816) (19,5,-0.6773620756979517) (19,6,-0.6449745956113119) - (19,16,0.5409014903184014) (19,19,0.5409014903184014) (21,0,0.45507632937870307) - (21,4,-0.5636364384637816) (21,5,-0.6773620756979517) (21,6,-0.6449745956113119) - (21,16,0.5409014903184014) (21,19,0.5409014903184014) (21,21,0.5409014903184014) - (22,0,0.46175428328363605) (22,7,-0.5636364384637816) (22,8,-0.6773620756979517) - (22,9,-0.6449745956113119) (22,22,0.5409014903184014) (25,0,0.46175428328363605) - (25,7,-0.5636364384637816) (25,8,-0.6773620756979517) (25,9,-0.6449745956113119) - (25,22,0.5409014903184014) (25,25,0.5409014903184014) (27,0,0.46175428328363605) - (27,7,-0.5636364384637816) (27,8,-0.6773620756979517) (27,9,-0.6449745956113119) - (27,22,0.5409014903184014) (27,25,0.5409014903184014) (27,27,0.5409014903184014) - M1 SYMMAT(28) (0,0,1) - M2 SYMMAT(28) (1,1,1) (2,2,1) (3,3,1) (10,0,-0.5) (13,0,-0.5) (15,0,-0.5) - M3 SYMMAT(28) (4,4,1) (5,5,1) (6,6,1) (16,0,-0.5) (19,0,-0.5) (21,0,-0.5) - M4 SYMMAT(28) (7,7,1) (8,8,1) (9,9,1) (22,0,-0.5) (25,0,-0.5) (27,0,-0.5) - M5 SYMMAT(28) (4,4,0.999999999999995) (5,5,-1) (16,0,-0.5000000000000066) (19,0,0.5000000000000049) - M6 SYMMAT(28) (1,1,-1) (3,3,0.9999999999999976) (10,0,0.5000000000000001) (15,0,-0.4999999999999977) - M7 SYMMAT(28) (7,7,0.9999999999999977) (8,8,-1) (22,0,-0.500000000000002) (25,0,0.49999999999999545) - M8 SYMMAT(28) (7,7,1.0000000000000016) (9,9,-1) (22,0,-0.499999999999997) (27,0,0.5) - M9 SYMMAT(28) (4,4,0.999999999999996) (6,6,-1) (16,0,-0.5) (21,0,0.49999999999999495) - M10 SYMMAT(28) (2,2,-1) (3,3,1.0000000000000009) (13,0,0.4999999999999978) (15,0,-0.4999999999999954) - M11 SYMMAT(28) (9,7,-0.7071067811865475) (24,0,0.7071067811865505) - M12 SYMMAT(28) (13,3,-0.7071067811865475) (14,2,0.7071067811865478) - M13 SYMMAT(28) (22,9,0.7071067811865459) (24,7,-0.7071067811865475) - M14 SYMMAT(28) (12,12,-1) (15,10,0.49999999999999767) - M15 SYMMAT(28) (17,5,0.7071067811865512) (19,4,-0.7071067811865475) - M16 SYMMAT(28) (17,17,-1) (19,16,0.5000000000000009) - M17 SYMMAT(28) (18,18,-1) (21,16,0.5000000000000004) - M18 SYMMAT(28) (18,6,0.7071067811865466) (21,4,-0.7071067811865475) - M19 SYMMAT(28) (23,8,-0.7071067811865475) (25,7,0.7071067811865481) - M20 SYMMAT(28) (14,14,-1) (15,13,0.4999999999999992) - M21 SYMMAT(28) (16,5,0.7071067811865467) (17,4,-0.7071067811865475) - M22 SYMMAT(28) (10,3,-0.7071067811865475) (12,1,0.7071067811865465) - M23 SYMMAT(28) (26,9,-0.7071067811865475) (27,8,0.7071067811865482) - M24 SYMMAT(28) (11,2,-0.7071067811865475) (13,1,0.7071067811865501) - M25 SYMMAT(28) (25,9,0.7071067811865468) (26,8,-0.7071067811865475) - M26 SYMMAT(28) (10,2,0.7071067811865477) (11,1,-0.7071067811865475) - M27 SYMMAT(28) (5,4,0.7071067811865475) (17,0,-0.7071067811865475) - M28 SYMMAT(28) (6,5,-0.7071067811865475) (20,0,0.707106781186549) - M29 SYMMAT(28) (19,6,0.7071067811865471) (20,5,-0.7071067811865475) - M30 SYMMAT(28) (20,20,-1) (21,19,0.49999999999999956) - M31 SYMMAT(28) (24,9,-0.7071067811865475) (27,7,0.7071067811865468) - M32 SYMMAT(28) (22,8,-0.7071067811865475) (23,7,0.7071067811865468) - M33 SYMMAT(28) (23,23,-1) (25,22,0.4999999999999994) - M34 SYMMAT(28) (26,26,-1) (27,25,0.5000000000000006) - M35 SYMMAT(28) (20,6,-0.7071067811865475) (21,5,0.7071067811865459) - M36 SYMMAT(28) (11,11,-1) (13,10,0.5000000000000001) - M37 SYMMAT(28) (16,6,-0.7071067811865475) (18,4,0.7071067811865467) - M38 SYMMAT(28) (17,6,0.7071067811865481) (20,4,-0.7071067811865475) - M39 SYMMAT(28) (14,3,0.7071067811865471) (15,2,-0.7071067811865475) - M40 SYMMAT(28) (12,2,-0.7071067811865475) (14,1,0.7071067811865465) - M41 SYMMAT(28) (18,17,-0.7071067811865475) (20,16,0.7071067811865475) - M42 SYMMAT(28) (12,3,-0.7071067811865475) (15,1,0.7071067811865497) - M43 SYMMAT(28) (24,8,0.7071067811865454) (26,7,-0.7071067811865475) - M44 SYMMAT(28) (24,23,0.7071067811865481) (26,22,-0.7071067811865475) - M45 SYMMAT(28) (9,8,0.707106781186547) (26,0,-0.7071067811865475) - M46 SYMMAT(28) (20,18,0.7071067811865475) (21,17,-0.7071067811865475) - M47 SYMMAT(28) (25,24,-0.7071067811865475) (26,23,0.7071067811865475) - M48 SYMMAT(28) (14,12,-0.7071067811865475) (15,11,0.7071067811865477) - M49 SYMMAT(28) (19,18,0.7071067811865477) (20,17,-0.7071067811865475) - M50 SYMMAT(28) (24,24,-1) (27,22,0.49999999999999906) - M51 SYMMAT(28) (26,24,0.7071067811865468) (27,23,-0.7071067811865475) - M52 SYMMAT(28) (12,11,-0.7071067811865475) (14,10,0.7071067811865472) - M53 SYMMAT(28) (13,12,-0.7071067811865475) (14,11,0.7071067811865481) - M54 SYMMAT(28) (23,9,-0.7071067811865475) (24,8,0.7071067811865479) - M55 SYMMAT(28) (11,3,-0.7071067811865475) (14,1,0.7071067811865472) - M56 SYMMAT(28) (17,6,0.7071067811865482) (18,5,-0.7071067811865475) - M57 SYMMAT(28) (6,4,-0.7071067811865475) (18,0,0.7071067811865479) - M58 SYMMAT(28) (3,2,-0.7071067811865475) (14,0,0.7071067811865469) - M59 SYMMAT(28) (3,1,0.7071067811865471) (12,0,-0.7071067811865475) + M0 SYMMAT(4) (1,0,-0.5005176343192309) (1,1,0.5116630113402288) (2,0,-0.9323560236297814) (2,1,0.5554848697946364) (2,2,1) + (3,0,0.19328062085948425) (3,1,-0.19049507566361382) (3,2,-0.21428571428571427) (3,3,0.07142857142857142) + M1 SYMMAT(4) (0,0,1) + M2 SYMMAT(4) (1,1,1) (2,2,1) (3,0,-0.5) diff --git a/starloc b/starloc index a342581..c555a79 160000 --- a/starloc +++ b/starloc @@ -1 +1 @@ -Subproject commit a342581cdeea01b05db3778af017340ff586a113 +Subproject commit c555a79b7ff2afa7fa14149b9003e0796f36f0af From 1124aac0a18d86314e1877dc868767dfd5147f69 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 31 Oct 2023 10:44:39 -0400 Subject: [PATCH 04/13] Add more examples --- .gitignore | 2 ++ certifiable-tools | 2 +- examples/ro_lifter.py | 64 +++++++++++++++++++++++++++++-------------- solve_mosek.ptf | 23 ---------------- 4 files changed, 47 insertions(+), 44 deletions(-) delete mode 100644 solve_mosek.ptf diff --git a/.gitignore b/.gitignore index 0a7508a..e5fa06c 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,5 @@ venv/ *.swp *.code-workspace +solve_mosek.ptf +solve_cvxpy*.ptf diff --git a/certifiable-tools b/certifiable-tools index b1bc105..e722c6b 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit b1bc105e174bafe8b8ba5398f3fafc3039dc1c56 +Subproject commit e722c6bc3502561478d371b60af16cb66a4a7fff diff --git a/examples/ro_lifter.py b/examples/ro_lifter.py index cd82bdb..54d7467 100644 --- a/examples/ro_lifter.py +++ b/examples/ro_lifter.py @@ -5,16 +5,20 @@ from utils.plotting_tools import savefig -def get_problem(t_init=[0, 0], centered=False, level="no"): +def get_problem(t_init, centered=False, level="no"): # Create lifter np.random.seed(0) n_landmarks = 4 d = 2 - lifter = Lifter(n_positions=1, n_landmarks=n_landmarks, d=d, level=level) + lifter = Lifter( + n_positions=t_init.shape[0], n_landmarks=n_landmarks, d=d, level=level + ) lifter.landmarks = np.c_[ np.ones(n_landmarks) + np.random.rand(4) * 0.5, np.arange(n_landmarks) ] - lifter.theta = np.array([0.3, 1.0]) + lifter.theta = np.hstack( + [np.full(n_positions, 0.3)[:, None], np.linspace(1, 3, n_positions)[:, None]] + ).flatten() Q, y = lifter.get_Q() @@ -39,20 +43,28 @@ def get_problem(t_init=[0, 0], centered=False, level="no"): def plot_problem(prob, lifter, fname=""): Q = prob["Q"] - xx, yy = np.meshgrid(np.arange(-1, 4, 0.1), np.arange(-1, 4, 0.1)) - zz = np.empty(xx.shape) - for i in range(xx.shape[0]): - for j in range(yy.shape[1]): - x = lifter.get_x(theta=np.array([xx[i, j], yy[i, j]])) - zz[i, j] = x.T @ Q @ x - - fig, ax = plt.subplots() - ax.pcolormesh(xx, yy, np.log10(zz)) - ax.scatter( - lifter.landmarks[:, 0], lifter.landmarks[:, 1], color="white", marker="o" + theta_hat = prob["x_cand"][1 : 1 + lifter.theta.size].reshape( + lifter.n_positions, -1 ) - ax.scatter(lifter.theta[0], lifter.theta[1], color="white", marker="*") - ax.scatter(prob["x_cand"][1], prob["x_cand"][2], color="green", marker="*") + theta_gt = lifter.theta.reshape(lifter.n_positions, -1) + n_plots = min(3, lifter.n_positions) + xx, yy = np.meshgrid(np.arange(-1, 4, 0.1), np.arange(-1, 4, 0.1)) + fig, ax = plt.subplots(1, n_plots, squeeze=False) + for n in range(n_plots): + zz = np.empty(xx.shape) + for i in range(xx.shape[0]): + for j in range(yy.shape[1]): + theta = theta_gt.copy() + theta[n, :] = xx[i, j], yy[i, j] + x = lifter.get_x(theta=theta) + zz[i, j] = x.T @ Q @ x + + ax[0, n].pcolormesh(xx, yy, np.log10(zz)) + ax[0, n].scatter( + lifter.landmarks[:, 0], lifter.landmarks[:, 1], color="white", marker="o" + ) + ax[0, n].scatter(theta_gt[n, 0], theta_gt[n, 1], color="white", marker="*") + ax[0, n].scatter(theta_hat[n, 0], theta_hat[n, 1], color="green", marker="*") plt.show() if fname != "": savefig(fig, fname) @@ -61,24 +73,36 @@ def plot_problem(prob, lifter, fname=""): if __name__ == "__main__": from problem_utils import save_test_problem - number = 10 - level = "no" + # number = 10 + # level = "no" # number = 11 # level = "quad" + # n_positions = 3 + # number = 12 + # level = "quad" + + n_positions = 10 + number = 13 + level = "quad" + for centered in [False, True]: appendix = "c" if centered else "" prob, lifter = get_problem( - t_init=np.array([0, 0]), centered=centered, level=level + t_init=np.zeros((n_positions, 2)), centered=centered, level=level ) fname = f"certifiable-tools/_test/test_prob_{number}G{appendix}.pkl" save_test_problem(**prob, fname=fname) plot_problem(prob, lifter, fname=fname.replace(".pkl", ".png")) prob, lifter = get_problem( - t_init=np.array([2, 0]), centered=centered, level=level + t_init=np.hstack( + [np.full(n_positions, 2)[:, None], np.zeros((n_positions, 1))] + ), + centered=centered, + level=level, ) fname = f"certifiable-tools/_test/test_prob_{number}L{appendix}.pkl" save_test_problem(**prob, fname=fname) diff --git a/solve_mosek.ptf b/solve_mosek.ptf deleted file mode 100644 index 8116623..0000000 --- a/solve_mosek.ptf +++ /dev/null @@ -1,23 +0,0 @@ -Task - # Written by MOSEK v10.1.15 - # problemtype: Conic Problem - # number of linear variables: 0 - # number of linear constraints: 2 - # number of old-style cones: 0 - # number of positive semidefinite variables: 1 - # number of positive semidefinite matrixes: 3 - # number of affine conic constraints: 0 - # number of disjunctive constraints: 0 - # number of old-style A nonzeros: 0 -Objective - Minimize + -Constraints - @c0 [1] + - @c1 [0] + -Variables - @X0 [PSD(4)] -SymmetricMatrixes - M0 SYMMAT(4) (1,0,-0.5005176343192309) (1,1,0.5116630113402288) (2,0,-0.9323560236297814) (2,1,0.5554848697946364) (2,2,1) - (3,0,0.19328062085948425) (3,1,-0.19049507566361382) (3,2,-0.21428571428571427) (3,3,0.07142857142857142) - M1 SYMMAT(4) (0,0,1) - M2 SYMMAT(4) (1,1,1) (2,2,1) (3,0,-0.5) From 98b857f256de0ffa6497deee0ed5cbcfe4b6a0ad Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 7 Nov 2023 14:43:06 -0500 Subject: [PATCH 05/13] Create robust Wahba example problems, revert SDP solver --- certifiable-tools | 2 +- examples/robust_lifter.py | 74 ++++++++++++++++ solvers/common.py | 178 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 253 insertions(+), 1 deletion(-) create mode 100644 examples/robust_lifter.py diff --git a/certifiable-tools b/certifiable-tools index e722c6b..0b36933 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit e722c6bc3502561478d371b60af16cb66a4a7fff +Subproject commit 0b369330a1d7a043609ed20ec0d772d9108969ce diff --git a/examples/robust_lifter.py b/examples/robust_lifter.py new file mode 100644 index 0000000..046a51f --- /dev/null +++ b/examples/robust_lifter.py @@ -0,0 +1,74 @@ +import numpy as np +import matplotlib.pylab as plt + +from lifters.wahba_lifter import WahbaLifter as Lifter +from utils.plotting_tools import savefig + + +def get_problem(robust=True): + if robust: + level = "xwT" + else: + level = "no" + # Create lifter + np.random.seed(0) + n_landmarks = 4 + d= 3 + if robust: + n_outliers = 1 + else: + n_outliers = 0 + + lifter = Lifter(d=d, n_landmarks=n_landmarks+n_outliers, robust=robust, n_outliers=n_outliers, level=level, variable_list=None) + Q, y = lifter.get_Q() + + from auto_template.learner import Learner + + learner = Learner(lifter=lifter, variable_list=lifter.variable_list, n_inits=1) + learner.run() + Constraints = learner.get_A_b_list() + + t0 = lifter.get_vec_around_gt(delta=0) # initialize at gt + t, *_ = lifter.local_solver(t0, y=y) + x_hat = lifter.get_x(theta=t) + return dict(Q=Q, Constraints=Constraints, x_cand=x_hat), lifter + + +def plot_problem(prob, lifter, fname=""): + Q = prob["Q"] + + theta_hat = prob["x_cand"][1 : 1 + lifter.theta.size].reshape( + lifter.n_positions, -1 + ) + theta_gt = lifter.theta.reshape(lifter.n_positions, -1) + n_plots = min(3, lifter.n_positions) + xx, yy = np.meshgrid(np.arange(-1, 4, 0.1), np.arange(-1, 4, 0.1)) + fig, ax = plt.subplots(1, n_plots, squeeze=False) + for n in range(n_plots): + zz = np.empty(xx.shape) + for i in range(xx.shape[0]): + for j in range(yy.shape[1]): + theta = theta_gt.copy() + theta[n, :] = xx[i, j], yy[i, j] + x = lifter.get_x(theta=theta) + zz[i, j] = x.T @ Q @ x + + ax[0, n].pcolormesh(xx, yy, np.log10(zz)) + ax[0, n].scatter( + lifter.landmarks[:, 0], lifter.landmarks[:, 1], color="white", marker="o" + ) + ax[0, n].scatter(theta_gt[n, 0], theta_gt[n, 1], color="white", marker="*") + ax[0, n].scatter(theta_hat[n, 0], theta_hat[n, 1], color="green", marker="*") + plt.show() + if fname != "": + savefig(fig, fname) + + +if __name__ == "__main__": + from problem_utils import save_test_problem + + for robust, number in zip([False, True], [14, 15]): + prob, lifter = get_problem(robust=robust) + fname = f"certifiable-tools/_examples/test_prob_{number}G.pkl" + save_test_problem(**prob, fname=fname) + #plot_problem(prob, lifter, fname=fname.replace(".pkl", ".png")) \ No newline at end of file diff --git a/solvers/common.py b/solvers/common.py index 1feef0c..301ce51 100644 --- a/solvers/common.py +++ b/solvers/common.py @@ -29,6 +29,35 @@ } +def adjust_Q(Q, offset=True, scale=True, plot=False): + from copy import deepcopy + + ii, jj = (Q == Q.max()).nonzero() + if (ii[0], jj[0]) != (0, 0) or (len(ii) > 1): + pass + # print("Warning: largest element of Q is not unique or not in top-left. Check ordering?") + + Q_mat = deepcopy(Q) + if offset: + Q_offset = Q_mat[0, 0] + else: + Q_offset = 0 + Q_mat[0, 0] -= Q_offset + + if scale: + # Q_scale = spl.norm(Q_mat, "fro") + Q_scale = Q_mat.max() + else: + Q_scale = 1.0 + Q_mat /= Q_scale + if plot: + fig, axs = plt.subplots(1, 2) + axs[0].matshow(np.log10(np.abs(Q.toarray()))) + axs[1].matshow(np.log10(np.abs(Q_mat.toarray()))) + plt.show() + return Q_mat, Q_scale, Q_offset + + def adjust_tol(options, tol): options["mosek_params"].update( { @@ -48,6 +77,155 @@ def solve_sdp_cvxpy( verbose=True, tol=None, ): + """Run CVXPY to solve a semidefinite program. + + Args: + Q (_type_): Cost matrix + A_b_list (_type_): constraint tuple, (A,b) such that trace(A @ X) == b + adjust (bool, optional): Choose to normalize Q matrix. + primal (bool, optional): Choose to solve primal or dual. Defaults to False (dual). + verbose (bool, optional): Print verbose ouptut. Defaults to True. + + Returns: + _type_: (X, cost_out): solution matrix and output cost. + """ + opts = solver_options + opts["verbose"] = verbose + + if tol: + adjust_tol(opts, tol) + + Q_here, scale, offset = adjust_Q(Q) if adjust else (Q, 1.0, 0.0) + if primal: + """ + min < Q, X > + s.t. trace(Ai @ X) == bi, for all i. + """ + X = cp.Variable(Q.shape, symmetric=True) + constraints = [X >> 0] + constraints += [cp.trace(A @ X) == b for A, b in A_b_list] + constraints += [cp.trace(B @ X) <= 0 for B in B_list] + cprob = cp.Problem(cp.Minimize(cp.trace(Q_here @ X)), constraints) + try: + cprob.solve( + solver="MOSEK", + **opts, + ) + except Exception as e: + print(e) + cost = None + X = None + H = None + yvals = None + msg = "infeasible / unknown" + else: + if np.isfinite(cprob.value): + cost = cprob.value + X = X.value + H = constraints[0].dual_value + yvals = [c.dual_value for c in constraints[1:]] + msg = "converged" + else: + cost = None + X = None + H = None + yvals = None + msg = "unbounded" + else: # Dual + """ + max < y, b > + s.t. sum(Ai * yi for all i) << Q + """ + m = len(A_b_list) + y = cp.Variable(shape=(m,)) + + k = len(B_list) + if k > 0: + u = cp.Variable(shape=(k,)) + + As, b = zip(*A_b_list) + b = np.concatenate([np.atleast_1d(bi) for bi in b]) + objective = cp.Maximize(b @ y) + + # We want the lagrangian to be H := Q - sum l_i * A_i + sum u_i * B_i. + # With this choice, l_0 will be negative + LHS = cp.sum( + [y[i] * Ai for (i, Ai) in enumerate(As)] + + [-u[i] * Bi for (i, Bi) in enumerate(B_list)] + ) + # this does not include symmetry of Q!! + constraints = [LHS << Q_here] + constraints += [LHS == LHS.T] + if k > 0: + constraints.append(u >= 0) + + cprob = cp.Problem(objective, constraints) + try: + cprob.solve( + solver="MOSEK", + **opts, + ) + except Exception as e: + print(e) + cost = None + X = None + H = None + yvals = None + msg = "infeasible / unknown" + else: + if np.isfinite(cprob.value): + cost = cprob.value + X = constraints[0].dual_value + H = Q_here - LHS.value + yvals = [x.value for x in y] + + # sanity check for inequality constraints. + # we want them to be inactive!!! + if len(B_list): + mu = np.array([ui.value for ui in u]) + i_nnz = np.where(mu > 1e-10)[0] + if len(i_nnz): + for i in i_nnz: + print( + f"Warning: is constraint {i} active? (mu={mu[i]:.4e}):" + ) + print(np.trace(B_list[i] @ X)) + msg = "converged" + else: + cost = None + X = None + H = None + yvals = None + msg = "unbounded" + + # reverse Q adjustment + if cost: + cost = cost * scale + offset + + H = Q_here - cp.sum( + [yvals[i] * Ai for (i, Ai) in enumerate(As)] + + [-u[i] * Bi for (i, Bi) in enumerate(B_list)] + ) + yvals[0] = yvals[0] * scale + offset + # H *= scale + # H[0, 0] += offset + + info = {"H": H, "yvals": yvals, "cost": cost, "msg": msg} + return X, info + + + +def solve_sdp_cvxpy_new( + Q, + A_b_list, + B_list=[], + adjust=True, + primal=False, + verbose=True, + tol=None, +): + # TODO: below doesn't currently give the same results as when running the above function. + # Need to figure out why and fix this. assert primal is False, "Option primal not supported anymore, it is less efficient." assert len(B_list) == 0, "Inequality constraints not supported anymore." From 63f469f9c554b072b6fc20e65e0b4f6dc9c384fa Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 15 Nov 2023 18:50:20 -0500 Subject: [PATCH 06/13] Generate toy examples and lynting --- auto_template/learner.py | 22 +++++++++------ certifiable-tools | 2 +- examples/ro_lifter.py | 11 ++++---- examples/toy_example.py | 59 ++++++++++++++++++++++++++++++++++++++++ lifters/poly_lifters.py | 47 ++++++++++++++++++++++---------- lifters/state_lifter.py | 22 +++++++-------- lifters/stereo_lifter.py | 2 +- setup.cfg | 3 +- utils/plotting_tools.py | 51 ++++++++++++++++++++++++---------- 9 files changed, 162 insertions(+), 57 deletions(-) create mode 100644 examples/toy_example.py diff --git a/auto_template/learner.py b/auto_template/learner.py index e52726d..d5ac8fd 100644 --- a/auto_template/learner.py +++ b/auto_template/learner.py @@ -38,6 +38,8 @@ USE_KNOWN = True GLOBAL_THRESH = 1e-3 +METHOD_NULL = "qrp" # use svd or qp for comparison only, otherwise leave it at qrp + class Learner(object): """ @@ -51,7 +53,9 @@ def __init__( apply_templates: bool = True, noise: float = None, n_inits: int = 10, + use_known: bool = USE_KNOWN, ): + self.use_known = use_known self.noise = noise self.lifter = lifter self.variable_iter = iter(variable_list) @@ -231,7 +235,7 @@ def is_tight(self, verbose=False, data_dict={}, tightness=None): def get_A_b_list(self): A_known = [] - if USE_KNOWN: + if self.use_known: # A_known = self.lifter.get_A_known() A_known += [constraint.A_sparse_ for constraint in self.templates_known] A_list = A_known + [constraint.A_sparse_ for constraint in self.constraints] @@ -290,7 +294,7 @@ def function(A_b_list_here, df_data): B_list = self.lifter.get_B_known() force_first = 1 - if USE_KNOWN: + if self.use_known: force_first += len(self.templates_known) if reorder: @@ -359,9 +363,9 @@ def function(A_b_list_here, df_data): minimal_indices = [] if tightness == "cost": - min_num = df_tight[df_tight.cost_tight == True].index.min() + min_num = df_tight[df_tight.cost_tight is True].index.min() elif tightness == "rank": - min_num = df_tight[df_tight.rank_tight == True].index.min() + min_num = df_tight[df_tight.rank_tight is True].index.min() if not np.isnan(min_num): minimal_indices = list(sorted_idx[:min_num]) return minimal_indices @@ -381,7 +385,7 @@ def find_local_solution(self, verbose=False, plot=False): self.solver_vars = dict(Q=Q, y=y, qcqp_cost=qcqp_cost, xhat=None) def find_global_solution(self, data_dict={}): - if self.tightness_dict["rank"] == True: + if self.tightness_dict["rank"] is True: X = self.solver_vars["X"] x, info = rank_project(X, p=1) print("rank projection info:", info) @@ -472,7 +476,7 @@ def learn_templates(self, plot=False, data_dict=None): t1 = time.time() Y = self.lifter.generate_Y(var_subset=self.mat_vars, factor=FACTOR) - if USE_KNOWN: + if self.use_known: a_vectors = [] for c in self.templates: ai = self.lifter.get_vec(c.A_poly_.get_matrix(self.mat_var_dict)) @@ -490,7 +494,7 @@ def learn_templates(self, plot=False, data_dict=None): print(f"data matrix Y has shape {Y.shape} ") for i in range(self.lifter.N_CLEANING_STEPS + 1): print(f"cleaning step {i+1}/{self.lifter.N_CLEANING_STEPS+1}...", end="") - basis_new, S = self.lifter.get_basis(Y) + basis_new, S = self.lifter.get_basis(Y, method=METHOD_NULL) print(f"...done") corank = basis_new.shape[0] if corank > 0: @@ -702,7 +706,7 @@ def run(self, verbose=False, plot=False): data = [] success = False - if USE_KNOWN: + if self.use_known: self.create_known_templates() while 1: @@ -713,7 +717,7 @@ def run(self, verbose=False, plot=False): print(f"======== {self.mat_vars} ========") n_new = 0 - if USE_KNOWN: + if self.use_known: n_new += self.extract_known_templates() data_dict = {"variables": self.mat_vars} diff --git a/certifiable-tools b/certifiable-tools index e722c6b..3b66dcf 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit e722c6bc3502561478d371b60af16cb66a4a7fff +Subproject commit 3b66dcf04b57c70f02026fc4810a594e8db831fd diff --git a/examples/ro_lifter.py b/examples/ro_lifter.py index 54d7467..317b75e 100644 --- a/examples/ro_lifter.py +++ b/examples/ro_lifter.py @@ -76,16 +76,17 @@ def plot_problem(prob, lifter, fname=""): # number = 10 # level = "no" - # number = 11 - # level = "quad" + n_positions = 1 + number = 11 + level = "quad" # n_positions = 3 # number = 12 # level = "quad" - n_positions = 10 - number = 13 - level = "quad" + # n_positions = 10 + # number = 13 + # level = "quad" for centered in [False, True]: appendix = "c" if centered else "" diff --git a/examples/toy_example.py b/examples/toy_example.py new file mode 100644 index 0000000..ec4c523 --- /dev/null +++ b/examples/toy_example.py @@ -0,0 +1,59 @@ +from auto_template.learner import Learner +from lifters.poly_lifters import Poly6Lifter +from utils.plotting_tools import plot_matrix, import_plt, savefig + +import numpy as np + +plt = import_plt() + + +def plot_poly_examples(): + lifter = Poly6Lifter() + + learner = Learner(lifter, variable_list=lifter.VARIABLE_LIST, use_known=False) + learner.run(plot=True) + + savefig(plt.gcf(), "_plots/toy_example_svd.png") + + A_learned = learner.get_A_b_list() + A_list = [lifter.get_A0()] + lifter.get_A_known() + + basis = np.empty((lifter.get_dim_X(), len(A_list))) + fig1, axs1 = plt.subplots(1, len(A_list)) + for i, A_i in enumerate(A_list): + plot_matrix(A_i, ax=axs1[i], colorbar=False) + if i == 0: + axs1[i].set_title(f"$A_{i}$") + elif i < len(A_list) - 1: + axs1[i].set_title(f"$A_{i}$ (known)") + else: + axs1[-1].set_title(f"$B_{0}$ (known)") + basis[:, i] = lifter.get_vec(A_i) + fig1b, *_ = plot_matrix(basis, colorbar=True, discrete=True) + + basis = np.empty((lifter.get_dim_X(), len(A_list))) + fig2, axs2 = plt.subplots(1, len(A_learned)) + for i, (A_i, b) in enumerate(A_learned): + plot_matrix(A_i.toarray(), ax=axs2[i], colorbar=False) + if i == 0: + axs2[i].set_title(f"$A_{i}$") + else: + axs2[i].set_title(f"$A_{i}$ (learned)") + basis[:, i] = lifter.get_vec(A_i) + fig2b, *_ = plot_matrix(basis, colorbar=True, discrete=True) + + savefig(fig1, "_plots/toy_example_1.png") + savefig(fig2, "_plots/toy_example_2.png") + savefig(fig1b, "_plots/toy_example_1b.png") + savefig(fig2b, "_plots/toy_example_2b.png") + + plt.show() + print("done") + +def plot_all_examples(): + + + +if __name__ == "__main__": + # plot_poly_examples() + plot_all_examples() diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index c56cb3b..6ae7089 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -9,30 +9,39 @@ class PolyLifter(StateLifter): def __init__(self, degree): self.degree = degree super().__init__(d=1) + self.parameters = [1.0] + + # TODO (FD): remove requirement for this variable + self.n_landmarks = 0 @property def var_dict(self): if self.var_dict_ is None: self.var_dict_ = {"h": 1, "t": 1} - self.var_dict_.update({f"z_{i}": 1 for i in range(self.M)}) + self.var_dict_.update({f"z{i}": 1 for i in range(self.M)}) return self.var_dict_ @property def M(self): return self.degree // 2 - 1 + def sample_theta(self): + return np.random.rand(1) + @property def theta(self): + if self.theta_ is None: + self.theta_ = self.sample_theta() return self.theta_ @abstractmethod def get_Q_mat(self): return - def sample_feasible(self): - return np.random.rand(1) + def get_error(self, t): + return {"MAE": float(abs(self.theta - t))} - def get_x(self, t=None): + def get_x(self, t=None, parameters=None, var_subset=None): if t is None: t = self.theta return np.array([t**i for i in range(self.degree // 2 + 1)]) @@ -58,6 +67,8 @@ def __repr__(self): class Poly4Lifter(PolyLifter): + VARIABLE_LIST = [["h", "t", "z0"]] + def __init__(self): # actual minimum super().__init__(degree=4) @@ -66,20 +77,25 @@ def get_Q_mat(self): Q = np.r_[np.c_[2, 1, 0], np.c_[1, -1 / 2, -1 / 3], np.c_[0, -1 / 3, 1 / 4]] return Q - def get_A_known(self): + def get_A_known(self, target_dict=None, output_poly=False): from poly_matrix import PolyMatrix # z_0 = t^2 A_1 = PolyMatrix(symmetric=True) - A_1["h", "z_0"] = -1 + A_1["h", "z0"] = -1 A_1["t", "t"] = 2 - return A_1.get_matrix(self.var_dict) + if output_poly: + return [A_1] + else: + return A_1.get_matrix(self.var_dict) def generate_random_setup(self): self.theta_ = np.array([-1]) class Poly6Lifter(PolyLifter): + VARIABLE_LIST = [["h", "t", "z0", "z1"]] + def __init__(self, poly_type="A"): assert poly_type in ["A", "B"] self.poly_type = poly_type @@ -105,24 +121,27 @@ def get_Q_mat(self): ] ) - def get_A_known(self): + def get_A_known(self, target_dict=None, output_poly=False): from poly_matrix import PolyMatrix # z_0 = t^2 A_1 = PolyMatrix(symmetric=True) - A_1["h", "z_0"] = -1 + A_1["h", "z0"] = -1 A_1["t", "t"] = 2 # z_1 = t^3 = t z_0 A_2 = PolyMatrix(symmetric=True) - A_2["h", "z_1"] = -1 - A_2["t", "z_0"] = 1 + A_2["h", "z1"] = -1 + A_2["t", "z0"] = 1 # t^4 = z_1 t = z_0 z_0 B_0 = PolyMatrix(symmetric=True) - B_0["z_0", "z_0"] = 2 - B_0["t", "z_1"] = -1 - return [A_i.get_matrix(self.var_dict) for A_i in [A_1, A_2, B_0]] + B_0["z0", "z0"] = 2 + B_0["t", "z1"] = -1 + if output_poly: + return [A_1, A_2, B_0] + else: + return [A_i.get_matrix(self.var_dict) for A_i in [A_1, A_2, B_0]] def get_D(self, that): # TODO(FD) generalize and move to PolyLifter diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 0162e71..4c7bdb9 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -96,7 +96,7 @@ def get_dim_x(len_vec): triu_i_nnz = triu[0][mask] triu_j_nnz = triu[1][mask] vec_nnz = vec[mask] - except: + except Exception: # vec is sparse len_vec = vec.shape[1] dim_x = get_dim_x(len_vec) @@ -146,7 +146,7 @@ def test_S_cutoff(self, S, corank): try: assert abs(S[-corank]) / self.EPS_SVD < 1e-1 # 1e-1 1e-10 assert abs(S[-corank - 1]) / self.EPS_SVD > 10 # 1e-11 1e-10 - except: + except AssertionError: print( f"there might be a problem with the chosen threshold {self.EPS_SVD}:" ) @@ -355,7 +355,7 @@ def get_labels(self, p, zi, zj): def var_list_row(self, var_subset=None, force_parameters_off=False): if var_subset is None: var_subset = list(self.var_dict.keys()) - elif type(var_subset) == dict: + elif type(var_subset) is dict: var_subset = list(var_subset.keys()) label_list = [] @@ -387,8 +387,8 @@ def var_list_row(self, var_subset=None, force_parameters_off=False): def var_dict_row(self, var_subset=None, force_parameters_off=False): return { - l: 1 - for l in self.var_list_row( + label: 1 + for label in self.var_list_row( var_subset, force_parameters_off=force_parameters_off ) } @@ -396,7 +396,7 @@ def var_dict_row(self, var_subset=None, force_parameters_off=False): def get_basis_from_poly_rows(self, basis_poly_list, var_subset=None): var_dict = self.get_var_dict(var_subset=var_subset) - all_dict = {l: 1 for l in self.var_list_row(var_subset)} + all_dict = {label: 1 for label in self.var_list_row(var_subset)} basis_reduced = np.empty((0, len(all_dict))) for i, bi_poly in enumerate(basis_poly_list): # test that this constraint holds @@ -450,7 +450,7 @@ def get_vector_dense(self, poly_row_sub): vector = np.r_[vector, mat[np.triu_indices(mat.shape[0])]] return np.array(vector) - def convert_polyrow_to_a(self, poly_row, var_subset, correct=True): + def old_convert_polyrow_to_a(self, poly_row, var_subset, correct=True): """Convert poly-row to reduced a. poly-row has elements with keys "pk:l-xi:m.xj:n", @@ -552,7 +552,7 @@ def convert_a_to_polyrow( try: dim_a = len(a) - except: + except Exception: dim_a = a.shape[1] assert dim_a == dim_X @@ -569,9 +569,9 @@ def convert_a_to_polyrow( # TODO: use get_vec instead? vals = val[np.triu_indices(val.shape[0])] assert len(labels) == len(vals) - for l, v in zip(labels, vals): + for label, v in zip(labels, vals): if np.any(np.abs(v) > self.EPS_SPARSE): - poly_row["h", l] = v + poly_row["h", label] = v return poly_row def convert_b_to_polyrow(self, b, var_subset, tol=1e-10) -> PolyMatrix: @@ -819,7 +819,7 @@ def generate_matrices( """ try: n_basis = len(basis) - except: + except Exception: n_basis = basis.shape[0] if type(var_dict) is list: diff --git a/lifters/stereo_lifter.py b/lifters/stereo_lifter.py index 259a25c..d6f9ee2 100644 --- a/lifters/stereo_lifter.py +++ b/lifters/stereo_lifter.py @@ -1,4 +1,4 @@ -from abc import ABC, abstractproperty +from abc import ABC import autograd.numpy as np diff --git a/setup.cfg b/setup.cfg index 132b23f..b49b6e2 100644 --- a/setup.cfg +++ b/setup.cfg @@ -19,5 +19,6 @@ packages = find: exclude=tests* [flake8] -ignore = W292, W391, F541, F841, +ignore = W292, W391, F541, F841, E203, E501 exclude = _notebooks/*, *.ipynb_checkpoints* +max-line-length = 99 diff --git a/utils/plotting_tools.py b/utils/plotting_tools.py index 86361c2..dab24f8 100644 --- a/utils/plotting_tools.py +++ b/utils/plotting_tools.py @@ -1,3 +1,8 @@ +import os +import numpy as np +from poly_matrix.poly_matrix import PolyMatrix + + def import_plt(): import matplotlib.pylab as plt import shutil @@ -14,12 +19,6 @@ def import_plt(): return plt -import numpy as np -from poly_matrix.poly_matrix import PolyMatrix - -import os -import numpy as np - plt = import_plt() @@ -222,7 +221,16 @@ def plot_tightness(df_tight, qcqp_cost, fname_root): def plot_matrix( - Ai, vmin=None, vmax=None, nticks=None, title="", colorbar=True, fig=None, ax=None + Ai, + vmin=None, + vmax=None, + nticks=None, + title="", + colorbar=True, + fig=None, + ax=None, + log=True, + discrete=False, ): import matplotlib @@ -231,17 +239,29 @@ def plot_matrix( if fig is None: fig = plt.gcf() - norm = matplotlib.colors.SymLogNorm(10**-5, vmin=vmin, vmax=vmax) - if type(Ai) == np.ndarray: - im = ax.matshow(Ai, norm=norm) + norm = None + if log: + norm = matplotlib.colors.SymLogNorm(10**-5, vmin=vmin, vmax=vmax) + + cmap = plt.get_cmap("viridis") + colorbar_yticks = None + if discrete: + values = np.unique(Ai[Ai != 0]) + nticks = None + cmap, norm, colorbar_yticks = initialize_discrete_cbar(values) + + if type(Ai) is np.ndarray: + im = ax.matshow(Ai, norm=norm, cmap=cmap) else: - im = ax.matshow(Ai.toarray(), norm=norm) + im = ax.matshow(Ai.toarray(), norm=norm, cmap=cmap) ax.axis("off") ax.set_title(title, y=1.0) if colorbar: - add_colorbar(fig, ax, im, nticks=nticks) + cax = add_colorbar(fig, ax, im, nticks=nticks) else: - add_colorbar(fig, ax, im, nticks=nticks, visible=False) + cax = add_colorbar(fig, ax, im, nticks=nticks, visible=False) + if colorbar_yticks is not None: + cax.set_yticklabels(colorbar_yticks) return fig, ax, im @@ -344,6 +364,7 @@ def plot_singular_values(S, eps=None, label="singular values", ax=None): ax.axhline(eps, color="C1") ax.grid() ax.set_xlabel("index") - ax.set_ylabel("magnitude of singular values") - ax.legend(loc="upper right") + ax.set_ylabel("abs. singular values") + if label is not None: + ax.legend(loc="upper right") return fig, ax From 2a71a36c6b65d53b584263e59cd4937125d36018 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 7 Dec 2023 17:19:43 -0500 Subject: [PATCH 07/13] Create quartic example --- certifiable-tools | 2 +- examples/poly4_lifter.py | 61 ++++++++++++++++++++++++++++++++++++++++ examples/poly6_lifter.py | 1 - examples/toy_example.py | 6 +--- lifters/poly_lifters.py | 13 ++++++++- 5 files changed, 75 insertions(+), 8 deletions(-) create mode 100644 examples/poly4_lifter.py diff --git a/certifiable-tools b/certifiable-tools index 0b36933..9506e70 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit 0b369330a1d7a043609ed20ec0d772d9108969ce +Subproject commit 9506e7017e307af45cd198f957a05534c2136fb9 diff --git a/examples/poly4_lifter.py b/examples/poly4_lifter.py new file mode 100644 index 0000000..4f764ef --- /dev/null +++ b/examples/poly4_lifter.py @@ -0,0 +1,61 @@ +import numpy as np + +import matplotlib.pylab as plt + +from lifters.poly_lifters import Poly4Lifter as Lifter +from utils.plotting_tools import savefig + + +def plot_problem(problem, t_lims=[-2, 5], fname=""): + lifter = Lifter() + Q = problem["Q"] + + ts = np.arange(*t_lims, 0.1) + ys = [lifter.get_x(t).T @ Q @ lifter.get_x(t) for t in ts] + + fig, ax = plt.subplots() + ax.plot(ts, ys) + t = problem["x_cand"][1] + ax.scatter([t], problem["x_cand"].T @ Q @ problem["x_cand"]) + ax.set_yscale("symlog") + if fname != "": + savefig(fig, fname) + + +def get_problem(centered=False, t_init=-1): + lifter = Lifter() + Q, __ = lifter.get_Q() + A_list = lifter.get_A_known() + Constraints = lifter.get_A_b_list(A_list) + + t_hat, info, cost = lifter.local_solver(t_init) + + if centered: + D = lifter.get_D(t_hat) + Q = D.T @ Q @ D + + x_hat = np.zeros(Q.shape[0]) + x_hat[0] = 1.0 + else: + x_hat = lifter.get_x(t_hat) + for A_i in A_list[1:]: + assert abs(x_hat.T @ A_i @ x_hat) <= 1e-10 + + return dict(Q=Q, Constraints=Constraints, x_cand=x_hat) + + +if __name__ == "__main__": + from problem_utils import save_test_problem + + number = 16 + for centered in [False, True]: + appendix = "c" if centered else "" + prob = get_problem(t_init=-5, centered=centered) + fname = f"certifiable-tools/_examples/test_prob_{number}G{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-4, 4], fname=fname.replace(".pkl", ".png")) + + prob = get_problem(t_init=2, centered=centered) + fname = f"certifiable-tools/_examples/test_prob_{number}L{appendix}.pkl" + save_test_problem(**prob, fname=fname) + plot_problem(prob, t_lims=[-4, 4], fname=fname.replace(".pkl", ".png")) diff --git a/examples/poly6_lifter.py b/examples/poly6_lifter.py index 1f690ae..9b6a112 100644 --- a/examples/poly6_lifter.py +++ b/examples/poly6_lifter.py @@ -1,5 +1,4 @@ import numpy as np -import scipy.sparse as sp import matplotlib.pylab as plt diff --git a/examples/toy_example.py b/examples/toy_example.py index ec4c523..b566f2a 100644 --- a/examples/toy_example.py +++ b/examples/toy_example.py @@ -50,10 +50,6 @@ def plot_poly_examples(): plt.show() print("done") -def plot_all_examples(): - - if __name__ == "__main__": - # plot_poly_examples() - plot_all_examples() + plot_poly_examples() diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index 6ae7089..34aaeac 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -87,11 +87,22 @@ def get_A_known(self, target_dict=None, output_poly=False): if output_poly: return [A_1] else: - return A_1.get_matrix(self.var_dict) + return [A_1.get_matrix(self.var_dict)] def generate_random_setup(self): self.theta_ = np.array([-1]) + def get_D(self, that): + # TODO(FD) generalize and move to PolyLifter + D = np.array( + [ + [1.0, 0.0, 0.0], + [that, 1.0, 0.0], + [that**2, 2 * that, 1.0], + ] + ) + return D + class Poly6Lifter(PolyLifter): VARIABLE_LIST = [["h", "t", "z0", "z1"]] From 2934d239a38c09b67bf72b34e8edd2d0aa6203c6 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 14:36:16 -0500 Subject: [PATCH 08/13] Fix tests --- _test/test_parameters.py | 146 +++------------------------------- _test/test_solvers.py | 42 +++++----- _test/test_special_cases.py | 3 +- _test/test_state_lifter.py | 20 +++-- _test/test_tools.py | 7 +- certifiable-tools | 2 +- lifters/poly_lifters.py | 17 ++-- lifters/robust_pose_lifter.py | 11 +-- lifters/state_lifter.py | 19 ++--- lifters/stereo1d_lifter.py | 6 +- lifters/stereo2d_problem.py | 25 ++++-- poly_matrix | 2 +- utils/constraint.py | 2 +- utils/geometry.py | 3 +- 14 files changed, 93 insertions(+), 212 deletions(-) diff --git a/_test/test_parameters.py b/_test/test_parameters.py index e27d2fa..5f09cda 100644 --- a/_test/test_parameters.py +++ b/_test/test_parameters.py @@ -5,9 +5,6 @@ from lifters.stereo2d_lifter import Stereo2DLifter from poly_matrix.poly_matrix import PolyMatrix -INCREMENTAL = True -NORMALIZE = True - def test_canonical_operations(): n_landmarks = 1 # z_0 and z_1 @@ -17,12 +14,12 @@ def test_canonical_operations(): # fmt: off Ai_sub = np.array( [ - [1, 0, 0, 0, 0, 0], - [0, 0, .5, 0, 0, 0], - [0, .5, 0, 0, .5, 0], - [0, 0, 0, 1, 0, 0], - [0, 0, .5, 0, 0, 0], - [0, 0, 0, 0, 0, 0], + [1, 0, 0, 0, 0, 0], + [0, 0,.5, 0, 0, 0], + [0,.5, 0, 0,.5, 0], + [0, 0, 0, 1, 0, 0], + [0, 0,.5, 0, 0, 0], + [0, 0, 0, 0, 0, 0], ] ) Ai_poly = PolyMatrix(symmetric=True) @@ -44,39 +41,6 @@ def test_canonical_operations(): def test_learned_constraints(d=2, param_level="ppT"): - n_landmarks = 2 # z_0 and z_1 - if d == 1: - lifter = Stereo1DLifter(n_landmarks=n_landmarks, param_level=param_level) - elif d == 2: - lifter = Stereo2DLifter( - n_landmarks=n_landmarks, param_level=param_level, level="urT" - ) - else: - raise ValueError(d) - - var_subset = ["l", "z_0", "z_1"] - label_dict = lifter.var_dict_row(var_subset) - - basis_row_list = lifter.get_basis_list(var_subset, plot=False) - basis_list = [ - basis_row.get_matrix((["l"], label_dict)) for basis_row in basis_row_list - ] - A_learned = lifter.generate_matrices(basis_list, var_dict=var_subset) - - # first, test that the learned constraints actually work on the original setup. - np.random.seed(0) - lifter.test_constraints(A_learned, errors="print") - - # then, with parameters, we can regenerate new learned variables for each new random setup. - for i in range(0, 10): - print(f"---- {i} -----") - np.random.seed(i) - lifter.generate_random_setup() - A_learned = lifter.generate_matrices(basis_list, var_dict=var_subset) - lifter.test_constraints(A_learned, errors="print") - - -def test_learned_constraints_augment(d=2, param_level="ppT"): n_landmarks = 2 # z_0 and z_1 if d == 1: lifter = Stereo1DLifter(n_landmarks=n_landmarks, param_level="p") @@ -87,46 +51,18 @@ def test_learned_constraints_augment(d=2, param_level="ppT"): else: raise ValueError(d) - if INCREMENTAL: - var_subset = ["l", "z_0", "z_1"] - label_dict = lifter.var_dict_row(var_subset) - - basis_list = lifter.get_basis_list(var_subset, plot=False) - basis_list_all = lifter.apply_templates(basis_list) - basis_poly_all = PolyMatrix.init_from_row_list(basis_list_all) - - basis_learned = basis_poly_all.get_matrix( - variables=(basis_poly_all.variable_dict_i, label_dict) - ) - A_learned = lifter.generate_matrices(basis_list, var_dict=var_subset) - else: - var_subset = list(lifter.var_dict.keys()) - label_dict = lifter.var_dict_row(var_subset) + A_learned = lifter.get_A_learned() - A_learned, basis_poly = lifter.get_A_learned(normalize=NORMALIZE) - basis_learned = basis_poly.get_matrix( - variables=(basis_poly.variable_dict_i, label_dict) - ) - - # first, test that the learned constraints actually work on the original setup. np.random.seed(0) lifter.test_constraints(A_learned, errors="raise") - # then, with parameters, we can regenerate new learned variables for each new random setup. - for i in range(10): - np.random.seed(i) - lifter.generate_random_setup() - A_learned = lifter.generate_matrices(basis_learned, var_dict=var_subset) - - lifter.test_constraints(A_learned, errors="print") - def test_b_to_a(): n_landmarks = 1 # z_0 only lifter = Stereo2DLifter(n_landmarks=n_landmarks, param_level="p", level="no") - for var_subset in [("l", "x"), ("l", "x", "z_0")]: + for var_subset in [("h", "x"), ("h", "x", "z_0")]: Y = lifter.generate_Y(var_subset=var_subset) basis_new, S = lifter.get_basis(Y) for i, bi_sub in enumerate(basis_new[:10, :]): @@ -151,70 +87,12 @@ def test_b_to_a(): ai_sub = lifter.get_reduced_a(bi_sub, var_subset=var_subset) assert abs(ai_sub @ x_sub) < 1e-10 - # put back in all other variables. - - bi_all = lifter.zero_pad_subvector( - bi_sub, var_subset, target_subset=lifter.var_dict - ) - # bi_all = lifter.get_vector_dense(bi_poly) - - if var_subset == tuple(lifter.var_dict.keys()): - try: - np.testing.assert_allclose(bi_all, bi_sub) - except: - fig, axs = plt.subplots(2, 1) - axs[0].matshow(bi_sub[None, :]) - axs[0].set_title("bi sub") - axs[1].matshow(bi_all[None, :]) - axs[1].set_title("bi all") - raise - - # generate the full matrix, placing the subvar in the correct place. - Ai = lifter.get_mat(ai_sub, var_dict=var_dict) - ai_all_test = lifter.get_vec(Ai) - - # generate the full matrix directly from the test vector - ai_all = lifter.get_reduced_a(bi_all) - Ai_test = lifter.get_mat(ai_all) - try: - np.testing.assert_allclose(ai_all_test, ai_all) - except: - fig, axs = plt.subplots(2, 1) - axs[0].matshow(ai_all[None, :]) - axs[0].set_title("ai all") - axs[1].matshow(ai_all_test[None, :]) - axs[1].set_title("ai all test") - raise - - try: - np.testing.assert_allclose(Ai.toarray(), Ai_test) - except: - fig, axs = plt.subplots(1, 2) - axs[0].matshow(Ai.toarray()) - axs[0].set_title("Ai") - axs[1].matshow(Ai_test) - axs[1].set_title("Ai test") - raise - - assert len(bi_all) == lifter.get_dim_X() * lifter.get_dim_P() - - x = lifter.get_x() - x_all = lifter.get_vec(np.outer(x, x)) - x_all_aug = lifter.augment_using_parameters(x_all) - - # test that x_all_aug @ bi_all holds (x includes parameters) - assert abs(bi_all @ x_all_aug) < 1e-10 - - ai_all = lifter.get_reduced_a(bi_all) - # test that ai_all @ x_all holds. - assert abs(ai_all @ x_all) < 1e-10 - def test_zero_padding(): n_landmarks = 1 # z_0 only lifter = Stereo2DLifter(n_landmarks=n_landmarks, param_level="p", level="no") - for var_subset in [("l", "x"), ("l", "x", "z_0")]: + for var_subset in [("h", "x"), ("h", "x", "z_0")]: var_dict = lifter.get_var_dict(var_subset) # get new patterns for this subset. Y = lifter.generate_Y(var_subset=var_subset) @@ -233,7 +111,7 @@ def test_zero_padding(): # Ai = lifter.get_mat(lifter.get_reduced_a(bi, lifter.var_dict)) try: lifter.test_constraints([Ai]) - except: + except AssertionError: b_poly_test = lifter.convert_b_to_polyrow(bi_sub, var_subset) print(b_poly_test) @@ -250,8 +128,6 @@ def test_zero_padding(): if __name__ == "__main__": test_zero_padding() test_b_to_a() - test_learned_constraints_augment(d=1, param_level="p") - test_learned_constraints_augment(d=2, param_level="ppT") - test_learned_constraints(d=2, param_level="ppT") + test_learned_constraints() test_canonical_operations() print("all tests passed") diff --git a/_test/test_solvers.py b/_test/test_solvers.py index e585ced..45c4a08 100644 --- a/_test/test_solvers.py +++ b/_test/test_solvers.py @@ -1,12 +1,16 @@ -import matplotlib.pylab as plt import numpy as np from lifters.poly_lifters import PolyLifter from lifters.mono_lifter import MonoLifter from lifters.robust_pose_lifter import RobustPoseLifter +from utils.geometry import get_xtheta_from_theta + from _test.test_tools import all_lifters +NOISE = 1e-2 + + def test_hess_finite_diff(): for lifter in all_lifters(): lifter.generate_random_setup() @@ -15,7 +19,7 @@ def test_hess_finite_diff(): errors = [] eps_list = np.logspace(-10, -5, 5) for eps in eps_list: - Q, y = lifter.get_Q(noise=1e-2) + Q, y = lifter.get_Q(noise=NOISE) theta = lifter.get_vec_around_gt(delta=0).flatten("C") try: @@ -110,7 +114,7 @@ def test_grad_finite_diff(): def test_cost_noisy(): - test_cost(noise=0.1) + test_cost(noise=NOISE) def test_cost(noise=0.0): @@ -142,7 +146,7 @@ def test_cost(noise=0.0): assert abs(cost - np.sum(w < 0)) < 1e-10 -def test_solvers_noisy(n_seeds=1, noise=1e-1): +def test_solvers_noisy(n_seeds=1, noise=NOISE): test_solvers(n_seeds=n_seeds, noise=noise) @@ -170,7 +174,8 @@ def test_solvers(n_seeds=1, noise=0.0): if len(theta_hat) == len(theta_gt): np.testing.assert_allclose(theta_hat, theta_gt) else: - theta_gt = lifter.get_vec_around_gt(delta=0) + # theta_gt = lifter.get_vec_around_gt(delta=0) + theta_gt = get_xtheta_from_theta(theta_gt, lifter.d) np.testing.assert_allclose(theta_hat, theta_gt) else: @@ -178,7 +183,7 @@ def test_solvers(n_seeds=1, noise=0.0): assert theta_hat is not None # test that we converge to real solution when initializing around it - theta_0 = lifter.get_vec_around_gt(delta=1e-2) + theta_0 = lifter.get_vec_around_gt(delta=NOISE) theta_hat, msg, cost_solver = lifter.local_solver(theta_0, y) print("init: ", theta_0) @@ -194,11 +199,16 @@ def test_solvers(n_seeds=1, noise=0.0): cost_lifter = lifter.get_cost(theta_hat, y) assert abs(cost_solver - cost_lifter) < 1e-10, (cost_solver, cost_lifter) - if noise == 0: - # test that "we made progress" + # test that "we made progress" + if len(theta_0) != len(theta_hat): + xtheta_0 = get_xtheta_from_theta(theta_0, lifter.d) + progress = np.linalg.norm(xtheta_0 - theta_hat) + else: progress = np.linalg.norm(theta_0 - theta_hat) - assert progress > 1e-10, progress + assert progress > 1e-10, progress + + if noise == 0: # test that cost decreased cost_0 = lifter.get_cost(theta_0, y) cost_hat = lifter.get_cost(theta_hat, y) @@ -227,17 +237,10 @@ def test_solvers(n_seeds=1, noise=0.0): except NotImplementedError: print("implement Hessian for further checks.") print(e) - else: - # test that "we made progress" - progress = np.linalg.norm(theta_0 - theta_hat) - assert progress > 1e-10, progress - - # just test that we converged when noise is added - assert theta_hat is not None def compare_solvers(): - noise = 1e-1 + noise = NOISE for lifter in all_lifters(): if isinstance(lifter, RobustPoseLifter): compare_solvers = [ @@ -278,6 +281,8 @@ def compare_solvers(): if theta_hat is None: print(solver, "failed") else: + if len(theta_hat) != len(theta_gt): + theta_gt = get_xtheta_from_theta(theta_gt, lifter.d) error = np.linalg.norm(theta_hat - theta_gt) print( f"{solver} finished in {ttot:.4f}s, final cost {cost_solver:.1e}, error {error:.1e}. \n\tmessage:{msg} " @@ -285,13 +290,13 @@ def compare_solvers(): if __name__ == "__main__": - import sys import warnings # import pytest # print("testing") # pytest.main([__file__, "-s"]) # print("all tests passed") + with warnings.catch_warnings(): # warnings.simplefilter("error") test_cost() @@ -307,4 +312,3 @@ def compare_solvers(): compare_solvers() print("all tests passed") - # sys.exit() diff --git a/_test/test_special_cases.py b/_test/test_special_cases.py index 3165135..98210cf 100644 --- a/_test/test_special_cases.py +++ b/_test/test_special_cases.py @@ -2,7 +2,6 @@ TODO: SLAM is currently not supported and therefore these tests are not being kept up to date. """ -import matplotlib.pylab as plt import numpy as np from lifters.range_only_slam1 import RangeOnlySLAM1Lifter @@ -84,7 +83,7 @@ def old_test_gauge(): try: S, V = np.linalg.eig(hess) - except: + except ValueError: S, V = np.linalg.eig(hess.toarray()) mask = np.abs(S) < 1e-10 n_null = np.sum(mask) diff --git a/_test/test_state_lifter.py b/_test/test_state_lifter.py index b58ecf2..03aa449 100644 --- a/_test/test_state_lifter.py +++ b/_test/test_state_lifter.py @@ -1,8 +1,11 @@ -import matplotlib.pylab as plt import numpy as np from _test.test_tools import all_lifters -from lifters.state_lifter import unravel_multi_index_triu, ravel_multi_index_triu +from lifters.state_lifter import ( + unravel_multi_index_triu, + ravel_multi_index_triu, + StateLifter, +) import pytest @@ -39,9 +42,9 @@ def test_ravel(): def _test_with_tol(lifter, A_list, tol): - x = lifter.get_x() + x = lifter.get_x().astype(float).reshape((-1, 1)) for Ai in A_list: - err = abs(x.T @ Ai @ x) + err = abs((x.T @ Ai @ x)[0, 0]) assert err < tol, err ai = lifter.get_vec(Ai.toarray()) @@ -85,9 +88,10 @@ def test_learned_constraints(): def test_vec_mat(): """Make sure that we can go back and forth from vec to mat.""" for lifter in all_lifters(): + assert isinstance(lifter, StateLifter) try: A_known = lifter.get_A_known() - except: + except AttributeError: print(f"could not get A_known of {lifter}") A_known = [] @@ -110,15 +114,10 @@ def test_vec_mat(): a_test = lifter.convert_polyrow_to_a(a_poly) np.testing.assert_allclose(a, a_test) - A_learned = lifter.get_A_learned(A_known=A_known, normalize=False) - for A_l, A_k in zip(A_learned[:3], A_known): - np.testing.assert_allclose(A_l.toarray(), A_k.toarray()) - pytest_configure() if __name__ == "__main__": - import sys import warnings test_ravel() @@ -137,4 +136,3 @@ def test_vec_mat(): warnings.simplefilter("ignore") print("all tests passed") - # sys.exit() diff --git a/_test/test_tools.py b/_test/test_tools.py index f10723b..c658087 100644 --- a/_test/test_tools.py +++ b/_test/test_tools.py @@ -4,6 +4,7 @@ from lifters.range_only_lifters import RangeOnlyLocLifter from lifters.range_only_slam1 import RangeOnlySLAM1Lifter from lifters.range_only_slam2 import RangeOnlySLAM2Lifter +from lifters.state_lifter import StateLifter from lifters.stereo1d_lifter import Stereo1DLifter from lifters.stereo2d_lifter import Stereo2DLifter from lifters.stereo3d_lifter import Stereo3DLifter @@ -14,8 +15,8 @@ n_landmarks = 3 n_poses = 4 Lifters = [ - # (Poly4Lifter, dict()), - # (Poly6Lifter, dict()), + (Poly4Lifter, dict()), + (Poly6Lifter, dict()), (WahbaLifter, dict(n_landmarks=3, d=2, robust=False, level="no", n_outliers=0)), (MonoLifter, dict(n_landmarks=5, d=2, robust=False, level="no", n_outliers=0)), (WahbaLifter, dict(n_landmarks=5, d=2, robust=True, level="xwT", n_outliers=1)), @@ -36,7 +37,7 @@ # Below, we always reset seeds to make sure tests are reproducible. -def all_lifters(): +def all_lifters() -> StateLifter: for Lifter, kwargs in Lifters: np.random.seed(1) yield Lifter(**kwargs) diff --git a/certifiable-tools b/certifiable-tools index 9506e70..d83c62d 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit 9506e7017e307af45cd198f957a05534c2136fb9 +Subproject commit d83c62dfd2b92f7a6dbab90b3e776974fb0be56c diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index 34aaeac..98a407a 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -41,26 +41,29 @@ def get_Q_mat(self): def get_error(self, t): return {"MAE": float(abs(self.theta - t))} - def get_x(self, t=None, parameters=None, var_subset=None): - if t is None: - t = self.theta - return np.array([t**i for i in range(self.degree // 2 + 1)]) + def get_x(self, theta=None, parameters=None, var_subset=None): + if theta is None: + theta = self.theta + return np.array([theta**i for i in range(self.degree // 2 + 1)]) def get_Q(self, noise=1e-3): Q = self.get_Q_mat() return Q, None - def get_cost(self, t, *args, **kwargs): + def get_cost(self, theta, *args, **kwargs): Q = self.get_Q_mat() - x = self.get_x(t) + x = self.get_x(theta) return x.T @ Q @ x + def get_hess(self, *args, **kwargs): + raise NotImplementedError + def local_solver(self, t0, *args, **kwargs): from scipy.optimize import minimize sol = minimize(self.get_cost, t0) info = {"success": sol.success} - return sol.x[0], info, sol.fun + return sol.x, info, sol.fun def __repr__(self): return f"poly{self.degree}" diff --git a/lifters/robust_pose_lifter.py b/lifters/robust_pose_lifter.py index 361b6d8..c439282 100644 --- a/lifters/robust_pose_lifter.py +++ b/lifters/robust_pose_lifter.py @@ -1,9 +1,6 @@ from abc import abstractmethod, ABC from copy import deepcopy -import matplotlib -import matplotlib.pylab as plt - import autograd.numpy as np from lifters.state_lifter import StateLifter @@ -267,7 +264,7 @@ def get_cost(self, theta, y): cost = 0 for i in range(self.n_landmarks): - res = self.residual(R, t, self.landmarks[i], y[i]) + res = self.residual_sq(R, t, self.landmarks[i], y[i]) if self.robust: cost += (1 + w[i]) / self.beta**2 * res + 1 - w[i] else: @@ -318,7 +315,7 @@ def cost(R, t): return 0.5 * cost + self.penalty(t) @pymanopt.function.autograd(manifold) - def euclidean_gradient(R, t): + def euclidean_gradient_unused(R, t): grad_R = np.zeros(R.shape) grad_t = np.zeros(t.shape) for i in range(self.n_landmarks): @@ -338,9 +335,9 @@ def euclidean_gradient(R, t): grad_t += Wi.T @ term return grad_R, grad_t - euclidean_gradient = None # set to None + euclidean_gradient = None problem = pymanopt.Problem( - manifold, cost, euclidean_gradient=euclidean_gradient # + manifold, cost, euclidean_gradient=euclidean_gradient ) optimizer = Optimizer(**solver_kwargs) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 4c7bdb9..d537dd6 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -170,9 +170,6 @@ def theta(self, value): @property def base_var_dict(self): - var_dict = {} - # var_dict.update({f"c_{i}": self.d for i in range(self.d)}) - # var_dict.update({"t": self.d}) var_dict = {f"x": self.d**2 + self.d} return var_dict @@ -308,6 +305,12 @@ def get_mat(self, vec, sparse=False, var_dict=None, correct=True): all_var_dict = {key[2]: 1 for key in augment_var_dict.values()} return Ai_poly.get_matrix(all_var_dict) + def get_A_learned(self, A_known=[], var_dict=None, method=METHOD) -> list: + Y = self.generate_Y(var_subset=var_dict) + basis, S = self.get_basis(Y, A_known=A_known, method=method) + A_learned = self.generate_matrices(basis) + return A_learned + def get_A_known(self, var_dict=None) -> list: return [] @@ -589,16 +592,6 @@ def convert_b_to_polyrow(self, b, var_subset, tol=1e-10) -> PolyMatrix: poly_row["h", key] = val return poly_row - def zero_pad_subvector(self, b, var_subset, target_subset=None): - b_row = self.convert_b_to_polyrow(b, var_subset) - target_row_dict = self.var_dict_row(var_subset=target_subset) - - # find out if the relevant variables of b are a subset of target_subset. - if set(b_row.variable_dict_j).issubset(target_row_dict): - return self.zero_pad_subvector_old(b, var_subset, target_subset) - else: - return None - def apply_templates(self, basis_list, n_landmarks=None, verbose=False): """ Apply the learned patterns in basis_list to all landmarks. diff --git a/lifters/stereo1d_lifter.py b/lifters/stereo1d_lifter.py index 84a1f10..a337418 100644 --- a/lifters/stereo1d_lifter.py +++ b/lifters/stereo1d_lifter.py @@ -68,13 +68,13 @@ def get_x(self, theta=None, parameters=None, var_subset=None): if key == "h": x_data.append(1.0) elif key == "x": - x_data.append(float(theta)) + x_data.append(float(theta[0])) elif "z" in key: idx = int(key.split("_")[-1]) if self.param_level == "p": - x_data.append(float(1 / (theta - parameters[idx + 1]))) + x_data.append(float(1 / (theta[0] - parameters[idx + 1]))) elif self.param_level == "no": - x_data.append(float(1 / (theta - self.landmarks[idx]))) + x_data.append(float(1 / (theta[0] - self.landmarks[idx]))) else: raise ValueError("unknown key in get_x", key) return np.array(x_data) diff --git a/lifters/stereo2d_problem.py b/lifters/stereo2d_problem.py index 9fbfcbe..b929030 100644 --- a/lifters/stereo2d_problem.py +++ b/lifters/stereo2d_problem.py @@ -29,14 +29,23 @@ def _T(phi): phi_flat = phi[:, 0] else: phi_flat = phi - x, y, theta = phi_flat - return np.array( - [ - [np.cos(theta), -np.sin(theta), x], - [np.sin(theta), np.cos(theta), y], - [0, 0, 1], - ] - ) + try: + x, y, theta = phi_flat + return np.array( + [ + [np.cos(theta), -np.sin(theta), x], + [np.sin(theta), np.cos(theta), y], + [0, 0, 1], + ] + ) + except ValueError: + x, y, *c = phi_flat + return np.vstack( + [ + np.hstack([np.array(c).reshape((2, 2)).T, np.array([x, y])[:, None]]), + np.array([0, 0, 1]), + ] + ) def generate_problem(N, sigma, M): diff --git a/poly_matrix b/poly_matrix index 14ee250..7910a28 160000 --- a/poly_matrix +++ b/poly_matrix @@ -1 +1 @@ -Subproject commit 14ee25087e9d46265985825ad5d028c67f3b9491 +Subproject commit 7910a28d60c7beeb6dabc0c236aa1f4898f107a3 diff --git a/utils/constraint.py b/utils/constraint.py index 41c8a82..9194ebe 100644 --- a/utils/constraint.py +++ b/utils/constraint.py @@ -118,9 +118,9 @@ def init_from_A_poly( @staticmethod def init_from_polyrow_b( - index: int, polyrow_b: PolyMatrix, lifter: StateLifter, + index: int = 0, known: bool = False, template_idx: int = None, ): diff --git a/utils/geometry.py b/utils/geometry.py index e80bb3e..acea108 100644 --- a/utils/geometry.py +++ b/utils/geometry.py @@ -92,6 +92,7 @@ def get_xtheta_from_T(T): r = T[:-1, -1] return np.r_[r, C.flatten("F")] + def get_pose_errors_from_xtheta(xtheta_hat, xtheta_gt, d): C_hat, r_hat = get_C_r_from_xtheta(xtheta_hat, d) C_gt, r_gt = get_C_r_from_xtheta(xtheta_gt, d) @@ -101,4 +102,4 @@ def get_pose_errors_from_xtheta(xtheta_hat, xtheta_gt, d): "r error": r_error, "C error": C_error, "total error": r_error + C_error, - } \ No newline at end of file + } From 7834ffb01534a55fdd2fb53dcfdea727b1e72e60 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 14:57:20 -0500 Subject: [PATCH 09/13] Add workflow --- .github/workflows/python-package-conda.yml | 44 ++++++++++++++++++++++ .gitignore | 2 + poly_matrix | 2 +- starloc | 2 +- 4 files changed, 48 insertions(+), 2 deletions(-) create mode 100644 .github/workflows/python-package-conda.yml diff --git a/.github/workflows/python-package-conda.yml b/.github/workflows/python-package-conda.yml new file mode 100644 index 0000000..29b0727 --- /dev/null +++ b/.github/workflows/python-package-conda.yml @@ -0,0 +1,44 @@ +# This workflow will install Python dependencies, run tests and lint with a single version of Python +# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python + +name: Python application + +on: + push: + branches: [ "master" ] + pull_request: + branches: [ "master" ] + +permissions: + contents: read + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Set up Python 3.10 + uses: actions/setup-python@v3 + with: + python-version: "3.10" + - name: Add conda to system path + run: | + # $CONDA is an environment variable pointing to the root of the miniconda directory + echo $CONDA/bin >> $GITHUB_PATH + - name: Install dependencies + run: | + conda env update --file environment.yml --name base + - name: Lint with flake8 + run: | + # stop the build if there are Python syntax errors or undefined names + flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics + # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide + flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics + - name: Test with pytest + run: | + conda install pytest + pytest diff --git a/.gitignore b/.gitignore index e5fa06c..f11811c 100644 --- a/.gitignore +++ b/.gitignore @@ -14,3 +14,5 @@ venv/ *.code-workspace solve_mosek.ptf solve_cvxpy*.ptf +build/ +dist/ diff --git a/poly_matrix b/poly_matrix index 7910a28..14ee250 160000 --- a/poly_matrix +++ b/poly_matrix @@ -1 +1 @@ -Subproject commit 7910a28d60c7beeb6dabc0c236aa1f4898f107a3 +Subproject commit 14ee25087e9d46265985825ad5d028c67f3b9491 diff --git a/starloc b/starloc index c555a79..d3ad541 160000 --- a/starloc +++ b/starloc @@ -1 +1 @@ -Subproject commit c555a79b7ff2afa7fa14149b9003e0796f36f0af +Subproject commit d3ad541c9c9d9c7f7cbfa5064a13fc81aebeb9e2 From be1367c8e5f6ee369445c8c9efb9a0a3a1c176f9 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 14:58:49 -0500 Subject: [PATCH 10/13] Update submodule --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 37a7936..7dc668f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,7 +3,7 @@ url = git@github.com:utiasASRL/poly_matrix [submodule "starloc"] path = starloc - url = git@github.com:duembgen/starloc + url = git@github.com:utiasASRL/starloc [submodule "certifiable-tools"] path = certifiable-tools url = git@github.com:utiasASRL/certifiable-tools From d6cb104311a43f9e05082f9d963202704a1e562c Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 15:04:21 -0500 Subject: [PATCH 11/13] Try adding private token --- .github/workflows/python-package-conda.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/python-package-conda.yml b/.github/workflows/python-package-conda.yml index 29b0727..38bc3f5 100644 --- a/.github/workflows/python-package-conda.yml +++ b/.github/workflows/python-package-conda.yml @@ -20,7 +20,8 @@ jobs: steps: - uses: actions/checkout@v3 with: - submodules: true + token: ${{ secrets.CONSTRAINT_LEARNING_PAT }} + submodules: recursive - name: Set up Python 3.10 uses: actions/setup-python@v3 with: From b23fcc7ffa050091c9d974393ee5e0d32259cbaa Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 15:10:13 -0500 Subject: [PATCH 12/13] Add flake8 --- environment.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/environment.yml b/environment.yml index 5eb1318..ea620df 100644 --- a/environment.yml +++ b/environment.yml @@ -8,6 +8,7 @@ channels: dependencies: - python=3.10 - pip=22.3 + - flake8 - numpy>=1.23.5 - scipy>=1.9.3 From 0a9392f8853b62b60358a2b869c11ba6fe4583f3 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 29 Dec 2023 15:17:51 -0500 Subject: [PATCH 13/13] Fix lynting errors --- lifters/robust_pose_lifter.py | 3 +- lifters/stereo1d_slam_lifter.py | 205 -------------------------------- utils/plotting_tools.py | 18 --- 3 files changed, 1 insertion(+), 225 deletions(-) delete mode 100644 lifters/stereo1d_slam_lifter.py diff --git a/lifters/robust_pose_lifter.py b/lifters/robust_pose_lifter.py index c439282..99c02bb 100644 --- a/lifters/robust_pose_lifter.py +++ b/lifters/robust_pose_lifter.py @@ -95,8 +95,7 @@ def penalty(self, t, rho=PENALTY_RHO, u=PENALTY_U): [rho * u * np.log10(1 + np.exp(hi / u)) for hi in self.h_list(t)] ) except RuntimeWarning: - PENALTY_U *= 0.1 - u = PENALTY_U + u *= 0.1 return np.sum( [rho * u * np.log10(1 + np.exp(hi / u)) for hi in self.h_list(t)] ) diff --git a/lifters/stereo1d_slam_lifter.py b/lifters/stereo1d_slam_lifter.py deleted file mode 100644 index fb92cf1..0000000 --- a/lifters/stereo1d_slam_lifter.py +++ /dev/null @@ -1,205 +0,0 @@ -import numpy as np - -from lifters.state_lifter import StateLifter - - -class Stereo1DSLAMLifter(StateLifter): - LEVELS = ["no", "za", "all"] - - def __init__(self, n_landmarks, level="no"): - self.n_landmarks = n_landmarks - self.d = 1 - self.W = 1.0 - self.theta_ = None - self.level = level - if self.level == "no": - L = 0 - elif self.level == "za": - L = self.n_landmarks**2 - elif self.level == "all": - # z*a + t*a + t*z + y_i_j * ak - # y_i_j ut_i vt_i + w_i_j_k - L = ( - self.n_landmarks**2 - + 2 * self.n_landmarks - + self.n_landmarks**2 * self.n_landmarks - ) - super().__init__(theta_shape=(1 + n_landmarks,), M=n_landmarks, L=L) - - @property - def theta(self): - if self.theta_ is None: - self.theta_ = self.sample_theta() - return self.theta_ - - def generate_random_setup(self): - pass - - def sample_theta(self): - # for SLAM, we also want to regenerate the landmarks! - # self.generate_random_setup() - - theta = np.random.rand(1) - landmarks = np.random.rand(self.n_landmarks) - counter = 0 - while np.min(np.abs(theta - landmarks)) <= 1e-3: - theta = np.random.rand(1) - landmarks = np.random.rand(self.n_landmarks) - counter += 1 - if counter >= 1000: - print("Warning: couldn't find valid setup") - return - return np.r_[theta, landmarks] - - def get_x(self, theta=None, var_subset=None): - """ - :param var_subset: list of variables to include in x vector. Set to None for all. - """ - if theta is None: - theta = self.theta - if var_subset is None: - var_subset = self.var_dict.keys() - - x, *landmarks = theta - - x_data = [] - for key in var_subset: - if key == "h": - x_data.append(1.0) - elif key == "x": - x_data.append(float(x)) - elif "a" in key: - idx = int(key.split("_")[-1]) - x_data.append(float(landmarks[idx])) - elif "z" in key: - idx = int(key.split("_")[-1]) - zi = float(1 / (x - landmarks[idx])) - x_data.append(zi) - elif "y" in key: - i, j = list(map(int, key.split("_")[-2:])) - zi = float(1 / (x - landmarks[i])) - aj = landmarks[j] - x_data.append(zi * aj) - elif "u" in key: - idx = int(key.split("_")[-1]) - x_data.append(float(x * landmarks[idx])) - elif "v" in key: - idx = int(key.split("_")[-1]) - zi = float(1 / (x - landmarks[idx])) - x_data.append(float(x * zi)) - elif "w" in key: - i, j, k = list(map(int, key.split("_")[-3:])) - zi = float(1 / (x - landmarks[i])) - aj = landmarks[j] - ak = landmarks[k] - x_data.append(zi * aj * ak) - else: - raise ValueError("unknown key in get_x", key) - return np.array(x_data) - - @property - def var_dict(self): - import itertools - - vars = ["h", "x"] - vars += [f"a_{j}" for j in range(self.n_landmarks)] - vars += [f"z_{j}" for j in range(self.n_landmarks)] - if self.level == "za": - vars += [ - f"y_{i}_{j}" - for i, j in itertools.product( - range(self.n_landmarks), range(self.n_landmarks) - ) - ] - elif self.level == "all": - vars += [ - f"y_{i}_{j}" - for i, j in itertools.product( - range(self.n_landmarks), range(self.n_landmarks) - ) - ] - vars += [f"u_{i}" for i in range(self.n_landmarks)] - vars += [f"v_{i}" for i in range(self.n_landmarks)] - vars += [ - f"w_{i}_{j}_{k}" - for i, j, k in itertools.product( - range(self.n_landmarks), - range(self.n_landmarks), - range(self.n_landmarks), - ) - ] - return {v: 1 for v in vars} - - def get_grad(self, t, y): - raise NotImplementedError("get_grad not implement yet") - - def get_Q(self, noise: float = 1e-3) -> tuple: - from poly_matrix.least_squares_problem import LeastSquaresProblem - - x, *landmarks = self.theta - - y = 1 / (x - landmarks) + np.random.normal( - scale=noise, loc=0, size=self.n_landmarks - ) - - ls_problem = LeastSquaresProblem() - for j in range(len(y)): - ls_problem.add_residual({"h": -y[j], f"z_{j}": 1}) - return ls_problem.get_Q().get_matrix(self.var_dict), y - - def get_A_known(self, add_known_redundant=False): - from poly_matrix.poly_matrix import PolyMatrix - - A_known = [] - - # enforce that z_j = 1/(x - a_j) <=> 1 - z_j*x + a_j*z_j = 0 - for j in range(self.n_landmarks): - A = PolyMatrix() - A[f"a_{j}", f"z_{j}"] = 0.5 - A["x", f"z_{j}"] = -0.5 - A["h", "h"] = 1.0 - A_known.append(A.get_matrix(variables=self.var_dict)) - - if True: # not add_known_redundant: - return A_known - - # TODO(FD) below constraint is not quadratic anymore! - # add known redundant constraints: - # enforce that z_j - z_i = (a_j - a_i) * z_j * z_i - for i in range(self.n_landmarks): - for j in range(i + 1, self.n_landmarks): - A = PolyMatrix() - A["h", f"z_{j}"] = 1 - A["h", f"z_{i}"] = -1 - A[f"z_{i}", f"z_{j}"] = self.landmarks[i] - landmarks[j] - A_known.append(A.get_matrix(variables=self.var_dict)) - return A_known - - def get_cost(self, x, y): - t, *landmarks = x - W = self.W - return np.sum((y - (1 / (t - landmarks))) ** 2) - - def local_solver( - self, t_init, y, num_iters=100, eps=1e-5, W=None, verbose=False, **kwargs - ): - x_op, *landmarks = t_init - for i in range(num_iters): - u = y - (1 / (x_op - landmarks)) - if verbose: - print(f"cost {i}", np.sum(u**2)) - du = 1 / ((x_op - landmarks) ** 2) - if np.linalg.norm(du) > 1e-10: - dx = -np.sum(u * du) / np.sum(du * du) - x_op = x_op + dx - t_op = np.r_[x_op, landmarks] - if np.abs(dx) < eps: - msg = f"converged in dx after {i} it" - return t_op, msg, self.get_cost(t_op, y) - else: - msg = f"converged in du after {i} it" - return t_op, msg, self.get_cost(t_op, y) - return None, "didn't converge", None - - def __repr__(self): - return "stereo1d_slam" diff --git a/utils/plotting_tools.py b/utils/plotting_tools.py index dab24f8..7124223 100644 --- a/utils/plotting_tools.py +++ b/utils/plotting_tools.py @@ -332,25 +332,7 @@ def plot_matrices(df_tight, fname_root): fname = fname_root + f"_{order_name}_{order_type}.png" savefig(fig, fname) - return - matrix_agg = { - "A": A_agg.astype(int), - "H": H, - "Q": Q.toarray(), - } - for matrix_type, matrix in matrix_agg.items(): - if matrix is None: - continue - fname = fname_root + f"_{order_name}_{order_type}_{matrix_type}.png" - fig, ax = plt.subplots() - plot_matrix( - ax=ax, - Ai=matrix, - colorbar=True, - title=matrix_type, - ) - savefig(fig, fname) def plot_singular_values(S, eps=None, label="singular values", ax=None):