diff --git a/.github/workflows/python-package-conda.yml b/.github/workflows/python-package-conda.yml index 38bc3f5..67ca5a4 100644 --- a/.github/workflows/python-package-conda.yml +++ b/.github/workflows/python-package-conda.yml @@ -21,7 +21,7 @@ jobs: - uses: actions/checkout@v3 with: token: ${{ secrets.CONSTRAINT_LEARNING_PAT }} - submodules: recursive + submodules: true - name: Set up Python 3.10 uses: actions/setup-python@v3 with: @@ -42,4 +42,4 @@ jobs: - name: Test with pytest run: | conda install pytest - pytest + pytest _test/ diff --git a/.gitignore b/.gitignore index b516269..7493223 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ _results/ _results_server/ +_results_laptop/ _plots/ starrynight/ log/ diff --git a/_scripts/get_server_results.sh b/_scripts/get_server_results.sh index 5dc13ed..681976c 100755 --- a/_scripts/get_server_results.sh +++ b/_scripts/get_server_results.sh @@ -1,7 +1,9 @@ #!/usr/bin/bash # arxiv mode, verbose, compress. -rsync -avz -e 'ssh' fdu@192.168.42.7:/home/fdu/constraint_learning/_results/* _results_server/ --exclude-from='_scripts/exclude-server.txt' --exclude="*.pdf" +#rsync -avz -e 'ssh' fdu@192.168.42.7:/home/fdu/constraint_learning/_results/* _results_server/ --exclude-from='_scripts/exclude-server.txt' --exclude="*.pdf" + +rsync -avz -e 'ssh' asrl@100.64.83.242:/home/asrl/research/constraint_learning/_results/* _results_laptop/ --exclude-from='_scripts/exclude-server.txt' --exclude="*.pdf" # not needed anymore: copy starrynight dataset over to server # rsync -avz -e 'ssh' ./starrynight fdu@192.168.42.7:/home/fdu/constraint_learning/ diff --git a/_scripts/run_all_study.py b/_scripts/run_all_study.py index 34638f5..5d564c6 100644 --- a/_scripts/run_all_study.py +++ b/_scripts/run_all_study.py @@ -50,14 +50,14 @@ def generate_results(lifters, seed=0): def run_all(recompute=RECOMPUTE): lifters = [ - # (RangeOnlyLocLifter, dict(n_positions=3, n_landmarks=10, d=3, level="no")), - # (RangeOnlyLocLifter, dict(n_positions=3, n_landmarks=10, d=3, level="quad")), + (RangeOnlyLocLifter, dict(n_positions=3, n_landmarks=10, d=3, level="no")), + (RangeOnlyLocLifter, dict(n_positions=3, n_landmarks=10, d=3, level="quad")), (Stereo2DLifter, dict(n_landmarks=3, param_level="ppT", level="urT")), - # (Stereo3DLifter, dict(n_landmarks=4, param_level="ppT", level="urT")), - # (WahbaLifter, dict(n_landmarks=5, d=3, robust=True, level="xwT", n_outliers=1)), - # (MonoLifter, dict(n_landmarks=6, d=3, robust=True, level="xwT", n_outliers=1)), - # (WahbaLifter, dict(n_landmarks=4, d=3, robust=False, level="no", n_outliers=0)), - # (MonoLifter, dict(n_landmarks=5, d=3, robust=False, level="no", n_outliers=0)), + (Stereo3DLifter, dict(n_landmarks=4, param_level="ppT", level="urT")), + (WahbaLifter, dict(n_landmarks=5, d=3, robust=True, level="xwT", n_outliers=1)), + (MonoLifter, dict(n_landmarks=6, d=3, robust=True, level="xwT", n_outliers=1)), + (WahbaLifter, dict(n_landmarks=4, d=3, robust=False, level="no", n_outliers=0)), + (MonoLifter, dict(n_landmarks=5, d=3, robust=False, level="no", n_outliers=0)), ] fname = f"{RESULTS_DIR}/all_df_new.pkl" diff --git a/_scripts/run_datasets_ro.py b/_scripts/run_datasets_ro.py index 3684a22..f765c4b 100644 --- a/_scripts/run_datasets_ro.py +++ b/_scripts/run_datasets_ro.py @@ -5,7 +5,7 @@ try: matplotlib.use("TkAgg") # non-interactive -except Exception as e: +except: pass import pandas as pd diff --git a/_scripts/run_other_study.py b/_scripts/run_other_study.py index 9979e6d..47fd79c 100644 --- a/_scripts/run_other_study.py +++ b/_scripts/run_other_study.py @@ -112,18 +112,18 @@ def run_wahba(n_seeds, recompute, tightness=True, scalability=True): print("================= Wahba study ==================") - # if tightness: - # lifter_tightness(WahbaLifter, d=d, n_landmarks=4, robust=False) + if tightness: + lifter_tightness(WahbaLifter, d=d, n_landmarks=4, robust=False) if scalability: - # lifter_scalability_new( - # WahbaLifter, - # d=d, - # n_landmarks=4, - # robust=False, - # n_outliers=0, - # n_seeds=n_seeds, - # recompute=recompute, - # ) + lifter_scalability_new( + WahbaLifter, + d=d, + n_landmarks=4, + robust=False, + n_outliers=0, + n_seeds=n_seeds, + recompute=recompute, + ) lifter_scalability_new( WahbaLifter, d=d, @@ -172,6 +172,6 @@ def run_all(n_seeds, recompute, tightness=True, scalability=True): if __name__ == "__main__": - # run_all(n_seeds=1, recompute=True) + run_all(n_seeds=1, recompute=True) # run_mono(n_seeds=1, recompute=True) - run_wahba(n_seeds=1, recompute=True) + # run_wahba(n_seeds=1, recompute=True) diff --git a/_scripts/run_stereo_study.py b/_scripts/run_stereo_study.py index 884b41d..d19edee 100644 --- a/_scripts/run_stereo_study.py +++ b/_scripts/run_stereo_study.py @@ -96,10 +96,10 @@ def stereo_scalability_new(n_seeds, recompute, d=2): learner = Learner(lifter=lifter, variable_list=lifter.variable_list) - # if lifter.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) + if lifter.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) df = run_scalability_new( learner, diff --git a/_scripts/run_time_study.py b/_scripts/run_time_study.py index 8129a27..496a624 100644 --- a/_scripts/run_time_study.py +++ b/_scripts/run_time_study.py @@ -168,8 +168,8 @@ def generate_results(lifter: MatWeightLocLifter, n_params_list=[10], fname=""): n_params_list = np.logspace(1, 6, 6).astype(int) # n_params_list = np.logspace(1, 2, 10).astype(int) - fname = f"_results/{lifter}_time_dsdp.pkl" - overwrite = True + fname = f"_results_laptop/{lifter}_time_dsdp.pkl" + overwrite = False try: assert overwrite is False diff --git a/_test/test_solvers.py b/_test/test_solvers.py index 264f85b..bb2c2cd 100644 --- a/_test/test_solvers.py +++ b/_test/test_solvers.py @@ -1,6 +1,7 @@ import numpy as np from _test.tools import all_lifters +from lifters.matweight_lifter import MatWeightLifter from lifters.mono_lifter import MonoLifter from lifters.poly_lifters import PolyLifter from lifters.robust_pose_lifter import RobustPoseLifter @@ -18,12 +19,12 @@ def test_hess_finite_diff(): eps_list = np.logspace(-10, -5, 5) for eps in eps_list: Q, y = lifter.get_Q(noise=NOISE) - theta = lifter.get_vec_around_gt(delta=0).flatten("C") + theta = lifter.get_vec_around_gt(delta=0) try: grad = lifter.get_grad(theta, y) hess = lifter.get_hess(theta, y).toarray() - except NotImplementedError: + except (NotImplementedError, AttributeError): print("get_hess not implemented?") return @@ -68,7 +69,7 @@ def test_grad_finite_diff(): for eps in eps_list: Q, y = lifter.get_Q(noise=1) - theta = lifter.get_vec_around_gt(delta=0).flatten("C") + theta = lifter.get_vec_around_gt(delta=0) cost = lifter.get_cost(theta, y) try: @@ -136,7 +137,12 @@ def test_cost(noise=0.0): # for Stereo3D problem. assert abs(cost - costQ) < 1e-6, (cost, costQ) - if noise == 0 and not isinstance(lifter, PolyLifter) and not lifter.robust: + if ( + noise == 0 + and not isinstance(lifter, PolyLifter) + and not lifter.robust + and not isinstance(lifter, MatWeightLifter) + ): assert cost < 1e-10, cost assert costQ < 1e-7, costQ elif noise == 0 and isinstance(lifter, MonoLifter): @@ -169,12 +175,19 @@ def test_solvers(n_seeds=1, noise=0.0): continue if noise == 0: # test that solution is ground truth with no noise - if len(theta_hat) == len(theta_gt): - np.testing.assert_allclose(theta_hat, theta_gt) + + if type(theta_gt) is dict: + for i in range(lifter.n_poses): + val_hat = theta_hat[f"xT0_{i}"] + val_gt = theta_gt[f"x_{i}"].matrix() + np.testing.assert_allclose(val_hat, val_gt) else: - # 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) + 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 = get_xtheta_from_theta(theta_gt, lifter.d) + np.testing.assert_allclose(theta_hat, theta_gt) else: # just test that we converged when noise is added @@ -195,15 +208,25 @@ def test_solvers(n_seeds=1, noise=0.0): print(f"{lifter} converged noise {noise}, seed {j}.") cost_lifter = lifter.get_cost(theta_hat, y) - assert abs(cost_solver - cost_lifter) < 1e-10, (cost_solver, cost_lifter) + if cost_lifter >= 1e-10: + assert abs(cost_solver - cost_lifter) / cost_lifter < 1e-5, ( + cost_solver, + cost_lifter, + ) - # 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) + # test that we made progress + if type(theta_hat) is dict: + progress = 0 + for i in range(lifter.n_poses): + val_hat = theta_hat[f"xT0_{i}"] + val_gt = theta_gt[f"x_{i}"].matrix() + progress += np.linalg.norm(val_hat - val_gt) else: - progress = np.linalg.norm(theta_0 - theta_hat) - + 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 if noise == 0: @@ -216,7 +239,18 @@ def test_solvers(n_seeds=1, noise=0.0): if lifter.n_outliers > 0: continue try: - np.testing.assert_allclose(theta_hat, theta_gt, rtol=1e-3) + if type(theta_gt) is dict: + for i in range(lifter.n_poses): + val_hat = theta_hat[f"xT0_{i}"] + val_gt = theta_gt[f"x_{i}"].matrix() + np.testing.assert_allclose(val_hat, val_gt, rtol=1e-3) + else: + if len(theta_hat) == len(theta_gt): + np.testing.assert_allclose(theta_hat, theta_gt, rtol=1e-3) + else: + # 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, rtol=1e-3) except AssertionError as e: print( f"Found solution for {lifter} is not ground truth in zero-noise! is the problem well-conditioned?" @@ -232,7 +266,7 @@ def test_solvers(n_seeds=1, noise=0.0): print( f"minimum eigenvalue at gt: {mineig_hess_gt:.1e} and at estimate: {mineig_hess_hat:.1e}" ) - except NotImplementedError: + except (NotImplementedError, AttributeError): print("implement Hessian for further checks.") print(e) @@ -279,9 +313,7 @@ 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) + error = lifter.get_error(theta_hat)["error"] print( f"{solver} finished in {ttot:.4f}s, final cost {cost_solver:.1e}, error {error:.1e}. \n\tmessage:{msg} " ) diff --git a/_test/test_state_lifter.py b/_test/test_state_lifter.py index 780f5f0..e331540 100644 --- a/_test/test_state_lifter.py +++ b/_test/test_state_lifter.py @@ -1,13 +1,8 @@ import numpy as np +import pytest from _test.tools import all_lifters -from lifters.state_lifter import ( - unravel_multi_index_triu, - ravel_multi_index_triu, - StateLifter, -) - -import pytest +from lifters.state_lifter import ravel_multi_index_triu, unravel_multi_index_triu def pytest_configure(): @@ -88,7 +83,6 @@ 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 AttributeError: @@ -128,7 +122,8 @@ def test_vec_mat(): # pytest.main([__file__, "-s"]) # print("all tests passed") with warnings.catch_warnings(): - warnings.simplefilter("error") + warnings.simplefilter("ignore") + # warnings.simplefilter("error") test_known_constraints() test_learned_constraints() diff --git a/_test/tools.py b/_test/tools.py index 561fc37..795f1d6 100644 --- a/_test/tools.py +++ b/_test/tools.py @@ -1,5 +1,7 @@ import numpy as np +from lifters.matweight_lifter import MatWeightLocLifter +from lifters.mono_lifter import MonoLifter from lifters.poly_lifters import Poly4Lifter, Poly6Lifter, PolyLifter from lifters.range_only_lifters import RangeOnlyLocLifter from lifters.range_only_slam1 import RangeOnlySLAM1Lifter @@ -8,9 +10,7 @@ from lifters.stereo1d_lifter import Stereo1DLifter from lifters.stereo2d_lifter import Stereo2DLifter from lifters.stereo3d_lifter import Stereo3DLifter -from lifters.mono_lifter import MonoLifter from lifters.wahba_lifter import WahbaLifter -from mwcerts.slam_lifter import MWSlamLifter d = 2 n_landmarks = 3 @@ -21,6 +21,7 @@ (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)), + (MatWeightLocLifter, dict(n_landmarks=5, n_poses=10)), (MonoLifter, dict(n_landmarks=6, d=2, robust=True, level="xwT", n_outliers=1)), ( RangeOnlyLocLifter, @@ -35,7 +36,7 @@ (Stereo2DLifter, dict(n_landmarks=n_landmarks)), (Stereo3DLifter, dict(n_landmarks=n_landmarks)), ] -Lifters = [(MWSlamLifter, dict(n_landmarks=5, d=3))] +# Lifters = [(MatWeightLocLifter, dict(n_landmarks=5, n_poses=10))] # Below, we always reset seeds to make sure tests are reproducible. diff --git a/certifiable-tools b/certifiable-tools index fd62782..aac35e4 160000 --- a/certifiable-tools +++ b/certifiable-tools @@ -1 +1 @@ -Subproject commit fd627820c7437aecc44adc5a00efb28c73753252 +Subproject commit aac35e4a0fe57efb9f63121ccaafc5fa512a20ef diff --git a/environment.yml b/environment.yml index a3e1d90..1c40240 100644 --- a/environment.yml +++ b/environment.yml @@ -19,16 +19,15 @@ dependencies: - pytest>=7.2.2 - jupyter>=1.0.0 - black>=23.1.0 - - autograd>=1.2.0 - mosek - tbb=2020.2 # sparseqr conflict - autograd>=1.6.2 - pip: - chompack>=2.3.1 - #- sparseqr - pymanopt>=2.1.1 - asrl-pylgmath>=1.0.3 - -e poly_matrix # build local poly_matrix submodule - -e certifiable-tools + - -e mat_weight_certs - -e . # build local lifting package diff --git a/lifters/base_class.py b/lifters/base_class.py index a7112ae..a0e6284 100644 --- a/lifters/base_class.py +++ b/lifters/base_class.py @@ -44,6 +44,8 @@ def __init__( self.d = d + self.generate_random_setup() + @abstractproperty def var_dict(self): """Return key,size pairs of all variables.""" @@ -71,21 +73,30 @@ def get_Q(self, noise=1e-3, output_poly=False): Warning("get_Q not implemented yet") return None, None - @abstractmethod - def get_error(self, theta_est) -> dict: + def generate_random_setup(self): return def get_A_known(self) -> list: return [] - def sample_parameters(self, x=None) -> list: + def sample_parameters(self, t=None) -> list: if self.param_level == "no": return [1.0] def get_parameters(self, var_subset=None) -> list: + if var_subset is not None: + raise ValueError("var_subset not supported for default get_parameters.") if self.param_level == "no": return [1.0] def get_grad(self, t, y) -> np.ndarray: - Warning("get_grad not implement yet") - return None + raise NotImplementedError("get_grad not implement yet") + + def get_cost(self, theta, y) -> float: + x = self.get_x(theta=theta) + Q, y = self.get_Q() + return x.T @ Q @ x + + def get_error(self, t) -> dict: + err = np.linalg.norm(t - self.theta) ** 2 / self.theta.size + return {"MSE": err, "error": err} diff --git a/lifters/matweight_lifter.py b/lifters/matweight_lifter.py index 20633d5..a5101bc 100644 --- a/lifters/matweight_lifter.py +++ b/lifters/matweight_lifter.py @@ -2,7 +2,7 @@ import spatialmath.base as sm from mwcerts.stereo_problems import LocalizationProblem, SLAMProblem -from constraint_learning.lifters.state_lifter import StateLifter +from lifters.state_lifter import StateLifter class MatWeightLifter(StateLifter): @@ -39,9 +39,6 @@ def var_dict(self): self.var_dict_ = self.prob.var_list return self.var_dict_ - def generate_random_setup(self): - pass - def get_A_known(self, var_dict=None, output_poly=False): if var_dict is None: var_dict = self.var_dict @@ -122,7 +119,7 @@ def theta(self): return self.theta_ def get_x(self, theta=None, parameters=None, var_subset=None): - if parameters is not None: + if (parameters is not None) and (not parameters == [1.0]): raise ValueError("we don't support parameters yet.") if theta is None: theta = self.theta @@ -145,7 +142,15 @@ def get_x(self, theta=None, parameters=None, var_subset=None): t_ki_i = C_i0 @ t_k0_0 - t_i0_i vec += [t_ki_i.flatten()] else: # Other variables - vec += [theta[var].flatten("F")] + try: + vec += [theta[var].flatten("F")] + except KeyError: + T = theta[var.replace("xC", "x").replace("xt", "x")] + if "xC" in var: + vec += [T.C_ba().flatten("F")] + elif "xt" in var: + vec += [T.r_ab_inb().flatten("F")] + return np.hstack(vec) def get_dim_x(self, var_subset=None): @@ -160,34 +165,43 @@ def local_solver(self, t0=None, verbose=False, **kwargs): xhat, info = self.prob.gauss_newton(x_init=t0, verbose=verbose) return xhat, info, info["cost"] + def get_cost(self, theta, y): + Q = self.prob.generate_cost(edges=y) + x = self.get_x(theta=theta) + return x.T @ Q.get_matrix(self.var_dict) @ x + def get_Q(self, noise: float = None, output_poly=False, use_cliques=None): if noise is None: noise = self.NOISE if use_cliques is None: use_cliques = [f"x_{i}" for i in np.arange(self.n_poses)] - if self.Q_poly is None: + if self.y_ is None: self.fill_graph(noise=noise) + self.y_ = self.prob.G.E - self.prob.generate_cost(use_nodes=use_cliques) - self.Q_poly = self.prob.Q + Q = self.prob.generate_cost(edges=self.y_, use_nodes=use_cliques) # make sure that all elements in cost matrix are actually in var_dict! - assert set(self.prob.Q.get_variables()).issubset(self.var_dict.keys()) + assert set(Q.get_variables()).issubset(self.var_dict.keys()) if output_poly: - return self.Q_poly, None + return Q, self.prob.G, self.y_ else: # using a lsit makes sure an error is thrown when a key is not available. - return self.Q_poly.get_matrix(self.var_dict), None + return Q.get_matrix(self.var_dict), self.y_ def get_error(self, theta_hat): - error_dict = {"error_trans": 0, "error_rot": 0} + error_dict = {"error_trans": 0, "error_rot": 0, "error": 0} for key, val in self.theta.items(): if "xt" in key: # translation errors - error_dict["error_trans"] = np.linalg.norm(val - theta_hat[key]) + err = np.linalg.norm(val - theta_hat[key]) + error_dict["error_trans"] += err + error_dict["error"] += err elif "xC" in key: # translation errors - error_dict["error_rot"] = np.linalg.norm(val - theta_hat[key]) + err = np.linalg.norm(val - theta_hat[key]) + error_dict["error_rot"] += err + error_dict["error"] += err return error_dict # clique stuff @@ -285,7 +299,7 @@ def fill_graph(self, noise): from mwcerts.stereo_problems import Camera edges_p2m = self.prob.G.gen_map_edges_full() - c = Camera.get_realistic_model() + c = Camera.get_realistic_model(sigma_u=noise, sigma_v=noise) self.prob.stereo_meas_model(edges_p2m, c=c) # self.prob.gauss_isotrp_meas_model(edges_p2m, sigma=noise) diff --git a/lifters/poly_lifters.py b/lifters/poly_lifters.py index 98a407a..40cc780 100644 --- a/lifters/poly_lifters.py +++ b/lifters/poly_lifters.py @@ -39,7 +39,7 @@ def get_Q_mat(self): return def get_error(self, t): - return {"MAE": float(abs(self.theta - t))} + return {"MAE": float(abs(self.theta - t)), "error": float(abs(self.theta - t))} def get_x(self, theta=None, parameters=None, var_subset=None): if theta is None: diff --git a/lifters/range_only_lifters.py b/lifters/range_only_lifters.py index 7cfc934..fa8e5cc 100644 --- a/lifters/range_only_lifters.py +++ b/lifters/range_only_lifters.py @@ -1,13 +1,13 @@ import matplotlib.pylab as plt import numpy as np -from scipy.optimize import minimize import scipy.sparse as sp - -plt.ion() +from scipy.optimize import minimize from lifters.state_lifter import StateLifter from poly_matrix.least_squares_problem import LeastSquaresProblem +plt.ion() + NOISE = 1e-2 # std deviation of distance noise METHOD = "BFGS" @@ -408,7 +408,8 @@ def get_position(self, theta=None, xtheta=None): return xtheta.reshape(self.n_positions, self.d) def get_error(self, that): - return {"total error": np.sqrt(np.mean((self.theta - that) ** 2))} + err = np.sqrt(np.mean((self.theta - that) ** 2)) + return {"total error": err, "error": err} def local_solver( self, diff --git a/lifters/robust_pose_lifter.py b/lifters/robust_pose_lifter.py index f65f3a1..7187a9b 100644 --- a/lifters/robust_pose_lifter.py +++ b/lifters/robust_pose_lifter.py @@ -299,12 +299,6 @@ def get_cost(self, theta, y): cost += res return 0.5 * cost - def get_grad(self, t, y): - raise NotImplementedError("get_grad not implement yet") - - def get_hess(self, t, y): - raise NotImplementedError("get_hess not implement yet") - def local_solver( self, t0, y, verbose=False, method=METHOD, solver_kwargs=SOLVER_KWARGS ): diff --git a/lifters/stereo1d_lifter.py b/lifters/stereo1d_lifter.py index a337418..174d1fc 100644 --- a/lifters/stereo1d_lifter.py +++ b/lifters/stereo1d_lifter.py @@ -119,9 +119,6 @@ def get_param_idx_dict(self, var_subset=None): param_dict_[f"p_{n}"] = n + 1 return param_dict_ - 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 diff --git a/lifters/stereo_lifter.py b/lifters/stereo_lifter.py index 5393309..15eb2b3 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 diff --git a/mat_weight_certs b/mat_weight_certs index 5970009..7f0fae3 160000 --- a/mat_weight_certs +++ b/mat_weight_certs @@ -1 +1 @@ -Subproject commit 5970009f40543ea95e572679997bdc8c33236d7f +Subproject commit 7f0fae30fd7df6d46f652868d60912551d647860 diff --git a/requirements.txt b/requirements.txt index 7d12188..310f452 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,8 +11,9 @@ autograd>=1.6.2 mosek pymanopt>=2.1.1 chompack>=2.3.1 -sparseqr +sparseqr>=1.2.1 asrl-pylgmath>=1.0.3 -e poly_matrix/ -e certifiable-tools/ +-e mat_weight_certs/ -e . diff --git a/utils/geometry.py b/utils/geometry.py index d1ad542..e0e00ae 100644 --- a/utils/geometry.py +++ b/utils/geometry.py @@ -1,5 +1,4 @@ import numpy as np - from scipy.spatial.transform import Rotation as R @@ -99,6 +98,7 @@ def get_pose_errors_from_xtheta(xtheta_hat, xtheta_gt, d): r_error = np.linalg.norm(r_hat - r_gt) C_error = np.linalg.norm(C_gt.T @ C_hat - np.eye(d)) return { + "error": r_error + C_error, "r error": r_error, "C error": C_error, "total error": r_error + C_error,