From 912e9b2616014de01967cfa495e2b78530066cd6 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 26 May 2025 18:53:27 -0400 Subject: [PATCH 01/10] Create rotation averaging example --- popr/examples/rotation_lifter.py | 53 ++++++++++----- popr/utils/plotting_tools.py | 29 +++++++-- scripts/rotation_averaging.ipynb | 107 +++++++++++++++++++++++++++++++ 3 files changed, 165 insertions(+), 24 deletions(-) create mode 100644 scripts/rotation_averaging.ipynb diff --git a/popr/examples/rotation_lifter.py b/popr/examples/rotation_lifter.py index be4367a..14697ba 100644 --- a/popr/examples/rotation_lifter.py +++ b/popr/examples/rotation_lifter.py @@ -70,26 +70,30 @@ def get_theta(self, x: np.ndarray) -> np.ndarray: C_flat = x[1 : 1 + self.d**2] return C_flat.reshape((self.d, self.d)) - def get_Q(self, noise: float | None = None): + def simulate_y(self, noise: float | None = None): if noise is None: noise = self.NOISE - if self.y_ is None: - self.y_ = [] - for i in range(self.n_meas): - # noise model: R_i = R.T @ Rnoise - if noise > 0: - # Generate a random small rotation as noise and apply it - noise_rotvec = np.random.normal(scale=noise, size=(self.d,)) - Rnoise = ( - R.from_rotvec(noise_rotvec).as_matrix() - if self.d == 3 - else R.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] - ) - Ri = self.theta.T @ Rnoise - else: - Ri = self.theta.T - self.y_.append(Ri) + self.y_ = [] + for i in range(self.n_meas): + # noise model: R_i = R.T @ Rnoise + if noise > 0: + # Generate a random small rotation as noise and apply it + noise_rotvec = np.random.normal(scale=noise, size=(self.d,)) + Rnoise = ( + R.from_rotvec(noise_rotvec).as_matrix() + if self.d == 3 + else R.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] + ) + Ri = self.theta.T @ Rnoise + else: + Ri = self.theta.T + self.y_.append(Ri) + return self.y_ + + def get_Q(self, noise: float | None = None): + if self.y_ is None: + self.y = self.simulate_y(noise=noise) return self.get_Q_from_y(self.y_) def get_Q_from_y(self, y, output_poly=False): @@ -217,3 +221,18 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): "Warning: consider implementing the determinant constraint for RobustPoseLifter, d=3" ) return A_list + + def plot(self, estimates={}): + import matplotlib.pyplot as plt + + from popr.utils.plotting_tools import plot_frame + + fig, ax = plt.subplots() + plot_frame(ax=ax, theta=self.theta, label="gt", ls="-", scale=0.5) + + for label, theta in estimates.items(): + plot_frame(ax=ax, theta=theta, label=label, ls="--", scale=1.0) + + ax.set_aspect("equal") + ax.legend() + return fig, ax diff --git a/popr/utils/plotting_tools.py b/popr/utils/plotting_tools.py index b3b79f0..d0040d1 100644 --- a/popr/utils/plotting_tools.py +++ b/popr/utils/plotting_tools.py @@ -105,7 +105,7 @@ def savefig(fig, name, verbose=True): def plot_frame( ax, - theta=None, + theta, color="k", marker="+", label=None, @@ -115,12 +115,17 @@ def plot_frame( d=3, **kwargs, ): - try: - C_cw, r_wc_c = get_C_r_from_theta(theta, d=d) - r_wc_w = -C_cw.T @ r_wc_c # r_wc_w - except: - C_cw = None - r_wc_w = theta + if np.ndim(theta) == 2: + # used by rotation averaging + C_cw = theta + r_wc_w = np.zeros((theta.shape[0])) + else: + try: + C_cw, r_wc_c = get_C_r_from_theta(theta, d=d) + r_wc_w = -C_cw.T @ r_wc_c # r_wc_w + except Exception as e: + C_cw = None + r_wc_w = theta assert r_wc_w is not None @@ -139,8 +144,18 @@ def plot_frame( *r_wc_w[:2].T, color=color, marker=marker, + label=None, + zorder=1, + **kwargs, + ) + ax.plot( + [], + [], + marker=marker, label=label, zorder=1, + ls=ls, + color="k", **kwargs, ) return r_wc_w, C_cw diff --git a/scripts/rotation_averaging.ipynb b/scripts/rotation_averaging.ipynb new file mode 100644 index 0000000..48c1a1a --- /dev/null +++ b/scripts/rotation_averaging.ipynb @@ -0,0 +1,107 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "b56347d1", + "metadata": {}, + "source": [ + "# Rotation averaging example\n", + "\n", + "This example script demonstrates how to solve rotation averaging problems using AutoTight." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3c8c3215", + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "\n", + "from popr.examples import RotationLifter\n", + "\n", + "lifter = RotationLifter(d=3, n_meas=3)\n", + "\n", + "# solve the rotation averaging problem locally.\n", + "y = lifter.simulate_y(noise=1e-1)\n", + "\n", + "theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False)\n", + "estimates = {\"init gt\": theta_gt}\n", + "for i in range(10):\n", + " theta_init = lifter.sample_theta()\n", + " theta_i, *_ = lifter.local_solver(theta_init, y, verbose=False)\n", + " estimates[f\"init random {i}\"] = theta_i\n", + "\n", + "fig, ax = lifter.plot(estimates=estimates)\n", + "ax.legend([])\n", + "plt.show(block=False)\n", + "print(\"done\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "64bbdf6b", + "metadata": {}, + "outputs": [], + "source": [ + "# solve the rotation averaging with a naive SDP.\n", + "from popr.utils.plotting_tools import plot_matrix\n", + "\n", + "Q = lifter.get_Q()\n", + "A_known = lifter.get_A_known()\n", + "constraints = lifter.get_A_b_list(lifter.get_A_known())\n", + "\n", + "fig, axs = plt.subplots(1, len(A_known) + 1)\n", + "fig.set_size_inches(3*(len(A_known) + 1), 3)\n", + "for i in range(len(A_known)):\n", + " plot_matrix(A_known[i].toarray(), ax=axs[i], title=f\"A{i} \", colorbar=False)\n", + "plot_matrix(Q.toarray(), ax=axs[i+1], title=\"Q\", colorbar=False)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "be73dc95", + "metadata": {}, + "outputs": [], + "source": [ + "from cert_tools.sdp_solvers import solve_sdp\n", + "from cert_tools.linalg_tools import rank_project\n", + "\n", + "X, info = solve_sdp(Q, constraints, verbose=False)\n", + "\n", + "x, info_rank = rank_project(X, p=1)\n", + "print(f\"EVR: {info_rank['EVR']:.2e}\")\n", + "\n", + "theta_opt = lifter.get_theta(x.flatten())\n", + "\n", + "estimates = {\"init gt\": theta_gt, \"SDP\": theta_opt}\n", + "fig, ax = lifter.plot(estimates=estimates)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "popr", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.16" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From 7615eb46206336b00d87878c0533de5469612814 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Wed, 28 May 2025 12:57:59 -0400 Subject: [PATCH 02/10] Export overview --- docs/source/_static/overview.svg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/source/_static/overview.svg b/docs/source/_static/overview.svg index 453213c..8d71623 100644 --- a/docs/source/_static/overview.svg +++ b/docs/source/_static/overview.svg @@ -61,9 +61,9 @@ id="page14" margin="0" bleed="0" - inkscape:export-filename="poster-new.pdf" - inkscape:export-xdpi="96" - inkscape:export-ydpi="96" /> Date: Tue, 3 Jun 2025 22:23:56 +0200 Subject: [PATCH 03/10] Finalize rotation averaging example --- popcor/base_lifters/state_lifter.py | 13 ++++-- popcor/examples/rotation_lifter.py | 22 +++++++---- scripts/rotation_averaging.ipynb | 61 +++++++++++++++++++++++------ tests/test_state_lifter.py | 6 ++- 4 files changed, 79 insertions(+), 23 deletions(-) diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index c5995a5..28a05b0 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -160,7 +160,13 @@ def get_cost(self, theta, y: np.ndarray | None = None) -> float: Q = self.get_Q() return float(x.T @ Q @ x) - def local_solver(self, t0, y: np.ndarray | None = None, *args, **kwargs): + def local_solver( + self, + t0, + y: np.ndarray | list | None = None, + *args, + **kwargs, + ): """ Default local solver that uses IPOPT to solve the QCQP problem defined by Q and the constraints matrices. Consider overwriting this for more efficient solvers. @@ -168,7 +174,8 @@ def local_solver(self, t0, y: np.ndarray | None = None, *args, **kwargs): print( "Warning: using default local_solver, which may be less efficient than a custom one." ) - print("Ignoring args and kwargs:", args, kwargs) + if len(args): + print(f"Warning: ignore args {args}") from cert_tools.sdp_solvers import solve_low_rank_sdp if y is not None: @@ -185,7 +192,7 @@ def local_solver(self, t0, y: np.ndarray | None = None, *args, **kwargs): Constraints = self.get_A_b_list(A_list=self.get_A_known()) x0 = self.get_x(theta=t0) X, info = solve_low_rank_sdp( - Q, Constraints=Constraints, rank=1, verbose=True, x_cand=x0 + Q, Constraints=Constraints, rank=1, x_cand=x0, **kwargs ) # TODO(FD) identify when the solve is not successful. info["success"] = True diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 7781412..9df3d14 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -67,10 +67,10 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: def get_theta(self, x: np.ndarray) -> np.ndarray: assert np.ndim(x) == 1 - C_flat = x[1 : 1 + self.d**2] + C_flat = x[: self.d**2] return C_flat.reshape((self.d, self.d)) - def simulate_y(self, noise: float | None = None) =-> list: + def simulate_y(self, noise: float | None = None) -> list: if noise is None: noise = self.NOISE @@ -248,22 +248,30 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): self.test_and_add(A_list, Ai, output_poly=output_poly) return A_list - def plot(self, estimates={}): + import itertools + import matplotlib.pyplot as plt - from popr.utils.plotting_tools import plot_frame + from popcor.utils.plotting_tools import plot_frame fig, ax = plt.subplots() - plot_frame(ax=ax, theta=self.theta, label="gt", ls="-", scale=0.5) + plot_frame(ax=ax, theta=self.theta, label="gt", ls="-", scale=0.5, marker="") + linestyles = itertools.cycle(["--", "-.", ":"]) for label, theta in estimates.items(): - plot_frame(ax=ax, theta=theta, label=label, ls="--", scale=1.0) + plot_frame( + ax=ax, + theta=theta, + label=label, + ls=next(linestyles), + scale=1.0, + marker="", + ) ax.set_aspect("equal") ax.legend() return fig, ax - def __repr__(self): return f"rotation_lifter{self.d}d" diff --git a/scripts/rotation_averaging.ipynb b/scripts/rotation_averaging.ipynb index 48c1a1a..dcd99d1 100644 --- a/scripts/rotation_averaging.ipynb +++ b/scripts/rotation_averaging.ipynb @@ -10,6 +10,27 @@ "This example script demonstrates how to solve rotation averaging problems using AutoTight." ] }, + { + "cell_type": "code", + "execution_count": null, + "id": "ed5be9cc-ad44-421a-b852-d44fc37c42ed", + "metadata": {}, + "outputs": [], + "source": [ + "%reload_ext autoreload\n", + "%autoreload 2" + ] + }, + { + "cell_type": "markdown", + "id": "e258188b", + "metadata": {}, + "source": [ + "## Setup a new problem and solve it locally\n", + "\n", + "We initialize either from ground truth or from randomly sampled rotations " + ] + }, { "cell_type": "code", "execution_count": null, @@ -20,12 +41,12 @@ "import matplotlib.pyplot as plt\n", "import numpy as np\n", "\n", - "from popr.examples import RotationLifter\n", + "from popcor.examples import RotationLifter\n", "\n", + "np.random.seed(2)\n", "lifter = RotationLifter(d=3, n_meas=3)\n", "\n", - "# solve the rotation averaging problem locally.\n", - "y = lifter.simulate_y(noise=1e-1)\n", + "y = lifter.simulate_y(noise=0.2)\n", "\n", "theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False)\n", "estimates = {\"init gt\": theta_gt}\n", @@ -36,8 +57,15 @@ "\n", "fig, ax = lifter.plot(estimates=estimates)\n", "ax.legend([])\n", - "plt.show(block=False)\n", - "print(\"done\")" + "plt.show(block=False)" + ] + }, + { + "cell_type": "markdown", + "id": "da3c8b79-8d8e-497c-9761-21b678450217", + "metadata": {}, + "source": [ + "## Solve the rotation averaging with an SDP" ] }, { @@ -47,10 +75,9 @@ "metadata": {}, "outputs": [], "source": [ - "# solve the rotation averaging with a naive SDP.\n", - "from popr.utils.plotting_tools import plot_matrix\n", + "from popcor.utils.plotting_tools import plot_matrix\n", "\n", - "Q = lifter.get_Q()\n", + "Q = lifter.get_Q_from_y(y=y)\n", "A_known = lifter.get_A_known()\n", "constraints = lifter.get_A_b_list(lifter.get_A_known())\n", "\n", @@ -58,7 +85,7 @@ "fig.set_size_inches(3*(len(A_known) + 1), 3)\n", "for i in range(len(A_known)):\n", " plot_matrix(A_known[i].toarray(), ax=axs[i], title=f\"A{i} \", colorbar=False)\n", - "plot_matrix(Q.toarray(), ax=axs[i+1], title=\"Q\", colorbar=False)" + "fig = plot_matrix(Q.toarray(), ax=axs[i+1], title=\"Q\", colorbar=False)" ] }, { @@ -76,16 +103,26 @@ "x, info_rank = rank_project(X, p=1)\n", "print(f\"EVR: {info_rank['EVR']:.2e}\")\n", "\n", - "theta_opt = lifter.get_theta(x.flatten())\n", + "theta_opt = lifter.get_theta(x.flatten()[1:])\n", "\n", "estimates = {\"init gt\": theta_gt, \"SDP\": theta_opt}\n", "fig, ax = lifter.plot(estimates=estimates)" ] + }, + { + "cell_type": "markdown", + "id": "b7c84433-d923-403b-92a1-bd5ab3e0ce92", + "metadata": {}, + "source": [ + "## Conclusion\n", + "\n", + "This problem is too easy! No redundant measurements are required for tightness. " + ] } ], "metadata": { "kernelspec": { - "display_name": "popr", + "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, @@ -99,7 +136,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.16" + "version": "3.10.17" } }, "nbformat": 4, diff --git a/tests/test_state_lifter.py b/tests/test_state_lifter.py index 8af7f35..b005d33 100644 --- a/tests/test_state_lifter.py +++ b/tests/test_state_lifter.py @@ -4,7 +4,11 @@ from popcor.base_lifters import StateLifter, StereoLifter from popcor.examples import Stereo2DLifter, Stereo3DLifter -from popcor.utils.common import get_vec, ravel_multi_index_triu, unravel_multi_index_triu +from popcor.utils.common import ( + get_vec, + ravel_multi_index_triu, + unravel_multi_index_triu, +) from popcor.utils.test_tools import all_lifters, constraints_test_with_tol From 5fbe77681abf43d2e723109b0ab04d727a756138 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 10:21:44 +0200 Subject: [PATCH 04/10] Fix links in contributing instructions --- CONTRIBUTING.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 5475969..1ab723a 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -13,7 +13,7 @@ Please try to the best of your abilities to: ## Adding a new lifter class -You can start with the [ExampleLifter](../popr/examples/ExampleLifter.py) skeleton, and feel free to add more functionalities depending on the nature of the problem. You can also consider adding a new base class similar to [RobustPoseLifter](../popr/base_lifters/RobustPoseLifter.py) or [StereoLifter](../popr/base_lifters/StereoLifter.py) if you want to create multiple new lifters that all share similar functionalities. +You can start with the [ExampleLifter](../popcor/examples/ExampleLifter.py) skeleton, and feel free to add more functionalities depending on the nature of the problem. You can also consider adding a new base class similar to [RobustPoseLifter](../popcor/base_lifters/RobustPoseLifter.py) or [StereoLifter](../popcor/base_lifters/StereoLifter.py) if you want to create multiple new lifters that all share similar functionalities. ## Adding new functionalities @@ -37,7 +37,7 @@ to make sure there are no errors. There is also the possibility to just use lite This has already been done -- just keeping track of the process here to make it is easy to redo this in the future. TAken from [here](https://docs.mosek.com/11.0/faq.pdf), page 11. -1. Go to Settings -> Security -> Secrets and variables -> Actions ([direct link](https://github.com/duembgen/popr/settings/secrets/actions)) +1. Go to Settings -> Security -> Secrets and variables -> Actions ([direct link](https://github.com/duembgen/popcor/settings/secrets/actions)) 2. Create secret called MSK_LICENSE with content ``` From 94e7620b8ba1cb77366eefd271de2bb25be54027 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 10:23:31 +0200 Subject: [PATCH 05/10] Remove obsolete badge --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 54265de..f5fbd7d 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,5 @@ [![Python Package using Conda](https://github.com/duembgen/popr/actions/workflows/python-package-conda.yml/badge.svg)](https://github.com/duembgen/popr/actions/workflows/python-package-conda.yml) [![Documentation build](https://github.com/duembgen/popr/actions/workflows/documentation.yml/badge.svg)](https://duembgen.github.io/popcor) -[![Documentation deploy](https://github.com/duembgen/popcor/actions/workflows/static.yml/badge.svg)](https://duembgen.github.io/popcor) ![](./docs/source/_static/overview.png) From 398401892fe92c836a2eb1af1acbfc81671b9f9a Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 11:04:10 +0200 Subject: [PATCH 06/10] Create get_valid_samples in StateLifter --- popcor/base_lifters/range_only_lifter.py | 38 ++++++++++++++--------- popcor/base_lifters/robust_pose_lifter.py | 4 ++- popcor/base_lifters/state_lifter.py | 4 +++ popcor/examples/ro_nsq_lifter.py | 32 +++---------------- 4 files changed, 35 insertions(+), 43 deletions(-) diff --git a/popcor/base_lifters/range_only_lifter.py b/popcor/base_lifters/range_only_lifter.py index e8634ca..03466fd 100644 --- a/popcor/base_lifters/range_only_lifter.py +++ b/popcor/base_lifters/range_only_lifter.py @@ -8,8 +8,6 @@ from .state_lifter import StateLifter -NOISE = 1e-2 # std deviation of distance noise - METHOD = "BFGS" NORMALIZE = True @@ -21,8 +19,8 @@ "TNC": dict(gtol=1e-6, xtol=1e-10), } -# size of the region of intereist: [0, SCALE]^d -SCALE = 2.0 + +MAX_TRIALS = 10 # number of trials to find a valid sample class RangeOnlyLifter(StateLifter, ABC): @@ -35,6 +33,10 @@ class RangeOnlyLifter(StateLifter, ABC): for concrete implementations. """ + NOISE = 1e-2 # std deviation of distance noise + SCALE = 2.0 # size of the region of intereist: [0, SCALE]^d + MIN_DIST = 1e-2 # minimum distance between landmarks and positions + def __init__( self, n_positions, @@ -76,13 +78,13 @@ def create_bad(n_positions, n_landmarks, d=2): # create landmarks that are roughly in a subspace of dimension d-1 landmarks = np.hstack( # type: ignore [ - np.random.rand(n_landmarks, d - 1) * SCALE, - np.random.rand(n_landmarks, 1) * 0.3 + SCALE / 2.0, + np.random.rand(n_landmarks, d - 1) * RangeOnlyLifter.SCALE, + np.random.rand(n_landmarks, 1) * 0.3 + RangeOnlyLifter.SCALE / 2.0, ] ) theta = np.hstack( [ - np.random.rand(n_positions, d - 1) * SCALE, + np.random.rand(n_positions, d - 1) * RangeOnlyLifter.SCALE, np.max(landmarks[:, -1]) + np.random.rand(n_positions, 1), ] ) @@ -95,7 +97,9 @@ def create_good(n_positions, n_landmarks, d=2): np.max(landmarks, axis=0) - np.min(landmarks, axis=0) ) # remove landmarks a bit from border for plotting reasons - landmarks = (landmarks + SCALE * 0.05) * SCALE * 0.9 + landmarks = ( + (landmarks + RangeOnlyLifter.SCALE * 0.05) * RangeOnlyLifter.SCALE * 0.9 + ) theta = np.random.uniform( [np.min(landmarks, axis=0)], [np.max(landmarks, axis=0)] ) @@ -158,7 +162,17 @@ def overwrite_theta(self, theta): self.theta_ = theta def sample_theta(self): - return np.random.rand(self.n_positions, self.d).flatten() + """Make sure we do not sample too close to landmarks (if distance is zero we get numerical problems).""" + for _ in range(MAX_TRIALS): + sample = ( + (np.random.rand(self.landmarks.shape[1]) - 0.5) * 2 * self.SCALE + ) # between 0 and SCALE + if np.all( + np.linalg.norm(sample[None, :] - self.landmarks, axis=1) > self.MIN_DIST + ): + return sample.reshape((1, self.d)) + print(f"Warning: did not find valid sample in {MAX_TRIALS} trials") + return sample def get_residuals(self, t, y, squared=True, ad=False): positions = t.reshape((-1, self.d)) @@ -203,7 +217,7 @@ def simulate_y(self, noise: float | None = None, squared: bool = True): assert self.landmarks is not None # N x K matrix if noise is None: - noise = NOISE + noise = self.NOISE positions = self.theta.reshape(self.n_positions, -1) y_gt = np.linalg.norm( self.landmarks[None, :, :] - positions[:, None, :], axis=2 @@ -353,10 +367,6 @@ def plot(self, y=None, xlims=[0, 2], ylims=[0, 2], ax=None): ax.set_aspect("equal") return fig, ax, im - def get_valid_samples(self, n_samples): - samples = [self.sample_theta() for _ in range(n_samples)] - return np.vstack(samples) - @property @abstractmethod def size_z(self) -> int: diff --git a/popcor/base_lifters/robust_pose_lifter.py b/popcor/base_lifters/robust_pose_lifter.py index dbce8b3..2ca2234 100644 --- a/popcor/base_lifters/robust_pose_lifter.py +++ b/popcor/base_lifters/robust_pose_lifter.py @@ -271,6 +271,7 @@ def get_vec_around_gt(self, delta: float = 0): return theta_noisy def get_cost(self, theta, y): + assert y is not None if self.robust: x = theta[: -self.n_landmarks] w = theta[-self.n_landmarks :] @@ -292,11 +293,12 @@ def get_cost(self, theta, y): def local_solver( self, t0, - y: np.ndarray | None, + y: np.ndarray, verbose=False, method=METHOD, solver_kwargs=SOLVER_KWARGS, ): + assert y is not None import pymanopt from pymanopt.manifolds import Euclidean, Product, SpecialOrthogonalGroup diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index 8fddf45..723cefb 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -235,3 +235,7 @@ def get_theta(self, x): "Please make sure that you use get_theta as intended." ) return x.flatten()[: self.d] + + def get_valid_samples(self, n_samples): + samples = [self.sample_theta() for _ in range(n_samples)] + return np.vstack(samples) diff --git a/popcor/examples/ro_nsq_lifter.py b/popcor/examples/ro_nsq_lifter.py index 65f44fe..3818220 100644 --- a/popcor/examples/ro_nsq_lifter.py +++ b/popcor/examples/ro_nsq_lifter.py @@ -4,10 +4,6 @@ from popcor.base_lifters import RangeOnlyLifter -NOISE = 1e-2 # std deviation of distance noise - -NORMALIZE = True - class RangeOnlyNsqLifter(RangeOnlyLifter): """Range-only localization in 2D or 3D. @@ -45,6 +41,8 @@ class RangeOnlyNsqLifter(RangeOnlyLifter): } MONOMIAL_DEGREE = 1 + SCALE = 1.0 + @staticmethod def create_good(n_positions, n_landmarks, d=2): landmarks, theta = RangeOnlyLifter.create_good(n_positions, n_landmarks, d) @@ -244,32 +242,10 @@ def size_z(self) -> int: def get_valid_samples( self, n_samples, - max_trials=3, - min_dist=1e-2, - radius=1.0, - center=None, - vectorized=True, ): - # quick and dirty implementation to make sure we don't sample too close - # from a landmark (otherwise length is zero and the normal vector will have nans) - if center is None: - center = np.ones(self.landmarks.shape[1]) - samples = [] - for i in range(n_samples): - for j in range(max_trials): - sample = ( - (np.random.rand(self.landmarks.shape[1]) - 0.5) * 2 * radius - ) + center # between 0 and 1 - if np.all( - np.linalg.norm(sample[None, :] - self.landmarks, axis=1) > min_dist - ): - samples.append(sample) - break - if j == max_trials - 1: - print(f"Warning: did not find valid sample in {max_trials} trials") - - samples = np.vstack(samples) + samples = super().get_valid_samples(n_samples) + # TODO(FD): maybe this should be moved to theta. normals = self.landmarks[None, :, :] - samples[:, None, :] normals /= np.linalg.norm(normals, axis=2)[:, :, None] return np.hstack([samples, normals.reshape(normals.shape[0], -1)]) From 11afa337d8fe87b8d91c5fdfac37efa4c1578341 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 12:07:52 +0200 Subject: [PATCH 07/10] Fix landmark sampling, add estimates to plot --- popcor/base_lifters/range_only_lifter.py | 63 +++++++++++++++++------- 1 file changed, 44 insertions(+), 19 deletions(-) diff --git a/popcor/base_lifters/range_only_lifter.py b/popcor/base_lifters/range_only_lifter.py index 03466fd..b9768db 100644 --- a/popcor/base_lifters/range_only_lifter.py +++ b/popcor/base_lifters/range_only_lifter.py @@ -92,6 +92,16 @@ def create_bad(n_positions, n_landmarks, d=2): @staticmethod def create_good(n_positions, n_landmarks, d=2): + landmarks = RangeOnlyLifter.sample_landmarks_filling_space(n_landmarks, d) + theta = np.random.uniform( + [np.min(landmarks, axis=0)], + [np.max(landmarks, axis=0)], + size=(n_positions, d), + ) + return landmarks, theta + + @staticmethod + def sample_landmarks_filling_space(n_landmarks, d=2): landmarks = np.random.rand(n_landmarks, d) landmarks = (landmarks - np.min(landmarks, axis=0)) / ( np.max(landmarks, axis=0) - np.min(landmarks, axis=0) @@ -100,14 +110,13 @@ def create_good(n_positions, n_landmarks, d=2): landmarks = ( (landmarks + RangeOnlyLifter.SCALE * 0.05) * RangeOnlyLifter.SCALE * 0.9 ) - theta = np.random.uniform( - [np.min(landmarks, axis=0)], [np.max(landmarks, axis=0)] - ) - return landmarks, theta + return landmarks @property def landmarks(self): - landmarks = np.random.rand(self.n_landmarks, self.d) + landmarks = RangeOnlyLifter.sample_landmarks_filling_space( + self.n_landmarks, self.d + ) if self.landmarks_ is None: self.landmarks_ = landmarks return self.landmarks_ @@ -163,16 +172,23 @@ def overwrite_theta(self, theta): def sample_theta(self): """Make sure we do not sample too close to landmarks (if distance is zero we get numerical problems).""" - for _ in range(MAX_TRIALS): - sample = ( - (np.random.rand(self.landmarks.shape[1]) - 0.5) * 2 * self.SCALE - ) # between 0 and SCALE - if np.all( - np.linalg.norm(sample[None, :] - self.landmarks, axis=1) > self.MIN_DIST - ): - return sample.reshape((1, self.d)) - print(f"Warning: did not find valid sample in {MAX_TRIALS} trials") - return sample + samples = np.empty((self.n_positions, self.d)) + for i in range(self.n_positions): + for j in range(MAX_TRIALS): + sample = ( + np.random.rand(self.landmarks.shape[1]) + ) * self.SCALE # between 0 and SCALE + if np.all( + np.linalg.norm(sample[None, :] - self.landmarks, axis=1) + > self.MIN_DIST + ): + samples[i, :] = sample + elif j == MAX_TRIALS - 1: + print( + f"Warning: did not find valid sample in {MAX_TRIALS} trials. Using last sample." + ) + samples[i, :] = sample + return samples.flatten("C") def get_residuals(self, t, y, squared=True, ad=False): positions = t.reshape((-1, self.d)) @@ -260,10 +276,12 @@ def get_theta(self, x): return x[: self.n_positions * self.d] def get_error(self, theta_hat, error_type="rmse", *args, **kwargs): + assert np.ndim(theta_hat) <= 1 + theta_gt = self.theta if np.ndim(self.theta) <= 1 else self.theta.flatten() if error_type == "rmse": - return np.sqrt(np.mean((self.theta - theta_hat) ** 2)) + return np.sqrt(np.mean((theta_gt - theta_hat) ** 2)) elif error_type == "mse": - return np.mean((self.theta - theta_hat) ** 2) + return np.mean((theta_gt - theta_hat) ** 2) else: raise ValueError(f"Unkwnon error_type {error_type}") @@ -336,15 +354,22 @@ def fun(x): info["cost"] = cost return that, info, cost - def plot(self, y=None, xlims=[0, 2], ylims=[0, 2], ax=None): + def plot(self, y=None, xlims=[0, 2], ylims=[0, 2], ax=None, estimates={}): if ax is None: fig, ax = plt.subplots() fig.set_size_inches(5, 5) else: fig = plt.gcf() + if np.ndim(self.theta) < 2: + theta_gt = self.theta.reshape((self.n_positions, self.d)) + ax.scatter(*self.landmarks[:, :2].T, color="k", marker="+", label="landmarks") - ax.scatter(*self.theta[:, :2].T, color="C0", marker="o") + ax.scatter(*theta_gt[:, :2].T, color="C0", marker="o") + for label, theta_est in estimates.items(): + if np.ndim(theta_est) < 2: + theta_est = theta_est.reshape((self.n_positions, self.d)) + ax.scatter(*theta_est[:, :2].T, marker="x", label=label) im = None if y is not None: From 41bb50d451c18fdf4c6bf68960180e04ce9991c7 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 12:08:24 +0200 Subject: [PATCH 08/10] Fix kwargs error when using default solver, add Warning --- popcor/base_lifters/state_lifter.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index 723cefb..df42d6a 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -189,6 +189,11 @@ def local_solver( "Inequality constraints are not currently considered by default solver. Must implement your own." ) + if method := kwargs.pop("method", None) is not None: + print( + f"Warning: ignoreing method argument {method} in local_solver, using default (IPOPT)." + ) + Constraints = self.get_A_b_list(A_list=self.get_A_known()) x0 = self.get_x(theta=t0) X, info = solve_low_rank_sdp( @@ -237,5 +242,5 @@ def get_theta(self, x): return x.flatten()[: self.d] def get_valid_samples(self, n_samples): - samples = [self.sample_theta() for _ in range(n_samples)] + samples = [self.sample_theta().flatten() for _ in range(n_samples)] return np.vstack(samples) From c29fda792849531b63026f4399e1b586a08b371f Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 12:09:14 +0200 Subject: [PATCH 09/10] Make default number of landmarks 4 to avoid ambiguities --- popcor/utils/test_tools.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/popcor/utils/test_tools.py b/popcor/utils/test_tools.py index c2ddfe5..0c80508 100644 --- a/popcor/utils/test_tools.py +++ b/popcor/utils/test_tools.py @@ -41,11 +41,11 @@ ), # not tight ( RangeOnlySqLifter, - dict(n_positions=n_positions, n_landmarks=n_landmarks, d=d, level="no"), + dict(n_positions=n_positions, n_landmarks=4, d=d, level="no"), ), # ok ( RangeOnlySqLifter, - dict(n_positions=n_positions, n_landmarks=n_landmarks, d=d, level="quad"), + dict(n_positions=n_positions, n_landmarks=4, d=d, level="quad"), ), # ok (Stereo1DLifter, dict(n_landmarks=n_landmarks)), # not tight (Stereo1DLifter, dict(n_landmarks=n_landmarks, param_level="p")), # skip From 2cbdb7beabf4f560bddcc9b178ef9dad34e186fa Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 5 Jun 2025 12:09:37 +0200 Subject: [PATCH 10/10] Minor changes --- popcor/base_lifters/robust_pose_lifter.py | 2 +- tests/test_solvers.py | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/popcor/base_lifters/robust_pose_lifter.py b/popcor/base_lifters/robust_pose_lifter.py index 2ca2234..6ae0c5c 100644 --- a/popcor/base_lifters/robust_pose_lifter.py +++ b/popcor/base_lifters/robust_pose_lifter.py @@ -293,7 +293,7 @@ def get_cost(self, theta, y): def local_solver( self, t0, - y: np.ndarray, + y: np.ndarray | None, verbose=False, method=METHOD, solver_kwargs=SOLVER_KWARGS, diff --git a/tests/test_solvers.py b/tests/test_solvers.py index 0cda1bb..19ee91c 100644 --- a/tests/test_solvers.py +++ b/tests/test_solvers.py @@ -193,7 +193,7 @@ 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) np.testing.assert_allclose(theta_hat, theta_gt) else: @@ -245,13 +245,18 @@ def test_solvers(n_seeds=1, noise=0.0): print( f"Found solution for {lifter} is not ground truth in zero-noise! is the problem well-conditioned?" ) + fig, ax, im = lifter.plot(estimates={"estimate": theta_hat}) + import matplotlib.pylab as plt + + plt.show(block=False) + ax.legend() try: mineig_hess_hat = np.linalg.eigvalsh( - lifter.get_hess(theta_hat, y) + lifter.get_hess(theta_hat, y).toarray() )[0] mineig_hess_gt = np.linalg.eigvalsh( - lifter.get_hess(theta_gt, y) + lifter.get_hess(theta_gt, y).toarray() )[0] print( f"minimum eigenvalue at gt: {mineig_hess_gt:.1e} and at estimate: {mineig_hess_hat:.1e}"