diff --git a/.gitignore b/.gitignore index 7493223..4741f54 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,8 @@ _results_server/ _results_laptop/ _plots/ starrynight/ +starloc/ +ro_certs/ log/ __pycache__ @@ -18,3 +20,4 @@ solve_cvxpy*.ptf build/ dist/ mosek_output.tmp + diff --git a/_scripts/run_datasets_ro.py b/_scripts/run_datasets_ro.py index f765c4b..2f0522e 100644 --- a/_scripts/run_datasets_ro.py +++ b/_scripts/run_datasets_ro.py @@ -16,6 +16,7 @@ plot_local_vs_global, plot_results, run_experiments, + save_rmse, ) DATASET_ROOT = str(Path(__file__).parent.parent) @@ -28,7 +29,7 @@ RECOMPUTE = True -RESULTS_DIR = "_results_server" +RESULTS_DIR = "_results" def run_all(recompute=RECOMPUTE, n_successful=100, plot_poses=False): @@ -66,12 +67,16 @@ def run_all(recompute=RECOMPUTE, n_successful=100, plot_poses=False): df_all["dataset"] = dataset df_list.append(df_all) + fname_root = f"{RESULTS_DIR}/ro_{level}" + df = pd.concat(df_list) + fname = fname_root + f"_dataset_errors_n{n_successful}.pkl" + df.to_pickle(fname) + print(f"result df as {fname}") + if n_successful > 10: - df = pd.concat(df_list) constraint_type = "sorted" df = df[df.type == constraint_type] df["RDG"] = df["RDG"].abs() - fname_root = f"{RESULTS_DIR}/ro_{level}" plot_local_vs_global(df, fname_root=fname_root) @@ -94,4 +99,12 @@ def run_all(recompute=RECOMPUTE, n_successful=100, plot_poses=False): if __name__ == "__main__": - run_all() + # run a few examples and plot the error. + # run_all(n_successful=10, plot_poses=False) + + # run many and plot distributions + # run_all(n_successful=100, plot_poses=False) + + # create error table + fname_root = f"{RESULTS_DIR}/ro_quad" + save_rmse(fname_root=fname_root, n_successful=100) diff --git a/_scripts/run_datasets_stereo.py b/_scripts/run_datasets_stereo.py index 0246d1d..6286602 100644 --- a/_scripts/run_datasets_stereo.py +++ b/_scripts/run_datasets_stereo.py @@ -2,6 +2,7 @@ import matplotlib import matplotlib.pylab as plt +import numpy as np import pandas as pd try: @@ -15,6 +16,7 @@ plot_local_vs_global, plot_results, run_experiments, + save_rmse, ) DATASET_ROOT = str(Path(__file__).parent.parent) @@ -26,7 +28,7 @@ RECOMPUTE = True -RESULTS_DIR = "_results_server" +RESULTS_DIR = "_results" def load_experiment(dataset): @@ -42,7 +44,7 @@ def load_experiment(dataset): return exp -def run_all(recompute=RECOMPUTE, n_successful=10): +def run_all(recompute=RECOMPUTE, n_successful=10, stride=1): df_list = [] # don't change order! (because of plotting) @@ -67,19 +69,23 @@ def run_all(recompute=RECOMPUTE, n_successful=10): out_name=fname, n_successful=n_successful, level="urT", - stride=1, + stride=stride, ) if df_all is not None: df_all["dataset"] = dataset df_list.append(df_all) + fname_root = f"{RESULTS_DIR}/stereo" + df = pd.concat(df_list) + fname = fname_root + f"_dataset_errors_n{n_successful}.pkl" + df.to_pickle(fname) + print(f"result df as {fname}") + if n_successful > 10: - df = pd.concat(df_list) constraint_type = "sorted" df = df[df.type == constraint_type] df["RDG"] = df["RDG"].abs() - fname_root = f"{RESULTS_DIR}/stereo" # cost below is found empirically plot_local_vs_global(df, fname_root=fname_root, cost_thresh=1e3) @@ -103,4 +109,12 @@ def run_all(recompute=RECOMPUTE, n_successful=10): if __name__ == "__main__": - run_all() + # run a few examples and plot the error. + # run_all(n_successful=10, stride=1) + + # run many and plot distributions + # run_all(n_successful=100, stride=1) + + # create error table + fname_root = f"{RESULTS_DIR}/stereo" + save_rmse(fname_root=fname_root, n_successful=100) diff --git a/_scripts/run_range_only_study.py b/_scripts/run_range_only_study.py index 7a57f74..1d7b977 100644 --- a/_scripts/run_range_only_study.py +++ b/_scripts/run_range_only_study.py @@ -109,4 +109,4 @@ def run_all(n_seeds, recompute, tightness=True, scalability=True): if __name__ == "__main__": - run_all(n_seeds=1, recompute=False) + run_all(n_seeds=1, recompute=True) diff --git a/_scripts/run_stereo_study.py b/_scripts/run_stereo_study.py index d19edee..946fba0 100644 --- a/_scripts/run_stereo_study.py +++ b/_scripts/run_stereo_study.py @@ -100,6 +100,7 @@ def stereo_scalability_new(n_seeds, recompute, d=2): fname_root = f"{RESULTS_DIR}/scalability_{learner.lifter}" learner = Learner(lifter=lifter, variable_list=lifter.variable_list) run_scalability_plot(learner, recompute=recompute, fname_root=fname_root) + return df = run_scalability_new( learner, diff --git a/auto_template/learner.py b/auto_template/learner.py index 93789e3..82b4cca 100644 --- a/auto_template/learner.py +++ b/auto_template/learner.py @@ -5,6 +5,7 @@ import pandas as pd import scipy.sparse as sp from cert_tools.linalg_tools import find_dependent_columns, rank_project +from cert_tools.sdp_solvers import solve_feasibility_sdp from lifters.state_lifter import StateLifter from poly_matrix.poly_matrix import PolyMatrix @@ -422,13 +423,27 @@ def find_local_solution(self, verbose=False, plot=False): qcqp_that, qcqp_cost, info = find_local_minimum( self.lifter, y=y, verbose=verbose, n_inits=self.n_inits, plot=plot ) + self.solver_vars = dict(Q=Q, y=y, qcqp_cost=qcqp_cost, xhat=None) + self.solver_vars.update(info) if qcqp_cost is not None: xhat = self.lifter.get_x(qcqp_that) - self.solver_vars = dict(Q=Q, y=y, qcqp_cost=qcqp_cost, xhat=xhat) - self.solver_vars.update(info) + self.solver_vars["xhat"] = xhat + + # calculate error for global estimate self.solver_vars.update(self.lifter.get_error(qcqp_that)) + # calculate errors for local estimates + for key, qcqp_that_local in info.items(): + if key.startswith("local solution"): + solution_idx = key.strip("local solution ") + error_dict = self.lifter.get_error(qcqp_that_local) + self.solver_vars.update( + { + f"local {solution_idx} {error_name}": err + for error_name, err in error_dict.items() + } + ) + return True - 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"] is True: @@ -440,8 +455,9 @@ def find_global_solution(self, data_dict={}): xhat = self.solver_vars["xhat"] A_b_list_all = self.get_A_b_list() - H, info = solve_certificate( - self.solver_vars["Q"], A_b_list_all, xhat, adjust=True + + H, info = solve_feasibility_sdp( + self.solver_vars["Q"], A_b_list_all, xhat, adjust=True, verbose=False ) if info["eps"] is not None: print(f"global solution eps: {info['eps']:.2e}") @@ -458,8 +474,12 @@ def find_global_solution(self, data_dict={}): for key in keys: x_local = data_dict[key] x_local = self.lifter.get_x(theta=x_local) - H, info = solve_certificate( - self.solver_vars["Q"], A_b_list_all, x_local, adjust=True + H, info = solve_feasibility_sdp( + self.solver_vars["Q"], + A_b_list_all, + x_local, + adjust=True, + verbose=False, ) if info["eps"] is not None: print(f"local solution eps: {info['eps']:.2e}") diff --git a/auto_template/real_experiments.py b/auto_template/real_experiments.py index f6c3810..ebce5ff 100644 --- a/auto_template/real_experiments.py +++ b/auto_template/real_experiments.py @@ -1,7 +1,7 @@ -from copy import deepcopy import os import pickle import time +from copy import deepcopy import matplotlib.pylab as plt import numpy as np @@ -9,14 +9,11 @@ import seaborn as sns from pylgmath.so3.operations import hat -from starloc.reader import read_landmarks, read_data, read_calib - 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 -from utils.plotting_tools import plot_frame - +from starloc.reader import read_calib, read_data, read_landmarks +from utils.plotting_tools import plot_frame, savefig RANGE_TYPE = "range_calib" PLOT_NUMBER = 10 @@ -526,6 +523,88 @@ def run_real_experiment( return df +def save_rmse(fname_root, n_successful=10): + fname = fname_root + f"_dataset_errors_n{n_successful}.pkl" + try: + df = pd.read_pickle(fname) + print(f"result df as {fname}") + except FileNotFoundError: + print("Need to generate results first! Use run_all().") + return + + # use only once local solution per row (the best one) + df.reset_index(inplace=True) + for i, row in df.iterrows(): + if "local cost 2" in df.columns: + local_min_idx = np.argmin( + row[["local cost 0", "local cost 1", "local cost 2"]] + ) + elif "local cost 1" in df.columns: + local_min_idx = np.argmin(row[["local cost 0", "local cost 1"]]) + elif "local cost 0" in df.columns: + local_min_idx = 0 + else: + continue + try: # for stereo + df.loc[i, "local C error"] = row[f"local {local_min_idx} C error"] + df.loc[i, "local r error"] = row[f"local {local_min_idx} r error"] + except: # for ro + df.loc[i, "local error"] = row[f"local {local_min_idx} error"] + df.loc[i, "local cost"] = row[f"local cost {local_min_idx}"] + df.loc[i, "local solution cert"] = row[f"local solution {local_min_idx} cert"] + + # drop the rows where no local solution was found. + print("total number of datapoints:", len(df), end=", ") + df = df[df["local cost"].notna()] + print("after pruning:", len(df)) + df = df.apply(pd.to_numeric, errors="ignore") + + # df.loc[:, "local C error"] = df[ + # ["local 0 C error", "local 1 C error", "local 2 C error"] + # ].min(axis=1) + if "local r error" in df.columns: + values = [ + "r error", + "C error", + "local r error", + "local C error", + # "local solution cert", + # "global solution cert", + ] + else: + values = [ + "error", + "local error", + # "local solution cert", + # "global solution cert", + ] + pt = pd.pivot_table( + data=df, + values=values, + index=["dataset", "time index"], + sort=False, + ) + pt.rename( + columns={ + "local r error": "local $e_t$", + "local C error": "local $e_C$", + "r error": "global $e_t$", + "C error": "global $e_C$", + "local error": "local $e_t$", + "error": "global $e_t$", + }, + inplace=True, + ) + error_table = pt.agg(["min", "max", "median", "mean", "std"]) + print(error_table) + fname = fname.replace(".pkl", ".tex") + # s = error_table.style.highlight_min(props="itshape:; bfseries:;", axis=1) + s = error_table.style + with open(fname, "w"): + s.to_latex(fname) + print(f"saved as {fname}") + + def run_experiments( exp: Experiment, level, diff --git a/auto_template/sim_experiments.py b/auto_template/sim_experiments.py index 465d094..dfe2dca 100644 --- a/auto_template/sim_experiments.py +++ b/auto_template/sim_experiments.py @@ -7,14 +7,13 @@ import pandas as pd from auto_template.learner import Learner +from lifters.matweight_lifter import MatWeightLocLifter, MatWeightSLAMLifter from lifters.mono_lifter import MonoLifter from lifters.range_only_lifters import RangeOnlyLocLifter from lifters.robust_pose_lifter import RobustPoseLifter from lifters.stereo2d_lifter import Stereo2DLifter from lifters.stereo3d_lifter import Stereo3DLifter from lifters.wahba_lifter import WahbaLifter -from lifters.matweight_lifter import MatWeightLocLifter -from lifters.matweight_lifter import MatWeightSLAMLifter from utils.plotting_tools import savefig plot_dict = { diff --git a/certifiable-tools b/certifiable-tools index aac35e4..af95e3d 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit aac35e4a0fe57efb9f63121ccaafc5fa512a20ef +Subproject commit af95e3da2871ca32eba507e45997ea92746ec137 diff --git a/lifters/state_lifter.py b/lifters/state_lifter.py index b4095f1..cf192f8 100644 --- a/lifters/state_lifter.py +++ b/lifters/state_lifter.py @@ -170,6 +170,10 @@ def theta(self): self.theta_ = self.generate_random_theta() return self.theta_ + @theta.setter + def theta(self, t): + self.theta_ = t + @property def base_var_dict(self): var_dict = {"x": self.d**2 + self.d} diff --git a/solvers/common.py b/solvers/common.py index f0a2876..5e52eb7 100644 --- a/solvers/common.py +++ b/solvers/common.py @@ -58,10 +58,10 @@ def find_local_minimum( info["max res"] = max_res[global_inds[0]] info["cond Hess"] = cond_Hess[global_inds[0]] - for local_cost in local_costs: + for j, local_cost in enumerate(local_costs): local_ind = np.where(costs == local_cost)[0][0] - info[f"local solution {i}"] = local_solutions[local_ind] - info[f"local cost {i}"] = local_cost + info[f"local solution {j}"] = local_solutions[local_ind] + info[f"local cost {j}"] = local_cost # if (info["n local"] or info["n fail"]) and fname_root != "": if plot: @@ -124,18 +124,6 @@ def find_local_minimum( # TODO(FD) delete below if not used anymore. -def solve_certificate( - Q, - A_b_list, - xhat, - adjust=True, - verbose=False, - tol=None, -): +def solve_certificate(*args, **kwargs): """Solve certificate.""" - print( - "common.py -- WARNING: use solve_feasibility_sdp instead of solve_certificate! " - ) - return solve_feasibility_sdp( - Q, A_b_list, xhat, adjust=adjust, tol=tol, verobose=verbose - ) + raise ValueError("Use solve_feasibility_sdp instead of solve_certificate!")