From 07f9c1f06a079c9c1ab4a9e680d3bbc52526a3f9 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 9 Jan 2024 17:47:25 -0500 Subject: [PATCH 1/7] Fix import error and remove starloc --- .gitmodules | 3 --- lifters/base_class.py | 2 +- lifters/state_lifter.py | 8 +++----- 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/.gitmodules b/.gitmodules index 7dc668f..a9bd0d5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,6 @@ [submodule "poly_matrix"] path = poly_matrix url = git@github.com:utiasASRL/poly_matrix -[submodule "starloc"] - path = starloc - url = git@github.com:utiasASRL/starloc [submodule "certifiable-tools"] path = certifiable-tools url = git@github.com:utiasASRL/certifiable-tools diff --git a/lifters/base_class.py b/lifters/base_class.py index e425064..c994592 100644 --- a/lifters/base_class.py +++ b/lifters/base_class.py @@ -1,4 +1,4 @@ -from abc import ABC, abstractproperty, abstractmethod +from abc import ABC, abstractmethod, abstractproperty import numpy as np diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index d537dd6..1610c1b 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -1,15 +1,13 @@ import itertools -import numpy as np import matplotlib.pylab as plt +import numpy as np import scipy.sparse as sp - - -from utils.common import upper_triangular +from cert_tools.linalg_tools import get_nullspace from lifters.base_class import BaseClass -from cert_tools.linalg_tools import get_nullspace from poly_matrix import PolyMatrix, unroll +from utils.common import upper_triangular def ravel_multi_index_triu(index_tuple, shape): From e17d4a44ffed15178b096eab2761464a289d19d1 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 9 Jan 2024 19:21:07 -0500 Subject: [PATCH 2/7] Clean unused --- lifters/base_class.py | 5 +++-- lifters/state_lifter.py | 6 +----- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/lifters/base_class.py b/lifters/base_class.py index c994592..5d3192c 100644 --- a/lifters/base_class.py +++ b/lifters/base_class.py @@ -32,6 +32,7 @@ def __init__( self.variable_list = self.VARIABLE_LIST # variables that get overwritten upon initialization + self.parameters = [1.0] self.theta_ = None self.var_dict_ = None @@ -85,9 +86,9 @@ def get_x(self, theta, var_subset=None) -> np.ndarray: def get_p(self, parameters=None, var_subset=None) -> np.ndarray: return - # @abstractmethod def get_parameters(self, var_subset=None) -> list: - return + if self.param_level == "no": + return [1.0] def get_grad(self, t, y): Warning("get_grad not implement yet") diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 1610c1b..4cc51ff 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -162,10 +162,6 @@ def theta(self): self.theta_ = self.generate_random_theta() return self.theta_ - @theta.setter - def theta(self, value): - self.theta_ = value - @property def base_var_dict(self): var_dict = {f"x": self.d**2 + self.d} @@ -703,7 +699,7 @@ def generate_Y(self, factor=FACTOR, ax=None, var_subset=None): else: ax.scatter(*theta[:, :2].T) - x = self.get_x(theta, parameters, var_subset=var_subset) + x = self.get_x(theta=theta, parameters=parameters, var_subset=var_subset) X = np.outer(x, x) # generates [1*x, a1*x, ..., aK*x] From 99f81ae0422163331c8c593138c1beaa137ea471 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 9 Jan 2024 21:44:32 -0500 Subject: [PATCH 3/7] Simplify base class --- lifters/base_class.py | 56 +++++++++++++++-------------------------- lifters/state_lifter.py | 4 +-- 2 files changed, 22 insertions(+), 38 deletions(-) diff --git a/lifters/base_class.py b/lifters/base_class.py index 5d3192c..9ecd639 100644 --- a/lifters/base_class.py +++ b/lifters/base_class.py @@ -4,6 +4,11 @@ class BaseClass(ABC): + """ + This BaseClass is to be used as a template for creating new Lifters. + All the listed functionalities need to be defined by inheriting functions. + """ + LEVELS = ["no"] PARAM_LEVELS = ["no"] VARIABLE_LIST = ["h"] @@ -37,54 +42,40 @@ def __init__( self.var_dict_ = None self.d = d - self.generate_random_setup() - - @property - def d(self): - return self.d_ - - @d.setter - def d(self, var): - assert var in [1, 2, 3] - self.d_ = var @abstractproperty def var_dict(self): """Return key,size pairs of all variables.""" return - @abstractmethod - def get_param_idx_dict(self, var_subset=None): - """Return key,index pairs of all parameters touched by var_subset""" + @abstractproperty + def theta(self): + """Return ground truth theta.""" return @abstractmethod - def get_level_dims(self, n=1): + def get_x(self, theta, var_subset=None) -> np.ndarray: return @abstractmethod - def generate_random_setup(self): + def get_p(self, parameters=None, var_subset=None) -> np.ndarray: return - # @abstractmethod - def generate_random_theta(self): - pass - - # @abstractmethod + @abstractmethod def sample_theta(self): return - # @abstractmethod - def sample_parameters(self, x=None): - return - @abstractmethod - def get_x(self, theta, var_subset=None) -> np.ndarray: - return + def get_Q(self, noise=1e-3): + Warning("get_Q not implemented yet") + return None, None - @abstractmethod - def get_p(self, parameters=None, var_subset=None) -> np.ndarray: - return + def get_A_known(self): + return [] + + def sample_parameters(self, x=None): + if self.param_level == "no": + return [1.0] def get_parameters(self, var_subset=None) -> list: if self.param_level == "no": @@ -93,10 +84,3 @@ def get_parameters(self, var_subset=None) -> list: def get_grad(self, t, y): Warning("get_grad not implement yet") return None - - 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/state_lifter.py b/lifters/state_lifter.py index 4cc51ff..94fb1d9 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -5,9 +5,9 @@ import scipy.sparse as sp from cert_tools.linalg_tools import get_nullspace from lifters.base_class import BaseClass +from utils.common import upper_triangular from poly_matrix import PolyMatrix, unroll -from utils.common import upper_triangular def ravel_multi_index_triu(index_tuple, shape): @@ -847,7 +847,7 @@ def test_constraints(self, A_list, errors: str = "raise", n_seeds: int = 3): np.random.seed(i) t = self.sample_theta() p = self.get_parameters() - x = self.get_x(t, p) + x = self.get_x(theta=t, parameters=p) constraint_violation = abs(x.T @ A @ x) max_violation = max(max_violation, constraint_violation) From ceb0d3c5cb5ea0a473e1617c3a5d890a1e7e51e9 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Tue, 9 Jan 2024 21:56:35 -0500 Subject: [PATCH 4/7] Fix import order --- lifters/state_lifter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 94fb1d9..6403ade 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -4,10 +4,10 @@ import numpy as np import scipy.sparse as sp from cert_tools.linalg_tools import get_nullspace -from lifters.base_class import BaseClass -from utils.common import upper_triangular +from lifters.base_class import BaseClass from poly_matrix import PolyMatrix, unroll +from utils.common import upper_triangular def ravel_multi_index_triu(index_tuple, shape): From 7ce28d321f857cc7d6f16e8334bfacb97e96d60b Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 10 Jan 2024 19:00:34 -0500 Subject: [PATCH 5/7] Formatting --- _scripts/run_other_study.py | 3 ++- examples/robust_lifter.py | 13 ++++++++++--- lifters/state_lifter.py | 4 ++-- lifters/stereo_lifter.py | 4 ++-- lifters/wahba_lifter.py | 28 ++++++++++++++-------------- poly_matrix | 2 +- solvers/common.py | 1 - 7 files changed, 31 insertions(+), 24 deletions(-) diff --git a/_scripts/run_other_study.py b/_scripts/run_other_study.py index 7d6d5d7..f70099b 100644 --- a/_scripts/run_other_study.py +++ b/_scripts/run_other_study.py @@ -13,7 +13,8 @@ DEBUG = False RESULTS_DIR = "_results" -#RESULTS_DIR = "_results_server" +# RESULTS_DIR = "_results_server" + def lifter_tightness( Lifter=MonoLifter, robust: bool = False, d: int = 2, n_landmarks=4, n_outliers=0 diff --git a/examples/robust_lifter.py b/examples/robust_lifter.py index 046a51f..64826da 100644 --- a/examples/robust_lifter.py +++ b/examples/robust_lifter.py @@ -13,13 +13,20 @@ def get_problem(robust=True): # Create lifter np.random.seed(0) n_landmarks = 4 - d= 3 + 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) + 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 @@ -71,4 +78,4 @@ def plot_problem(prob, lifter, fname=""): 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 + # plot_problem(prob, lifter, fname=fname.replace(".pkl", ".png")) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 6403ade..94fb1d9 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -4,11 +4,11 @@ import numpy as np import scipy.sparse as sp from cert_tools.linalg_tools import get_nullspace - from lifters.base_class import BaseClass -from poly_matrix import PolyMatrix, unroll from utils.common import upper_triangular +from poly_matrix import PolyMatrix, unroll + def ravel_multi_index_triu(index_tuple, shape): """Equivalent of np.multi_index_triu, but using only the upper-triangular part of matrix.""" diff --git a/lifters/stereo_lifter.py b/lifters/stereo_lifter.py index 8048e9d..906190d 100644 --- a/lifters/stereo_lifter.py +++ b/lifters/stereo_lifter.py @@ -50,8 +50,8 @@ class StereoLifter(StateLifter, ABC): ["h", "x"], ["h", "z_0"], ["h", "x", "z_0"], - ["h", "z_0", "z_1"], # should achieve tightness here - # ["h", "x", "z_0", "z_1"], + ["h", "z_0", "z_1"], # should achieve tightness here + # ["h", "x", "z_0", "z_1"], # ["h", "z_0", "z_1", "z_2"], ] diff --git a/lifters/wahba_lifter.py b/lifters/wahba_lifter.py index 71e27e9..470ba36 100644 --- a/lifters/wahba_lifter.py +++ b/lifters/wahba_lifter.py @@ -83,17 +83,17 @@ def get_Q(self, noise: float = None): def get_Q_from_y(self, y): """ - every cost term can be written as - (1 + wi)/b^2 r^2(x, zi) + (1 - wi) - - residual term: - (Rpi + t - ui).T Wi (Rpi + t - ui) = - [t', vec(R)'] @ [I (pi x I)]' @ Wi @ [I (pi x I)] @ [t ; vec(R)] - ------x'----- -----Pi'----- - - 2 [t', vec(R)'] @ [I (pi x I)]' Wi @ ui - -----x'------ ---------Pi_xl-------- - + ui.T @ Wi @ ui - -----Pi_ll------ + every cost term can be written as + (1 + wi)/b^2 r^2(x, zi) + (1 - wi) + + residual term: + (Rpi + t - ui).T Wi (Rpi + t - ui) = + [t', vec(R)'] @ [I (pi x I)]' @ Wi @ [I (pi x I)] @ [t ; vec(R)] + ------x'----- -----Pi'----- + - 2 [t', vec(R)'] @ [I (pi x I)]' Wi @ ui + -----x'------ ---------Pi_xl-------- + + ui.T @ Wi @ ui + -----Pi_ll------ """ from poly_matrix.poly_matrix import PolyMatrix @@ -107,9 +107,9 @@ def get_Q_from_y(self, y): ui = y[i] Pi = np.c_[np.eye(self.d), np.kron(pi, np.eye(self.d))] - Pi_ll = ui.T @ Wi @ ui - Pi_xl = -(Pi.T @ Wi @ ui)[:, None] - Qi = Pi.T @ Wi @ Pi + Pi_ll = ui.T @ Wi @ ui + Pi_xl = -(Pi.T @ Wi @ ui)[:, None] + Qi = Pi.T @ Wi @ Pi if NORMALIZE: Pi_ll /= norm Pi_xl /= norm diff --git a/poly_matrix b/poly_matrix index 14ee250..abdd3cb 160000 --- a/poly_matrix +++ b/poly_matrix @@ -1 +1 @@ -Subproject commit 14ee25087e9d46265985825ad5d028c67f3b9491 +Subproject commit abdd3cbaf5fafffb61b7baf661071213dbac3df1 diff --git a/solvers/common.py b/solvers/common.py index 301ce51..f404231 100644 --- a/solvers/common.py +++ b/solvers/common.py @@ -214,7 +214,6 @@ def solve_sdp_cvxpy( return X, info - def solve_sdp_cvxpy_new( Q, A_b_list, From 59ea38fb73f69ef2b42416fe7562c722db443e0b Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 11 Jan 2024 21:29:23 -0500 Subject: [PATCH 6/7] Introduce variable name for homogenization --- lifters/state_lifter.py | 43 +++++++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 94fb1d9..87895cb 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -46,6 +46,7 @@ def unravel_multi_index_triu(flat_indices, shape): class StateLifter(BaseClass): + HOM = "h" # consider singular value zero below this EPS_SVD = 1e-5 @@ -176,7 +177,7 @@ def sub_var_dict(self): @property def var_dict(self): if self.var_dict_ is None: - self.var_dict_ = {"h": 1} + self.var_dict_ = {self.HOM: 1} self.var_dict_.update(self.base_var_dict) self.var_dict_.update(self.sub_var_dict) return self.var_dict_ @@ -357,7 +358,7 @@ def var_list_row(self, var_subset=None, force_parameters_off=False): label_list = [] if force_parameters_off: - param_dict = {"h": 0} + param_dict = {self.HOM: 0} else: param_dict = self.get_param_idx_dict(var_subset) for idx, key in enumerate(param_dict.keys()): @@ -398,7 +399,7 @@ def get_basis_from_poly_rows(self, basis_poly_list, var_subset=None): for i, bi_poly in enumerate(basis_poly_list): # test that this constraint holds - bi = bi_poly.get_matrix((["h"], all_dict)) + bi = bi_poly.get_matrix(([self.HOM], all_dict)) if bi.shape[1] == self.get_dim_X(var_subset) * self.get_dim_P(): ai = self.get_reduced_a(bi, var_subset=var_subset) @@ -424,13 +425,15 @@ def get_basis_from_poly_rows(self, basis_poly_list, var_subset=None): def get_vector_dense(self, poly_row_sub): # complete the missing variables var_dict = self.var_dict_row() - poly_row_all = poly_row_sub.get_matrix((["h"], var_dict), output_type="poly") + poly_row_all = poly_row_sub.get_matrix( + ([self.HOM], var_dict), output_type="poly" + ) vector = np.empty(0) for param in self.get_param_idx_dict().keys(): # extract each block corresponding to a bigger matrix sub_mat = PolyMatrix(symmetric=True) for vari, varj in itertools.combinations_with_replacement(self.var_dict, 2): - val = poly_row_all["h", f"{param}.{vari}.{varj}"] + val = poly_row_all[self.HOM, f"{param}.{vari}.{varj}"] if np.ndim(val) > 0: if vari != varj: sub_mat[vari, varj] = val.reshape( @@ -464,13 +467,13 @@ def old_convert_polyrow_to_a(self, poly_row, var_subset, correct=True): keyi_m, keyj_n = var_keys.split(".") m = keyi_m.split(":")[-1] n = keyj_n.split(":")[-1] - if param in ["h", "h.h"]: + if param in [self.HOM, f"{self.HOM}.{self.HOM}"]: param_val = 1.0 else: param_val = parameters[param_dict[param]] # divide off-diagonal elements by sqrt(2) - newval = poly_row["h", key] * param_val + newval = poly_row[self.HOM, key] * param_val if correct and not ((keyi_m == keyj_n) and (m == n)): newval /= np.sqrt(2) @@ -500,13 +503,13 @@ def convert_polyrow_to_Apoly(self, poly_row, correct=True): keyi_m, keyj_n = var_keys.split(".") m = keyi_m.split(":")[-1] n = keyj_n.split(":")[-1] - if param in ["h", "h.h"]: + if param in [self.HOM, f"{self.HOM}.{self.HOM}"]: param_val = 1.0 else: param_val = parameters[param_dict[param]] # divide off-diagonal elements by sqrt(2) - newval = poly_row["h", key] * param_val + newval = poly_row[self.HOM, key] * param_val if correct and not ((keyi_m == keyj_n) and (m == n)): newval /= np.sqrt(2) @@ -559,7 +562,7 @@ def convert_a_to_polyrow( for keyi, keyj in itertools.combinations_with_replacement(var_dict, 2): if keyi in poly_mat.matrix and keyj in poly_mat.matrix[keyi]: val = poly_mat.matrix[keyi][keyj] - labels = self.get_labels("h", keyi, keyj) + labels = self.get_labels(self.HOM, keyi, keyj) if keyi != keyj: vals = val.flatten() else: @@ -568,7 +571,7 @@ def convert_a_to_polyrow( assert len(labels) == len(vals) for label, v in zip(labels, vals): if np.any(np.abs(v) > self.EPS_SPARSE): - poly_row["h", label] = v + poly_row[self.HOM, label] = v return poly_row def convert_b_to_polyrow(self, b, var_subset, tol=1e-10) -> PolyMatrix: @@ -583,7 +586,7 @@ def convert_b_to_polyrow(self, b, var_subset, tol=1e-10) -> PolyMatrix: mask = np.abs(b) > tol var_list = [v for i, v in enumerate(self.var_list_row(var_subset)) if mask[i]] for key, val in zip(var_list, b[mask]): - poly_row["h", key] = val + poly_row[self.HOM, key] = val return poly_row def apply_templates(self, basis_list, n_landmarks=None, verbose=False): @@ -656,7 +659,7 @@ def apply_template(self, bi_poly, n_landmarks=None, verbose=False): ) except ValueError as e: pass - new_poly_row["h", key_ij] = bi_poly["h", key] + new_poly_row[self.HOM, key_ij] = bi_poly["h", key] new_poly_rows.append(new_poly_row) return new_poly_rows @@ -762,7 +765,7 @@ def get_reduced_a(self, bi, var_subset=None, sparse=False): if isinstance(bi, np.ndarray): len_b = len(bi) elif isinstance(bi, PolyMatrix): - bi = bi.get_matrix((["h"], self.var_dict_row(var_subset))) + bi = bi.get_matrix(([self.HOM], self.var_dict_row(var_subset))) len_b = bi.shape[1] else: # bi can be a scipy sparse matrix, @@ -870,7 +873,7 @@ def get_A0(self, var_subset=None): else: var_dict = self.var_dict A0 = PolyMatrix() - A0["h", "h"] = 1.0 + A0[self.HOM, self.HOM] = 1.0 return A0.get_matrix(var_dict) def get_A_b_list(self, A_list, var_subset=None): @@ -888,20 +891,22 @@ def get_param_idx_dict(self, var_subset=None): - if param_level == 'ppT': {'l': 0, 'p_0:0.p_0:0': 1, ..., 'p_0:d-1:.p_0:d-1': 1} """ if self.param_level == "no": - return {"h": 0} + return {self.HOM: 0} if var_subset is None: var_subset = self.var_dict variables = self.get_variable_indices(var_subset) - param_keys = ["h"] + [f"p_{i}:{d}" for i in variables for d in range(self.d)] + param_keys = [self.HOM] + [ + f"p_{i}:{d}" for i in variables for d in range(self.d) + ] if self.param_level == "p": param_dict = {p: i for i, p in enumerate(param_keys)} elif self.param_level == "ppT": i = 0 param_dict = {} for pi, pj in itertools.combinations_with_replacement(param_keys, 2): - if pi == pj == "h": - param_dict["h"] = i + if pi == pj == self.HOM: + param_dict[self.HOM] = i else: param_dict[f"{pi}.{pj}"] = i i += 1 From 2b7edb4a761b156b32bfca929de926081631dc5b Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 11 Jan 2024 22:03:02 -0500 Subject: [PATCH 7/7] Sort imports --- lifters/state_lifter.py | 4 ++-- lifters/stereo_lifter.py | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index 87895cb..211042d 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -4,10 +4,10 @@ import numpy as np import scipy.sparse as sp from cert_tools.linalg_tools import get_nullspace -from lifters.base_class import BaseClass -from utils.common import upper_triangular +from lifters.base_class import BaseClass from poly_matrix import PolyMatrix, unroll +from utils.common import upper_triangular def ravel_multi_index_triu(index_tuple, shape): diff --git a/lifters/stereo_lifter.py b/lifters/stereo_lifter.py index 906190d..a4a2044 100644 --- a/lifters/stereo_lifter.py +++ b/lifters/stereo_lifter.py @@ -7,13 +7,12 @@ from utils.geometry import ( get_C_r_from_theta, get_C_r_from_xtheta, + get_pose_errors_from_xtheta, get_T, - get_xtheta_from_theta, get_theta_from_xtheta, - get_pose_errors_from_xtheta, + get_xtheta_from_theta, ) - NOISE = 1.0 # NORMALIZE = True @@ -340,7 +339,7 @@ def get_error(self, xtheta_hat): def local_solver_manopt(self, t0, y, W=None, verbose=False, method="CG", **kwargs): import pymanopt - from pymanopt.manifolds import SpecialOrthogonalGroup, Euclidean, Product + from pymanopt.manifolds import Euclidean, Product, SpecialOrthogonalGroup if method == "CG": from pymanopt.optimizers import ConjugateGradient as Optimizer # fastest