Create RMSE results

.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
+

_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)

_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)

_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)

_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,

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}")

auto_template/real_experiments.py

@@ -1,22 +1,19 @@
-from copy import deepcopy
 import os
 import pickle
 import time
+from copy import deepcopy
 
 import matplotlib.pylab as plt
 import numpy as np
 import pandas as pd
 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,

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 = {

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}

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!")