From 912e9b2616014de01967cfa495e2b78530066cd6 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 26 May 2025 18:53:27 -0400 Subject: [PATCH 01/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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}" From ef47709232b94d0c55189acc3b46d48aa6a230bc Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 13 Jun 2025 10:53:55 +0200 Subject: [PATCH 11/21] Enable estimating more than one rotations --- CHANGELOG.md | 5 +- popcor/base_lifters/state_lifter.py | 2 +- popcor/examples/rotation_lifter.py | 247 +++++++++++++++++----------- popcor/utils/plotting_tools.py | 4 +- scripts/rotation_averaging.ipynb | 4 +- 5 files changed, 159 insertions(+), 103 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 30491e7..87eae0e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,16 +5,19 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). -## [Unreleased] -- YYYY-MM-DD +## [Unreleased] -- 2025-06-13 (00-changelog)= ### Added +- RangeOnlyLifter: sample_landmarks_filling_space +- RotationLifter: support to plot 2d frames (01-changelog)= ### Changed (02-changelog)= ### Fixed +- Link fixes in documentation ## [0.0.2] - 2025-06-04 diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index df42d6a..9a3ad05 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -163,7 +163,7 @@ def get_cost(self, theta, y: np.ndarray | None = None) -> float: def local_solver( self, t0, - y: np.ndarray | list | None = None, + y: np.ndarray | list | dict | None = None, *args, **kwargs, ): diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 9df3d14..50ccb87 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -15,7 +15,7 @@ class RotationLifter(StateLifter): LEVELS = ["no"] HOM = "h" - VARIABLE_LIST = [["h", "c"]] + VARIABLE_LIST = [["h", "c_0"], ["h", "c_0", "c_1"]] # whether or not to include the determinant constraints in the known constraints. ADD_DETERMINANT = False @@ -23,8 +23,9 @@ class RotationLifter(StateLifter): NOISE = 1e-3 # Add any parameters here that describe the problem (e.g. number of landmarks etc.) - def __init__(self, level="no", param_level="no", d=2, n_meas=2): + def __init__(self, level="no", param_level="no", d=2, n_meas=2, n_rot=1): self.n_meas = n_meas + self.n_rot = n_rot self.level = level super().__init__( level=level, @@ -34,16 +35,21 @@ def __init__(self, level="no", param_level="no", d=2, n_meas=2): @property def var_dict(self): - return {self.HOM: 1, "c": self.d**2} + var_dict = {self.HOM: 1} + var_dict.update({f"c_{i}": self.d**2 for i in range(self.n_rot)}) + return var_dict def sample_theta(self): """Generate a random new feasible point.""" - - if self.d == 2: - angle = np.random.uniform(0, 2 * np.pi) - C = R.from_euler("z", angle).as_matrix()[:2, :2] - elif self.d == 3: - C = R.random().as_matrix() + C = np.empty((self.n_rot * self.d, self.d)) + for i in range(self.n_rot): + if self.d == 2: + angle = np.random.uniform(0, 2 * np.pi) + C[i * self.d : (i + 1) * self.d, :] = R.from_euler( + "z", angle + ).as_matrix()[:2, :2] + elif self.d == 3: + C[i * self.d : (i + 1) * self.d, :] = R.random().as_matrix() return C def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: @@ -59,37 +65,40 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: for key in var_subset: if key == self.HOM: x_data.append(1.0) - elif key == "c": - x_data += list(theta.flatten("C")) + elif "c" in key: + i = int(key.split("_")[1]) + x_data += list(theta[i * self.d : (i + 1) * self.d].flatten("C")) dim_x = self.get_dim_x(var_subset=var_subset) assert len(x_data) == dim_x return np.array(x_data) def get_theta(self, x: np.ndarray) -> np.ndarray: assert np.ndim(x) == 1 - C_flat = x[: self.d**2] - return C_flat.reshape((self.d, self.d)) + C_flat = x[: self.n_rot * self.d**2] + return C_flat.reshape((self.n_rot * self.d, self.d)) - def simulate_y(self, noise: float | None = None) -> list: + def simulate_y(self, noise: float | None = None) -> dict: if noise is None: noise = self.NOISE - R_gt = self.theta.reshape((self.d, self.d)) - 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 = R_gt.T @ Rnoise - else: - Ri = R_gt.T - y.append(Ri) + y = {} + for i in range(self.n_rot): + R_gt = self.theta[i * self.d : (i + 1) * self.d, :] + y[i] = [] + for n 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 = R_gt.T @ Rnoise + else: + Ri = R_gt.T + y[i].append(Ri) return y def get_Q(self, noise: float | None = None, output_poly: bool = False): @@ -109,9 +118,17 @@ def get_Q_from_y(self, y, output_poly=False): # || R @ R.T - I ||_F^2 = 0 """param y: list of noisy rotation matrices.""" Q = PolyMatrix() - for Ri in y: - Q[self.HOM, "c"] -= Ri.T.flatten("C")[None, :] - Q[self.HOM, self.HOM] += len(y) * self.d + + for key, R in y.items(): + # treat unary factors + if isinstance(key, int): + assert isinstance(R, list) + for Ri in R: + Q[self.HOM, f"c_{key}"] -= Ri.T.flatten("C")[None, :] + Q[self.HOM, self.HOM] += len(R) * self.d + elif isinstance(key, tuple): + i, j = key + Q[f"c_{i}", f"c_{j}"] -= R.T.flatten("C")[None, :] if output_poly: return Q else: @@ -180,72 +197,73 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): if var_dict is None: var_dict = self.var_dict - if "c" in var_dict: - # enforce diagonal == 1 for R'R = I - for i in range(self.d): - Ei = np.zeros((self.d, self.d)) - Ei[i, i] = 1.0 - constraint = np.kron(Ei, np.eye(self.d)) - Ai = PolyMatrix(symmetric=True) - Ai["c", "c"] = constraint - Ai[self.HOM, self.HOM] = -1 - self.test_and_add(A_list, Ai, output_poly=output_poly) - - # enforce off-diagonal == 0 for R'R = I - for i in range(self.d): - for j in range(i + 1, self.d): + for k in range(self.n_rot): + if f"c_{k}" in var_dict: + # enforce diagonal == 1 for R'R = I + for i in range(self.d): Ei = np.zeros((self.d, self.d)) - Ei[i, j] = 1.0 - Ei[j, i] = 1.0 + Ei[i, i] = 1.0 constraint = np.kron(Ei, np.eye(self.d)) Ai = PolyMatrix(symmetric=True) - Ai["c", "c"] = constraint + Ai[f"c_{k}", f"c_{k}"] = constraint + Ai[self.HOM, self.HOM] = -1 self.test_and_add(A_list, Ai, output_poly=output_poly) - # enforce that determinant is one. - if self.d == 2 and self.ADD_DETERMINANT: - # C = [a b; c d]; ad - bc - 1 = 0 - # a b c d - # a 1 - # b -1 - # c -1 - # d 1 - Ai = PolyMatrix(symmetric=True) - constraint = np.zeros((self.d**2, self.d**2)) - constraint[0, 3] = constraint[3, 0] = 1.0 - constraint[1, 2] = constraint[2, 1] = -1.0 - Ai[self.HOM, self.HOM] = -2 - Ai["c", "c"] = constraint - self.test_and_add(A_list, Ai, output_poly=output_poly) - elif self.d == 3 and self.ADD_DETERMINANT: - # c11 c12 c13 c21 * c32 - c31 * c22 = c13 - # C = [c21, c22, c23]; c1 x c2 = c3: c31 * c12 - c11 * c12 = c23 - # c31 c32 c33 c11 * c22 - c21 * c12 = c33 - print( - "Warning: consider implementing the determinant constraint for RobustPoseLifter, d=3" - ) - - if add_redundant and "c" in var_dict: - # enforce diagonal == 1 for RR' = I - for i in range(self.d): - Ei = np.zeros((self.d, self.d)) - Ei[i, i] = 1.0 - constraint = np.kron(np.eye(self.d), Ei) - Ai = PolyMatrix(symmetric=True) - Ai["c", "c"] = constraint - Ai[self.HOM, self.HOM] = -1 - self.test_and_add(A_list, Ai, output_poly=output_poly) - - # enforce off-diagonal == 0 for RR' = I - for i in range(self.d): - for j in range(i + 1, self.d): + # enforce off-diagonal == 0 for R'R = I + for i in range(self.d): + for j in range(i + 1, self.d): + Ei = np.zeros((self.d, self.d)) + Ei[i, j] = 1.0 + Ei[j, i] = 1.0 + constraint = np.kron(Ei, np.eye(self.d)) + Ai = PolyMatrix(symmetric=True) + Ai[f"c_{k}", f"c_{k}"] = constraint + self.test_and_add(A_list, Ai, output_poly=output_poly) + + # enforce that determinant is one. + if self.d == 2 and self.ADD_DETERMINANT: + # C = [a b; c d]; ad - bc - 1 = 0 + # a b c d + # a 1 + # b -1 + # c -1 + # d 1 + Ai = PolyMatrix(symmetric=True) + constraint = np.zeros((self.d**2, self.d**2)) + constraint[0, 3] = constraint[3, 0] = 1.0 + constraint[1, 2] = constraint[2, 1] = -1.0 + Ai[self.HOM, self.HOM] = -2 + Ai[f"c_{k}", f"c_{k}"] = constraint + self.test_and_add(A_list, Ai, output_poly=output_poly) + elif self.d == 3 and self.ADD_DETERMINANT: + # c11 c12 c13 c21 * c32 - c31 * c22 = c13 + # C = [c21, c22, c23]; c1 x c2 = c3: c31 * c12 - c11 * c12 = c23 + # c31 c32 c33 c11 * c22 - c21 * c12 = c33 + print( + "Warning: consider implementing the determinant constraint for RobustPoseLifter, d=3" + ) + + if add_redundant and f"c_{k}" in var_dict: + # enforce diagonal == 1 for RR' = I + for i in range(self.d): Ei = np.zeros((self.d, self.d)) - Ei[i, j] = 1.0 - Ei[j, i] = 1.0 + Ei[i, i] = 1.0 constraint = np.kron(np.eye(self.d), Ei) Ai = PolyMatrix(symmetric=True) - Ai["c", "c"] = constraint + Ai[f"c_{k}", f"c_{k}"] = constraint + Ai[self.HOM, self.HOM] = -1 self.test_and_add(A_list, Ai, output_poly=output_poly) + + # enforce off-diagonal == 0 for RR' = I + for i in range(self.d): + for j in range(i + 1, self.d): + Ei = np.zeros((self.d, self.d)) + Ei[i, j] = 1.0 + Ei[j, i] = 1.0 + constraint = np.kron(np.eye(self.d), Ei) + Ai = PolyMatrix(symmetric=True) + Ai[f"c_{k}", f"c_{k}"] = constraint + self.test_and_add(A_list, Ai, output_poly=output_poly) return A_list def plot(self, estimates={}): @@ -256,22 +274,55 @@ def plot(self, estimates={}): 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, marker="") - - linestyles = itertools.cycle(["--", "-.", ":"]) - for label, theta in estimates.items(): + for i in range(self.n_rot): plot_frame( ax=ax, - theta=theta, - label=label, - ls=next(linestyles), - scale=1.0, + theta=self.theta[i * self.d : (i + 1) * self.d, :], + label="gt", + ls="-", + scale=0.5, marker="", + r_wc_w=np.hstack([i] + [0.0] * (self.d - 1)), # type.ignore ) + linestyles = itertools.cycle(["--", "-.", ":"]) + for label, theta in estimates.items(): + for i in range(self.n_rot): + plot_frame( + ax=ax, + theta=theta[i * self.d : (i + 1) * self.d, :], + label=label, + ls=next(linestyles), + scale=1.0, + marker="", + r_wc_w=np.hstack([i] + [0.0] * (self.d - 1)), # type.ignore + ) + ax.set_aspect("equal") ax.legend() return fig, ax def __repr__(self): return f"rotation_lifter{self.d}d" + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + import numpy as np + + np.random.seed(2) + lifter = RotationLifter(d=3, n_meas=3, n_rot=2) + + y = lifter.simulate_y(noise=0.2) + + theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False) + estimates = {"init gt": theta_gt} + for i in range(10): + theta_init = lifter.sample_theta() + theta_i, *_ = lifter.local_solver(theta_init, y, verbose=False) + estimates[f"init random {i}"] = theta_i + + fig, ax = lifter.plot(estimates=estimates) + ax.legend([]) + plt.show(block=False) + print("done") diff --git a/popcor/utils/plotting_tools.py b/popcor/utils/plotting_tools.py index 4adc893..070c5d0 100644 --- a/popcor/utils/plotting_tools.py +++ b/popcor/utils/plotting_tools.py @@ -113,12 +113,14 @@ def plot_frame( ls="--", alpha=0.5, d=3, + r_wc_w: np.ndarray | None = None, **kwargs, ): if np.ndim(theta) == 2: # used by rotation averaging C_cw = theta - r_wc_w = np.zeros((theta.shape[0])) + if r_wc_w is None: + r_wc_w = np.zeros((theta.shape[0])) else: try: C_cw, r_wc_c = get_C_r_from_theta(theta, d=d) diff --git a/scripts/rotation_averaging.ipynb b/scripts/rotation_averaging.ipynb index dcd99d1..95a3abc 100644 --- a/scripts/rotation_averaging.ipynb +++ b/scripts/rotation_averaging.ipynb @@ -122,7 +122,7 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "field_guide", "language": "python", "name": "python3" }, @@ -136,7 +136,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.17" + "version": "3.10.16" } }, "nbformat": 4, From d5a375fb49bff761ac29102eca7495ffde81ed69 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 13 Jun 2025 12:40:55 +0200 Subject: [PATCH 12/21] Implement rank-d version for RotationLifter --- popcor/examples/rotation_lifter.py | 149 +++++++++++++++++++++-------- tests/test_rotation_lifter.py | 30 ++++++ 2 files changed, 141 insertions(+), 38 deletions(-) create mode 100644 tests/test_rotation_lifter.py diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 50ccb87..725d94b 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -11,9 +11,14 @@ class RotationLifter(StateLifter): - """Rotation averaging problem.""" + """Rotation averaging problem. - LEVELS = ["no"] + - level "no" corresponds to the rank-1 version. + - level "bm" corresponds to the rank-d version (bm=Bourer Monteiro, for later extension). + + """ + + LEVELS = ["no", "bm"] HOM = "h" VARIABLE_LIST = [["h", "c_0"], ["h", "c_0", "c_1"]] @@ -23,10 +28,13 @@ class RotationLifter(StateLifter): NOISE = 1e-3 # Add any parameters here that describe the problem (e.g. number of landmarks etc.) - def __init__(self, level="no", param_level="no", d=2, n_meas=2, n_rot=1): + def __init__( + self, level="no", param_level="no", d=2, n_meas=2, n_rot=1, sparsity="chain" + ): self.n_meas = n_meas self.n_rot = n_rot self.level = level + self.sparsity = sparsity super().__init__( level=level, param_level=param_level, @@ -35,8 +43,11 @@ def __init__(self, level="no", param_level="no", d=2, n_meas=2, n_rot=1): @property def var_dict(self): - var_dict = {self.HOM: 1} - var_dict.update({f"c_{i}": self.d**2 for i in range(self.n_rot)}) + if self.level == "no": + var_dict = {self.HOM: 1} + var_dict.update({f"c_{i}": self.d**2 for i in range(self.n_rot)}) + else: + var_dict = {f"c_{i}": self.d for i in range(self.n_rot)} return var_dict def sample_theta(self): @@ -62,43 +73,74 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: var_subset = self.var_dict.keys() x_data = [] - for key in var_subset: - if key == self.HOM: - x_data.append(1.0) - elif "c" in key: - i = int(key.split("_")[1]) - x_data += list(theta[i * self.d : (i + 1) * self.d].flatten("C")) - dim_x = self.get_dim_x(var_subset=var_subset) - assert len(x_data) == dim_x - return np.array(x_data) + if self.level == "no": + for key in var_subset: + if key == self.HOM: + x_data.append(1.0) + elif "c" in key: + i = int(key.split("_")[1]) + x_data += list(theta[i * self.d : (i + 1) * self.d].flatten("C")) + dim_x = self.get_dim_x(var_subset=var_subset) + assert len(x_data) == dim_x + return np.hstack(x_data) + elif self.level == "bm": + for key in var_subset: + if "c" in key: + i = int(key.split("_")[1]) + x_data.append(theta[i * self.d : (i + 1) * self.d]) + return np.vstack(x_data) def get_theta(self, x: np.ndarray) -> np.ndarray: - assert np.ndim(x) == 1 - C_flat = x[: self.n_rot * self.d**2] - return C_flat.reshape((self.n_rot * self.d, self.d)) + if self.level == "no": + assert np.ndim(x) == 1 + C_flat = x[: self.n_rot * self.d**2] + return C_flat.reshape((self.n_rot * self.d, self.d)) + elif self.level == "bm": + return np.array(x[: self.n_rot * self.d, : self.d]) def simulate_y(self, noise: float | None = None) -> dict: if noise is None: noise = self.NOISE y = {} - for i in range(self.n_rot): - R_gt = self.theta[i * self.d : (i + 1) * self.d, :] - y[i] = [] - for n in range(self.n_meas): - # noise model: R_i = R.T @ Rnoise + if self.n_meas > 0: + for i in range(self.n_rot): + R_gt = self.theta[i * self.d : (i + 1) * self.d, :] + y[i] = [] + for _ 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 = R_gt.T @ Rnoise + else: + Ri = R_gt.T + y[i].append(Ri) + if self.sparsity == "chain": + for i in range(self.n_rot - 1): + j = i + 1 + R_i = self.theta[i * self.d : (i + 1) * self.d, :] + R_j = self.theta[j * self.d : (j + 1) * self.d, :] + R_gt = R_i @ R_j.T + + # Generate a random small rotation as noise and apply it 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 = R_gt.T @ Rnoise + y[(i, j)] = R_gt @ Rnoise else: - Ri = R_gt.T - y[i].append(Ri) + y[(i, j)] = R_gt + else: + raise ValueError(f"Unknown sparsity {self.sparsity}") return y def get_Q(self, noise: float | None = None, output_poly: bool = False): @@ -122,13 +164,21 @@ def get_Q_from_y(self, y, output_poly=False): for key, R in y.items(): # treat unary factors if isinstance(key, int): + if self.level == "bm": + raise NotImplementedError( + "no support for unary factors in bm formulation yet" + ) assert isinstance(R, list) for Ri in R: - Q[self.HOM, f"c_{key}"] -= Ri.T.flatten("C")[None, :] + if self.level == "no": + Q[self.HOM, f"c_{key}"] -= Ri.T.flatten("C")[None, :] Q[self.HOM, self.HOM] += len(R) * self.d elif isinstance(key, tuple): i, j = key - Q[f"c_{i}", f"c_{j}"] -= R.T.flatten("C")[None, :] + if self.level == "no": + Q[f"c_{i}", f"c_{j}"] -= np.kron(np.eye(self.d), R.T) + elif self.level == "bm": + Q[f"c_{i}", f"c_{j}"] -= R if output_poly: return Q else: @@ -137,6 +187,8 @@ def get_Q_from_y(self, y, output_poly=False): def local_solver_old( self, t0, y, verbose=False, method=METHOD, solver_kwargs=SOLVER_KWARGS ): + """Not used anymore, kept for reference. We now use the default + QCQP local solver.""" import pymanopt from pymanopt.manifolds import SpecialOrthogonalGroup @@ -203,10 +255,13 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): for i in range(self.d): Ei = np.zeros((self.d, self.d)) Ei[i, i] = 1.0 - constraint = np.kron(Ei, np.eye(self.d)) Ai = PolyMatrix(symmetric=True) - Ai[f"c_{k}", f"c_{k}"] = constraint Ai[self.HOM, self.HOM] = -1 + if self.level == "no": + constraint = np.kron(Ei, np.eye(self.d)) + Ai[f"c_{k}", f"c_{k}"] = constraint + else: + Ai[f"c_{k}", f"c_{k}"] = Ei self.test_and_add(A_list, Ai, output_poly=output_poly) # enforce off-diagonal == 0 for R'R = I @@ -215,19 +270,34 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): Ei = np.zeros((self.d, self.d)) Ei[i, j] = 1.0 Ei[j, i] = 1.0 - constraint = np.kron(Ei, np.eye(self.d)) Ai = PolyMatrix(symmetric=True) - Ai[f"c_{k}", f"c_{k}"] = constraint + if self.level == "no": + constraint = np.kron(Ei, np.eye(self.d)) + Ai[f"c_{k}", f"c_{k}"] = constraint + else: + Ai[f"c_{k}", f"c_{k}"] = Ei self.test_and_add(A_list, Ai, output_poly=output_poly) # enforce that determinant is one. if self.d == 2 and self.ADD_DETERMINANT: + # level "no": # C = [a b; c d]; ad - bc - 1 = 0 # a b c d # a 1 # b -1 # c -1 # d 1 + # level "bm" + # C = [a b + # c d] + # C @ C.T + # [a b] [a c] [a^2 + b^2 a*c + b*d] + # [c d] [b d] = [a*c + b*d c^2 + d^2] + # cannot be implemented... + if self.level == "bm": + raise NotImplementedError( + "Cannot add determinant constraint for level bm" + ) Ai = PolyMatrix(symmetric=True) constraint = np.zeros((self.d**2, self.d**2)) constraint[0, 3] = constraint[3, 0] = 1.0 @@ -244,14 +314,18 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): ) if add_redundant and f"c_{k}" in var_dict: + if self.level == "bm": + print("No known redundant constraints for level bm") + continue + # enforce diagonal == 1 for RR' = I for i in range(self.d): Ei = np.zeros((self.d, self.d)) Ei[i, i] = 1.0 - constraint = np.kron(np.eye(self.d), Ei) Ai = PolyMatrix(symmetric=True) - Ai[f"c_{k}", f"c_{k}"] = constraint Ai[self.HOM, self.HOM] = -1 + constraint = np.kron(np.eye(self.d), Ei) + Ai[f"c_{k}", f"c_{k}"] = constraint self.test_and_add(A_list, Ai, output_poly=output_poly) # enforce off-diagonal == 0 for RR' = I @@ -303,16 +377,15 @@ def plot(self, estimates={}): return fig, ax def __repr__(self): - return f"rotation_lifter{self.d}d" + return f"rotation_lifter{self.d}d_{self.level}" if __name__ == "__main__": import matplotlib.pyplot as plt import numpy as np - np.random.seed(2) - lifter = RotationLifter(d=3, n_meas=3, n_rot=2) - + np.random.seed(0) + lifter = RotationLifter(d=3, n_meas=0, n_rot=3, sparsity="chain", level="no") y = lifter.simulate_y(noise=0.2) theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False) diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py new file mode 100644 index 0000000..96fab2f --- /dev/null +++ b/tests/test_rotation_lifter.py @@ -0,0 +1,30 @@ +import numpy as np + +from popcor.examples import RotationLifter + + +def test_rank1_vs_rankd(): + np.random.seed(0) + r1 = RotationLifter(level="no", n_meas=0, n_rot=3, d=2) + + np.random.seed(0) + r2 = RotationLifter(level="bm", n_meas=0, n_rot=3, d=2) + + np.testing.assert_array_equal(r1.theta, r2.theta) + + y = r1.simulate_y(noise=1e-5) + + x1 = r1.get_x() + x2 = r2.get_x() + + Q1 = r1.get_Q_from_y(y) + Q2 = r2.get_Q_from_y(y) + + cost1 = x1.T @ Q1 @ x1 + cost2 = np.trace(x2.T @ Q2 @ x2) + assert abs(cost1 - cost2) < 1e-10 + + +if __name__ == "__main__": + + test_rank1_vs_rankd() From 51876dd10e7c908bbc574768ffbc0fbf19387eea Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 13 Jun 2025 18:03:14 +0200 Subject: [PATCH 13/21] Test solvers. rank-1 passing, not rank-d. --- popcor/base_lifters/_base_class.py | 17 ++- popcor/base_lifters/state_lifter.py | 21 +++- popcor/examples/rotation_lifter.py | 164 +++++++++++++++++++++++----- tests/test_rotation_lifter.py | 32 +++++- 4 files changed, 201 insertions(+), 33 deletions(-) diff --git a/popcor/base_lifters/_base_class.py b/popcor/base_lifters/_base_class.py index a39d579..aef71dc 100644 --- a/popcor/base_lifters/_base_class.py +++ b/popcor/base_lifters/_base_class.py @@ -685,7 +685,22 @@ def get_A0(self, var_subset=None): return A0.get_matrix(var_dict) def get_A_b_list(self, A_list, var_subset=None): - return [(self.get_A0(var_subset), 1.0)] + [(A, 0.0) for A in A_list] + """get equality constraint tuples (Ai, bi) s.t. x.t @ Ai @ bi, 0 + + :param A_list: normally, this is just the list of equality constaints that equal zero. We will add the homogenization. + for certain cases, such as the RotationLifter with level="bm", this is already a tuple of (Ai, bi). + + """ + if var_subset is None: + var_subset = self.var_dict + + if isinstance(A_list, list): + # TODO(FD): do more with var_subset + assert self.HOM in var_subset + return [(self.get_A0(var_subset), 1.0)] + [(A, 0.0) for A in A_list] + else: + assert isinstance(A_list, tuple) + return [(Ai, bi) for Ai, bi in zip(*A_list)] def sample_parameters_landmarks(self, landmarks: np.ndarray): """Used by RobustPoseLifter, RangeOnlyLocLifter: the default way of adding landmarks to parameters.""" diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index 9a3ad05..d5f1ddf 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -153,12 +153,12 @@ def get_cost(self, theta, y: np.ndarray | None = None) -> float: print( "Warning: using default get_cost, which may be less efficient than a custom one." ) - x = self.get_x(theta=theta).flatten("C") if y is not None: Q = self.get_Q_from_y(y) else: Q = self.get_Q() - return float(x.T @ Q @ x) + x = self.get_x(theta=theta) + return float(np.trace(x.T @ Q @ x)) def local_solver( self, @@ -196,13 +196,21 @@ def local_solver( Constraints = self.get_A_b_list(A_list=self.get_A_known()) x0 = self.get_x(theta=t0) + if np.ndim(x0) == 1: + x0 = x0.reshape((-1, 1)) + + try: + L = self.get_L() + except AttributeError: + L = None + X, info = solve_low_rank_sdp( - Q, Constraints=Constraints, rank=1, x_cand=x0, **kwargs + Q, Constraints=Constraints, rank=x0.shape[1], x_cand=x0, L=L, **kwargs ) # TODO(FD) identify when the solve is not successful. info["success"] = True try: - theta = self.get_theta(X[1:, 0]) + theta = self.get_theta(X[:, : x0.shape[1]]) except AttributeError: theta = X[1 : 1 + self.d, 0] return theta, info, info["cost"] @@ -239,7 +247,10 @@ def get_theta(self, x): "Warning: got homogenized vector x. The convention is that get_theta should get x[1:]." "Please make sure that you use get_theta as intended." ) - return x.flatten()[: self.d] + if self.HOM in self.var_dict: + return x.flatten()[1 : 1 + self.d] + else: + return x.flatten()[: self.d] def get_valid_samples(self, n_samples): samples = [self.sample_theta().flatten() for _ in range(n_samples)] diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 725d94b..d1b8e8f 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -89,14 +89,19 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: i = int(key.split("_")[1]) x_data.append(theta[i * self.d : (i + 1) * self.d]) return np.vstack(x_data) + else: + raise ValueError(f"Unknown level {self.level} for RotationLifter") def get_theta(self, x: np.ndarray) -> np.ndarray: if self.level == "no": - assert np.ndim(x) == 1 - C_flat = x[: self.n_rot * self.d**2] + if np.ndim(x) == 2: + assert x.shape[1] == 1 + C_flat = x[1 : 1 + self.n_rot * self.d**2] return C_flat.reshape((self.n_rot * self.d, self.d)) elif self.level == "bm": return np.array(x[: self.n_rot * self.d, : self.d]) + else: + raise ValueError(f"Unknown level {self.level} for RotationLifter") def simulate_y(self, noise: float | None = None) -> dict: if noise is None: @@ -104,6 +109,13 @@ def simulate_y(self, noise: float | None = None) -> dict: y = {} if self.n_meas > 0: + """ + _ + || R_i - R_i || + + measurements: + R_ij = R_i @ R_j.T + """ for i in range(self.n_rot): R_gt = self.theta[i * self.d : (i + 1) * self.d, :] y[i] = [] @@ -122,6 +134,12 @@ def simulate_y(self, noise: float | None = None) -> dict: Ri = R_gt.T y[i].append(Ri) if self.sparsity == "chain": + """ + _ + || R_i - R_ij @ R_j ||_F^2 + _ + measurements: R_ij = R_i @ R_j.T + """ for i in range(self.n_rot - 1): j = i + 1 R_i = self.theta[i * self.d : (i + 1) * self.d, :] @@ -151,18 +169,46 @@ def get_Q(self, noise: float | None = None, output_poly: bool = False): return self.get_Q_from_y(self.y_, output_poly=output_poly) + def get_L(self, theta=None): + """ + Returns matrix L from the cost term, so that we can add non-quadratic terms. + F is the fixed rotation matrix + + || R - F ||_F = tr(R'R) - 2 tr(F'R) + tr(F'F) + = 2 tr(I) - 2 tr(F'R) + + # R is Nd x d + argmin "" = argmin -2 * tr(F'R_0) + = argmin -2 * vec(F).T @ vec(R_0) + + will add trace(L'R), so L is of shape Nd x d + """ + if theta is None: + theta = self.theta + R0 = theta[: self.d, : self.d] + + if self.level == "bm": + L = PolyMatrix(symmetric=False) + L["c_0", "width"] = -R0 + return L.get_matrix(variables=(self.var_dict, {"width": self.d})) + elif self.level == "no": + L = PolyMatrix(symmetric=False) + L["c_0", "width"] = -R0.flatten("C") + return L.get_matrix(variables=(self.var_dict, {"width": 1})) + else: + raise ValueError(f"Unknown level {self.level} for RotationLifter") + def get_Q_from_y(self, y, output_poly=False): - # f(R) = sum_i || R @ R_i - I ||_F^2 - # argmin f(R) = argmin sum_i || R_i.T @ R_i ||^2 - 2 tr(R.T @ R_i) + ||I||_F^2 - # = argmin sum_i -2 tr(R.T @ R_i) + sum_i d - # = argmin sum_i -2 vec(R).T @ vec(R_i.T) + N * d - # sanity check for zero noise: - # || R @ R.T - I ||_F^2 = 0 """param y: list of noisy rotation matrices.""" Q = PolyMatrix() for key, R in y.items(): # treat unary factors + # f(R) = sum_i || R - Ri ||_F^2 + # argmin f(R) = argmin sum_i tr((R - Ri)'(R - Ri)) + # = argmin sum_i -2 tr(Ri.T @ R) + # tr(A.T @ B) = vec(A).T @ vec(B) + # = argmin sum_i -2 vec(Ri).T @ vec(R) if isinstance(key, int): if self.level == "bm": raise NotImplementedError( @@ -173,12 +219,18 @@ def get_Q_from_y(self, y, output_poly=False): if self.level == "no": Q[self.HOM, f"c_{key}"] -= Ri.T.flatten("C")[None, :] Q[self.HOM, self.HOM] += len(R) * self.d + # treat binary factors + # f(R) = sum_ij || Ri - Rij @ Rj ||_F^2 + # argmin f(R) = argmin sum_i -2 tr(Ri.T @ R_ij @ R_j) + # = tr(R_ij @ R_j @ Ri.T) + # tr(A.T @ C @ B) = vec(A).T @ (I kron C) @ vec(B) + # = argmin sum_i -2 tr((I kron R_ij) @ vec(R_j) vec(Ri).T) elif isinstance(key, tuple): i, j = key if self.level == "no": - Q[f"c_{i}", f"c_{j}"] -= np.kron(np.eye(self.d), R.T) + Q[f"c_{j}", f"c_{i}"] -= np.kron(np.eye(self.d), R) elif self.level == "bm": - Q[f"c_{i}", f"c_{j}"] -= R + Q[f"c_{j}", f"c_{i}"] -= R.T if output_poly: return Q else: @@ -234,10 +286,10 @@ def cost(R): if success: return theta_hat, info, cost - def test_and_add(self, A_list, Ai, output_poly): + def test_and_add(self, A_list, Ai, output_poly, bi=0): x = self.get_x() Ai_sparse = Ai.get_matrix(self.var_dict) - err = x.T @ Ai_sparse @ x + err = np.trace(np.atleast_2d(x.T @ Ai_sparse @ x)) - bi assert abs(err) <= 1e-10, err if output_poly: A_list.append(Ai) @@ -246,6 +298,7 @@ def test_and_add(self, A_list, Ai, output_poly): def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): A_list = [] + b_list = [] if var_dict is None: var_dict = self.var_dict @@ -256,13 +309,17 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): Ei = np.zeros((self.d, self.d)) Ei[i, i] = 1.0 Ai = PolyMatrix(symmetric=True) - Ai[self.HOM, self.HOM] = -1 if self.level == "no": constraint = np.kron(Ei, np.eye(self.d)) + Ai[self.HOM, self.HOM] = -1 Ai[f"c_{k}", f"c_{k}"] = constraint + b_list.append(0.0) else: Ai[f"c_{k}", f"c_{k}"] = Ei - self.test_and_add(A_list, Ai, output_poly=output_poly) + b_list.append(1.0) + self.test_and_add( + A_list, Ai, output_poly=output_poly, bi=b_list[-1] + ) # enforce off-diagonal == 0 for R'R = I for i in range(self.d): @@ -274,9 +331,13 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): if self.level == "no": constraint = np.kron(Ei, np.eye(self.d)) Ai[f"c_{k}", f"c_{k}"] = constraint + b_list.append(0.0) else: Ai[f"c_{k}", f"c_{k}"] = Ei - self.test_and_add(A_list, Ai, output_poly=output_poly) + b_list.append(0.0) + self.test_and_add( + A_list, Ai, output_poly=output_poly, bi=b_list[-1] + ) # enforce that determinant is one. if self.d == 2 and self.ADD_DETERMINANT: @@ -304,7 +365,10 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): constraint[1, 2] = constraint[2, 1] = -1.0 Ai[self.HOM, self.HOM] = -2 Ai[f"c_{k}", f"c_{k}"] = constraint - self.test_and_add(A_list, Ai, output_poly=output_poly) + b_list.append(0.0) + self.test_and_add( + A_list, Ai, output_poly=output_poly, bi=b_list[-1] + ) elif self.d == 3 and self.ADD_DETERMINANT: # c11 c12 c13 c21 * c32 - c31 * c22 = c13 # C = [c21, c22, c23]; c1 x c2 = c3: c31 * c12 - c11 * c12 = c23 @@ -326,7 +390,10 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): Ai[self.HOM, self.HOM] = -1 constraint = np.kron(np.eye(self.d), Ei) Ai[f"c_{k}", f"c_{k}"] = constraint - self.test_and_add(A_list, Ai, output_poly=output_poly) + b_list.append(0.0) + self.test_and_add( + A_list, Ai, output_poly=output_poly, bi=b_list[-1] + ) # enforce off-diagonal == 0 for RR' = I for i in range(self.d): @@ -337,8 +404,14 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): constraint = np.kron(np.eye(self.d), Ei) Ai = PolyMatrix(symmetric=True) Ai[f"c_{k}", f"c_{k}"] = constraint - self.test_and_add(A_list, Ai, output_poly=output_poly) - return A_list + b_list.append(0.0) + self.test_and_add( + A_list, Ai, output_poly=output_poly, bi=b_list[-1] + ) + if self.level == "bm": + return A_list, b_list + else: + return A_list def plot(self, estimates={}): import itertools @@ -348,16 +421,18 @@ def plot(self, estimates={}): from popcor.utils.plotting_tools import plot_frame fig, ax = plt.subplots() + label = "gt" for i in range(self.n_rot): plot_frame( ax=ax, theta=self.theta[i * self.d : (i + 1) * self.d, :], - label="gt", + label=label, ls="-", scale=0.5, marker="", - r_wc_w=np.hstack([i] + [0.0] * (self.d - 1)), # type.ignore + r_wc_w=np.hstack([i * 2.0] + [0.0] * (self.d - 1)), # type.ignore ) + label = None linestyles = itertools.cycle(["--", "-.", ":"]) for label, theta in estimates.items(): @@ -369,8 +444,9 @@ def plot(self, estimates={}): ls=next(linestyles), scale=1.0, marker="", - r_wc_w=np.hstack([i] + [0.0] * (self.d - 1)), # type.ignore + r_wc_w=np.hstack([i * 2.0] + [0.0] * (self.d - 1)), # type.ignore ) + label = None ax.set_aspect("equal") ax.legend() @@ -383,19 +459,55 @@ def __repr__(self): if __name__ == "__main__": import matplotlib.pyplot as plt import numpy as np + from cert_tools.linalg_tools import rank_project + from cert_tools.sdp_solvers import solve_sdp + + from popcor.utils.plotting_tools import plot_matrix + + # level = "no" + level = "bm" np.random.seed(0) - lifter = RotationLifter(d=3, n_meas=0, n_rot=3, sparsity="chain", level="no") - y = lifter.simulate_y(noise=0.2) + lifter = RotationLifter(d=2, n_meas=0, n_rot=3, sparsity="chain", level=level) + y = lifter.simulate_y(noise=1e-10) + + x = lifter.get_x() + rank = x.shape[1] if np.ndim(x) == 2 else 1 theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False) estimates = {"init gt": theta_gt} - for i in range(10): + for i in range(0): theta_init = lifter.sample_theta() theta_i, *_ = lifter.local_solver(theta_init, y, verbose=False) estimates[f"init random {i}"] = theta_i fig, ax = lifter.plot(estimates=estimates) - ax.legend([]) + ax.legend() plt.show(block=False) + + Q = lifter.get_Q_from_y(y=y, output_poly=False) + A_known = lifter.get_A_known(output_poly=False) + constraints = lifter.get_A_b_list(lifter.get_A_known()) + + fig, axs = plt.subplots(1, len(constraints) + 1) + fig.set_size_inches(3 * (len(constraints) + 1), 3) + for i in range(len(constraints)): + Ai = constraints[i][0] + plot_matrix(Ai.toarray(), ax=axs[i], title=f"A{i} ", colorbar=False) # type: ignore + + fig = plot_matrix(Q.toarray(), ax=axs[i + 1], title="Q", colorbar=False) # type: ignore + + X, info = solve_sdp(Q, constraints, verbose=False) + + x, info_rank = rank_project(X, p=rank) + print(f"EVR: {info_rank['EVR']:.2e}") + + if rank == 1: + theta_opt = lifter.get_theta(x.flatten()[1:]) + else: + theta_opt = lifter.get_theta(x[:, :rank]) + + estimates = {"init gt": theta_gt, "SDP": theta_opt} + fig, ax = lifter.plot(estimates=estimates) + print("done") diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index 96fab2f..d683d44 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -4,6 +4,8 @@ def test_rank1_vs_rankd(): + """make sure that the rank-1 and rank-d lifters give the same cost + for the same measurements""" np.random.seed(0) r1 = RotationLifter(level="no", n_meas=0, n_rot=3, d=2) @@ -24,7 +26,35 @@ def test_rank1_vs_rankd(): cost2 = np.trace(x2.T @ Q2 @ x2) assert abs(cost1 - cost2) < 1e-10 + # actual cost should be + # 2 sum_ij tr(I) - 2 sum_ij tr(Ri.T@R_ij@R_j) + # ==========our cost ========= + cost_optimal = cost1 + 2 * len(y) * r1.d + assert abs(cost_optimal) < 1e-8 -if __name__ == "__main__": +def test_measurements(level="no"): + """make sure that the forward model for measurements is correct""" + lifter = RotationLifter(d=2, n_meas=0, n_rot=3, sparsity="chain", level=level) + y = lifter.simulate_y(noise=1e-10) + + for (i, j), R in y.items(): + R_i = lifter.theta[i * lifter.d : (i + 1) * lifter.d, :] + R_j = lifter.theta[j * lifter.d : (j + 1) * lifter.d, :] + np.testing.assert_allclose(R_i @ R_j.T, R, atol=1e-5) + + x = lifter.get_x() + rank = x.shape[1] if np.ndim(x) == 2 else 1 + + theta = lifter.get_theta(x) + for (i, j), R in y.items(): + R_i = theta[i * lifter.d : (i + 1) * lifter.d, :] + R_j = theta[j * lifter.d : (j + 1) * lifter.d, :] + np.testing.assert_allclose(R_i @ R_j.T, R, atol=1e-5) + + +if __name__ == "__main__": + test_measurements(level="no") + test_measurements(level="bm") test_rank1_vs_rankd() + print("all tests passed") From 3a07d6a1d0b8a397a0cdf0b3b44fb71b51219f7d Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Mon, 16 Jun 2025 11:01:25 +0200 Subject: [PATCH 14/21] Create new homogenization with dummy rotation, add n_rel and n_abs to init --- popcor/base_lifters/_base_class.py | 5 +- popcor/base_lifters/state_lifter.py | 7 +- popcor/examples/rotation_lifter.py | 306 +++++++++++++++++----------- tests/test_rotation_lifter.py | 6 +- 4 files changed, 198 insertions(+), 126 deletions(-) diff --git a/popcor/base_lifters/_base_class.py b/popcor/base_lifters/_base_class.py index aef71dc..e282880 100644 --- a/popcor/base_lifters/_base_class.py +++ b/popcor/base_lifters/_base_class.py @@ -700,7 +700,10 @@ def get_A_b_list(self, A_list, var_subset=None): return [(self.get_A0(var_subset), 1.0)] + [(A, 0.0) for A in A_list] else: assert isinstance(A_list, tuple) - return [(Ai, bi) for Ai, bi in zip(*A_list)] + A0_list, b0_list = self.get_A0(var_subset) + return [ + (Ai, bi) for Ai, bi in zip(A0_list + A_list[0], b0_list + A_list[1]) + ] def sample_parameters_landmarks(self, landmarks: np.ndarray): """Used by RobustPoseLifter, RangeOnlyLocLifter: the default way of adding landmarks to parameters.""" diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index d5f1ddf..07f1257 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -199,13 +199,8 @@ def local_solver( if np.ndim(x0) == 1: x0 = x0.reshape((-1, 1)) - try: - L = self.get_L() - except AttributeError: - L = None - X, info = solve_low_rank_sdp( - Q, Constraints=Constraints, rank=x0.shape[1], x_cand=x0, L=L, **kwargs + Q, Constraints=Constraints, rank=x0.shape[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 d1b8e8f..386546d 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -1,3 +1,5 @@ +from typing import List + import numpy as np from poly_matrix.poly_matrix import PolyMatrix from scipy.spatial.transform import Rotation as R @@ -10,6 +12,44 @@ ) +def get_orthogonal_constraints(key, hom, d, level): + A_list = [] + b_list = [] + for i in range(d): + # enforce diagonal == 1 for R'R = I + Ei = np.zeros((d, d)) + Ei[i, i] = 1.0 + Ai = PolyMatrix(symmetric=True) + if level == "no": + constraint = np.kron(Ei, np.eye(d)) + Ai[hom, hom] = -1 + Ai[key, key] = constraint + A_list.append(Ai) + b_list.append(0.0) + else: + Ai[key, key] = Ei + A_list.append(Ai) + b_list.append(1.0) + + # enforce off-diagonal == 0 for R'R = I + for i in range(d): + for j in range(i + 1, d): + Ei = np.zeros((d, d)) + Ei[i, j] = 1.0 + Ei[j, i] = 1.0 + Ai = PolyMatrix(symmetric=True) + if level == "no": + constraint = np.kron(Ei, np.eye(d)) + Ai[key, key] = constraint + A_list.append(Ai) + b_list.append(0.0) + else: + Ai[key, key] = Ei + A_list.append(Ai) + b_list.append(0.0) + return A_list, b_list + + class RotationLifter(StateLifter): """Rotation averaging problem. @@ -29,10 +69,22 @@ class RotationLifter(StateLifter): # Add any parameters here that describe the problem (e.g. number of landmarks etc.) def __init__( - self, level="no", param_level="no", d=2, n_meas=2, n_rot=1, sparsity="chain" + self, + level="no", + param_level="no", + d=2, + n_abs=2, + n_rot=1, + n_rel=1, + sparsity="chain", ): - self.n_meas = n_meas + assert n_rel in [ + 0, + 1, + ], "do not support more than 1 relative measurement per pair currently." self.n_rot = n_rot + self.n_abs = n_abs + self.n_rel = n_rel self.level = level self.sparsity = sparsity super().__init__( @@ -47,7 +99,8 @@ def var_dict(self): var_dict = {self.HOM: 1} var_dict.update({f"c_{i}": self.d**2 for i in range(self.n_rot)}) else: - var_dict = {f"c_{i}": self.d for i in range(self.n_rot)} + var_dict = {self.HOM: self.d} + var_dict.update({f"c_{i}": self.d for i in range(self.n_rot)}) return var_dict def sample_theta(self): @@ -80,14 +133,20 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: elif "c" in key: i = int(key.split("_")[1]) x_data += list(theta[i * self.d : (i + 1) * self.d].flatten("C")) + else: + raise ValueError(f"untreated key {key}") dim_x = self.get_dim_x(var_subset=var_subset) assert len(x_data) == dim_x return np.hstack(x_data) elif self.level == "bm": for key in var_subset: - if "c" in key: + if key == self.HOM: + x_data.append(np.eye(self.d)) + elif "c" in key: i = int(key.split("_")[1]) x_data.append(theta[i * self.d : (i + 1) * self.d]) + else: + raise ValueError(f"untreated key {key}") return np.vstack(x_data) else: raise ValueError(f"Unknown level {self.level} for RotationLifter") @@ -99,66 +158,81 @@ def get_theta(self, x: np.ndarray) -> np.ndarray: C_flat = x[1 : 1 + self.n_rot * self.d**2] return C_flat.reshape((self.n_rot * self.d, self.d)) elif self.level == "bm": - return np.array(x[: self.n_rot * self.d, : self.d]) + # d x d + R0 = x[: self.d, : self.d] + # nd x d + Ri = np.array(x[self.d : (self.n_rot + 1) * self.d, : self.d]) + # below is a block-diagonal matrix + return np.kron(np.eye(self.n_rot), R0.T) @ Ri else: raise ValueError(f"Unknown level {self.level} for RotationLifter") + def add_relative_measurement(self, i, j, noise) -> np.ndarray: + """ + _ + || R_i - R_ij @ R_j ||_F^2 + _ + measurements: R_ij = R_i @ R_j.T + """ + R_i = self.theta[i * self.d : (i + 1) * self.d, :] + R_j = self.theta[j * self.d : (j + 1) * self.d, :] + R_gt = R_i @ R_j.T + + # Generate a random small rotation as noise and apply it + if noise > 0: + 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] + ) + return R_gt @ Rnoise + else: + return R_gt + + def add_absolute_measurement(self, i, noise, n_meas=1) -> List[np.ndarray]: + """_ + || R_i - R_i || + + measurements: _ + R_i = R_i @ noise + """ + R_gt = self.theta[i * self.d : (i + 1) * self.d, :] + y = [] + for _ in range(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 = R_gt @ Rnoise + else: + Ri = R_gt + y.append(Ri) + return y + def simulate_y(self, noise: float | None = None) -> dict: if noise is None: noise = self.NOISE y = {} - if self.n_meas > 0: - """ - _ - || R_i - R_i || - - measurements: - R_ij = R_i @ R_j.T - """ + if self.n_abs > 0: for i in range(self.n_rot): - R_gt = self.theta[i * self.d : (i + 1) * self.d, :] - y[i] = [] - for _ 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 = R_gt.T @ Rnoise - else: - Ri = R_gt.T - y[i].append(Ri) - if self.sparsity == "chain": - """ - _ - || R_i - R_ij @ R_j ||_F^2 - _ - measurements: R_ij = R_i @ R_j.T - """ - for i in range(self.n_rot - 1): - j = i + 1 - R_i = self.theta[i * self.d : (i + 1) * self.d, :] - R_j = self.theta[j * self.d : (j + 1) * self.d, :] - R_gt = R_i @ R_j.T - - # Generate a random small rotation as noise and apply it - if noise > 0: - 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] - ) - y[(i, j)] = R_gt @ Rnoise - else: - y[(i, j)] = R_gt + y[i] = self.add_absolute_measurement(i, noise, self.n_abs) else: - raise ValueError(f"Unknown sparsity {self.sparsity}") + y[0] = self.add_absolute_measurement(0, 0.0, 1) + + if self.n_rel > 0: + if self.sparsity == "chain": + for i in range(self.n_rot - 1): + j = i + 1 + y[(i, j)] = self.add_relative_measurement(i, j, noise) + else: + raise ValueError(f"Unknown sparsity {self.sparsity}") return y def get_Q(self, noise: float | None = None, output_poly: bool = False): @@ -170,7 +244,8 @@ def get_Q(self, noise: float | None = None, output_poly: bool = False): return self.get_Q_from_y(self.y_, output_poly=output_poly) def get_L(self, theta=None): - """ + """This function is deprecated as we now introduce a homogeneous variable R0. + Returns matrix L from the cost term, so that we can add non-quadratic terms. F is the fixed rotation matrix @@ -210,15 +285,13 @@ def get_Q_from_y(self, y, output_poly=False): # tr(A.T @ B) = vec(A).T @ vec(B) # = argmin sum_i -2 vec(Ri).T @ vec(R) if isinstance(key, int): - if self.level == "bm": - raise NotImplementedError( - "no support for unary factors in bm formulation yet" - ) assert isinstance(R, list) for Ri in R: if self.level == "no": - Q[self.HOM, f"c_{key}"] -= Ri.T.flatten("C")[None, :] - Q[self.HOM, self.HOM] += len(R) * self.d + Q[self.HOM, f"c_{key}"] -= Ri.flatten("C")[None, :] + Q[self.HOM, self.HOM] += len(R) * self.d + elif self.level == "bm": + Q[self.HOM, f"c_{key}"] -= Ri # treat binary factors # f(R) = sum_ij || Ri - Rij @ Rj ||_F^2 # argmin f(R) = argmin sum_i -2 tr(Ri.T @ R_ij @ R_j) @@ -230,7 +303,8 @@ def get_Q_from_y(self, y, output_poly=False): if self.level == "no": Q[f"c_{j}", f"c_{i}"] -= np.kron(np.eye(self.d), R) elif self.level == "bm": - Q[f"c_{j}", f"c_{i}"] -= R.T + Q[f"c_{j}", f"c_{i}"] -= R + if output_poly: return Q else: @@ -286,7 +360,7 @@ def cost(R): if success: return theta_hat, info, cost - def test_and_add(self, A_list, Ai, output_poly, bi=0): + def test_and_add(self, A_list, Ai, output_poly, b_list=[], bi=0.0): x = self.get_x() Ai_sparse = Ai.get_matrix(self.var_dict) err = np.trace(np.atleast_2d(x.T @ Ai_sparse @ x)) - bi @@ -295,6 +369,19 @@ def test_and_add(self, A_list, Ai, output_poly, bi=0): A_list.append(Ai) else: A_list.append(Ai_sparse) + b_list.append(bi) + + def get_A0(self, var_subset=None): + if var_subset is None: + var_subset = self.var_dict + if self.level == "no": + return super().get_A0(var_subset=var_subset) + else: + # using self.HOM, None just to make it clear that the second argument is not used. + A_orth, b_orth = get_orthogonal_constraints( + self.HOM, None, self.d, self.level + ) + return [Ai.get_matrix(var_subset) for Ai in A_orth], b_orth def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): A_list = [] @@ -304,40 +391,11 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): for k in range(self.n_rot): if f"c_{k}" in var_dict: - # enforce diagonal == 1 for R'R = I - for i in range(self.d): - Ei = np.zeros((self.d, self.d)) - Ei[i, i] = 1.0 - Ai = PolyMatrix(symmetric=True) - if self.level == "no": - constraint = np.kron(Ei, np.eye(self.d)) - Ai[self.HOM, self.HOM] = -1 - Ai[f"c_{k}", f"c_{k}"] = constraint - b_list.append(0.0) - else: - Ai[f"c_{k}", f"c_{k}"] = Ei - b_list.append(1.0) - self.test_and_add( - A_list, Ai, output_poly=output_poly, bi=b_list[-1] - ) - - # enforce off-diagonal == 0 for R'R = I - for i in range(self.d): - for j in range(i + 1, self.d): - Ei = np.zeros((self.d, self.d)) - Ei[i, j] = 1.0 - Ei[j, i] = 1.0 - Ai = PolyMatrix(symmetric=True) - if self.level == "no": - constraint = np.kron(Ei, np.eye(self.d)) - Ai[f"c_{k}", f"c_{k}"] = constraint - b_list.append(0.0) - else: - Ai[f"c_{k}", f"c_{k}"] = Ei - b_list.append(0.0) - self.test_and_add( - A_list, Ai, output_poly=output_poly, bi=b_list[-1] - ) + A_orth, b_orth = get_orthogonal_constraints( + f"c_{k}", self.HOM, self.d, self.level + ) + for Ai, bi in zip(A_orth, b_orth): + self.test_and_add(A_list, Ai, output_poly, b_list, bi) # enforce that determinant is one. if self.d == 2 and self.ADD_DETERMINANT: @@ -365,10 +423,7 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): constraint[1, 2] = constraint[2, 1] = -1.0 Ai[self.HOM, self.HOM] = -2 Ai[f"c_{k}", f"c_{k}"] = constraint - b_list.append(0.0) - self.test_and_add( - A_list, Ai, output_poly=output_poly, bi=b_list[-1] - ) + self.test_and_add(A_list, Ai, output_poly, b_list, 0.0) elif self.d == 3 and self.ADD_DETERMINANT: # c11 c12 c13 c21 * c32 - c31 * c22 = c13 # C = [c21, c22, c23]; c1 x c2 = c3: c31 * c12 - c11 * c12 = c23 @@ -390,10 +445,20 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): Ai[self.HOM, self.HOM] = -1 constraint = np.kron(np.eye(self.d), Ei) Ai[f"c_{k}", f"c_{k}"] = constraint - b_list.append(0.0) - self.test_and_add( - A_list, Ai, output_poly=output_poly, bi=b_list[-1] - ) + self.test_and_add(A_list, Ai, output_poly, b_list, 0.0) + + if self.d == 2: + # enforce structure: + # [cos(x) -sin(x)] + # [sin(x) cos(x)] + # => [c s -s c] + Ai = PolyMatrix(symmetric=True) + Ai[self.HOM, f"c_{k}"] = np.array([1.0, 0, 0, -1.0])[None, :] + self.test_and_add(A_list, Ai, output_poly, b_list, 0.0) + + Ai = PolyMatrix(symmetric=True) + Ai[self.HOM, f"c_{k}"] = np.array([0, 1.0, 1.0, 0.0])[None, :] + self.test_and_add(A_list, Ai, output_poly, b_list, 0.0) # enforce off-diagonal == 0 for RR' = I for i in range(self.d): @@ -404,10 +469,7 @@ def get_A_known(self, var_dict=None, output_poly=False, add_redundant=False): constraint = np.kron(np.eye(self.d), Ei) Ai = PolyMatrix(symmetric=True) Ai[f"c_{k}", f"c_{k}"] = constraint - b_list.append(0.0) - self.test_and_add( - A_list, Ai, output_poly=output_poly, bi=b_list[-1] - ) + self.test_and_add(A_list, Ai, output_poly, b_list, 0.0) if self.level == "bm": return A_list, b_list else: @@ -459,16 +521,27 @@ def __repr__(self): if __name__ == "__main__": import matplotlib.pyplot as plt import numpy as np + from cert_tools.linalg_tools import rank_project from cert_tools.sdp_solvers import solve_sdp - from popcor.utils.plotting_tools import plot_matrix - # level = "no" - level = "bm" + level = "no" + # level = "bm" np.random.seed(0) - lifter = RotationLifter(d=2, n_meas=0, n_rot=3, sparsity="chain", level=level) + # lifter = RotationLifter( + # d=2, n_abs=0, n_rel=1, n_rot=4, sparsity="chain", level=level + # ) + lifter = RotationLifter( + d=2, n_abs=1, n_rel=0, n_rot=4, sparsity="chain", level=level + ) + + # add relative measurements between all subsequent rotations. + # lifter.add_relative_measurement(0, 1, noise=1e-3) + # add one absolute measurement to fix Gauge freedom + # lifter.add_absolute_measurement(0, noise=1e-5) + y = lifter.simulate_y(noise=1e-10) x = lifter.get_x() @@ -486,8 +559,8 @@ def __repr__(self): plt.show(block=False) Q = lifter.get_Q_from_y(y=y, output_poly=False) - A_known = lifter.get_A_known(output_poly=False) - constraints = lifter.get_A_b_list(lifter.get_A_known()) + A_known = lifter.get_A_known(output_poly=False, add_redundant=True) + constraints = lifter.get_A_b_list(A_known) fig, axs = plt.subplots(1, len(constraints) + 1) fig.set_size_inches(3 * (len(constraints) + 1), 3) @@ -496,6 +569,7 @@ def __repr__(self): plot_matrix(Ai.toarray(), ax=axs[i], title=f"A{i} ", colorbar=False) # type: ignore fig = plot_matrix(Q.toarray(), ax=axs[i + 1], title="Q", colorbar=False) # type: ignore + plt.show(block=False) X, info = solve_sdp(Q, constraints, verbose=False) @@ -503,7 +577,7 @@ def __repr__(self): print(f"EVR: {info_rank['EVR']:.2e}") if rank == 1: - theta_opt = lifter.get_theta(x.flatten()[1:]) + theta_opt = lifter.get_theta(x.flatten()) else: theta_opt = lifter.get_theta(x[:, :rank]) diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index d683d44..5fedd26 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -7,10 +7,10 @@ def test_rank1_vs_rankd(): """make sure that the rank-1 and rank-d lifters give the same cost for the same measurements""" np.random.seed(0) - r1 = RotationLifter(level="no", n_meas=0, n_rot=3, d=2) + r1 = RotationLifter(level="no", n_abs=0, n_rot=3, d=2) np.random.seed(0) - r2 = RotationLifter(level="bm", n_meas=0, n_rot=3, d=2) + r2 = RotationLifter(level="bm", n_abs=0, n_rot=3, d=2) np.testing.assert_array_equal(r1.theta, r2.theta) @@ -35,7 +35,7 @@ def test_rank1_vs_rankd(): def test_measurements(level="no"): """make sure that the forward model for measurements is correct""" - lifter = RotationLifter(d=2, n_meas=0, n_rot=3, sparsity="chain", level=level) + lifter = RotationLifter(d=2, n_abs=0, n_rot=3, sparsity="chain", level=level) y = lifter.simulate_y(noise=1e-10) for (i, j), R in y.items(): From 7e2a89f89094e39c6ff627b0335992ffa40016cb Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 21 Aug 2025 09:51:00 +0200 Subject: [PATCH 15/21] Move notebook to dedicated folder --- .../rotation_averaging.ipynb | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) rename {scripts => notebooks}/rotation_averaging.ipynb (65%) diff --git a/scripts/rotation_averaging.ipynb b/notebooks/rotation_averaging.ipynb similarity index 65% rename from scripts/rotation_averaging.ipynb rename to notebooks/rotation_averaging.ipynb index 95a3abc..c597ff4 100644 --- a/scripts/rotation_averaging.ipynb +++ b/notebooks/rotation_averaging.ipynb @@ -12,7 +12,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "id": "ed5be9cc-ad44-421a-b852-d44fc37c42ed", "metadata": {}, "outputs": [], @@ -33,10 +33,22 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "id": "3c8c3215", "metadata": {}, - "outputs": [], + "outputs": [ + { + "ename": "TypeError", + "evalue": "RotationLifter.__init__() got an unexpected keyword argument 'n_meas'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[2], line 7\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpopcor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mexamples\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotationLifter\n\u001b[1;32m 6\u001b[0m np\u001b[38;5;241m.\u001b[39mrandom\u001b[38;5;241m.\u001b[39mseed(\u001b[38;5;241m2\u001b[39m)\n\u001b[0;32m----> 7\u001b[0m lifter \u001b[38;5;241m=\u001b[39m \u001b[43mRotationLifter\u001b[49m\u001b[43m(\u001b[49m\u001b[43md\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mn_meas\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m 9\u001b[0m y \u001b[38;5;241m=\u001b[39m lifter\u001b[38;5;241m.\u001b[39msimulate_y(noise\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0.2\u001b[39m)\n\u001b[1;32m 11\u001b[0m theta_gt, \u001b[38;5;241m*\u001b[39m_ \u001b[38;5;241m=\u001b[39m lifter\u001b[38;5;241m.\u001b[39mlocal_solver(lifter\u001b[38;5;241m.\u001b[39mtheta, y, verbose\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m)\n", + "\u001b[0;31mTypeError\u001b[0m: RotationLifter.__init__() got an unexpected keyword argument 'n_meas'" + ] + } + ], "source": [ "import matplotlib.pyplot as plt\n", "import numpy as np\n", @@ -122,7 +134,7 @@ ], "metadata": { "kernelspec": { - "display_name": "field_guide", + "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, @@ -136,7 +148,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.16" + "version": "3.10.17" } }, "nbformat": 4, From 48cd24d0447103ca99c6cfcd9dbefd4bd280af19 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 21 Aug 2025 16:41:02 +0200 Subject: [PATCH 16/21] Fix RotationLifter 2D and 3D --- popcor/examples/rotation_lifter.py | 247 ++++++++++++++--------------- tests/test_rotation_lifter.py | 72 ++++++--- 2 files changed, 168 insertions(+), 151 deletions(-) diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 386546d..fac5c1d 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -1,8 +1,21 @@ +""" +A class for solving rotation averaging problems. + +Some notes and conventions: +- The rotation matrices are called R_i. +- We consider two different forms of "lifting": + - "no" corresponds to the rank-1 version, where we define x = [1, c_1, ..., c_N] with each c_i = R_i.flatten("C"), x in (d**2 x N + 1) + - "bm" corresponds to the rank-d version, where we define X.T = [R_0.T; R_1.T; ...; R_N.T], X in (d x (N + 1)*d), and R_0 is the world frame. +- According to above conventions, HOM is either 1 or R_0, the world frame. +- Relative measurements are defined such that R_j = R_i @ R_ij + +""" + from typing import List import numpy as np from poly_matrix.poly_matrix import PolyMatrix -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation from popcor.base_lifters import StateLifter @@ -11,6 +24,8 @@ min_gradient_norm=1e-7, max_iterations=10000, min_step_size=1e-8, verbosity=1 ) +DEBUG = True + def get_orthogonal_constraints(key, hom, d, level): A_list = [] @@ -105,15 +120,15 @@ def var_dict(self): def sample_theta(self): """Generate a random new feasible point.""" - C = np.empty((self.n_rot * self.d, self.d)) + C = np.empty((self.d, self.n_rot * self.d)) for i in range(self.n_rot): if self.d == 2: angle = np.random.uniform(0, 2 * np.pi) - C[i * self.d : (i + 1) * self.d, :] = R.from_euler( + C[:, i * self.d : (i + 1) * self.d] = Rotation.from_euler( "z", angle ).as_matrix()[:2, :2] elif self.d == 3: - C[i * self.d : (i + 1) * self.d, :] = R.random().as_matrix() + C[:, i * self.d : (i + 1) * self.d] = Rotation.random().as_matrix() return C def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: @@ -132,7 +147,7 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: x_data.append(1.0) elif "c" in key: i = int(key.split("_")[1]) - x_data += list(theta[i * self.d : (i + 1) * self.d].flatten("C")) + x_data += list(theta[:, i * self.d : (i + 1) * self.d].flatten("F")) else: raise ValueError(f"untreated key {key}") dim_x = self.get_dim_x(var_subset=var_subset) @@ -144,7 +159,7 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray: x_data.append(np.eye(self.d)) elif "c" in key: i = int(key.split("_")[1]) - x_data.append(theta[i * self.d : (i + 1) * self.d]) + x_data.append(theta[:, i * self.d : (i + 1) * self.d].T) else: raise ValueError(f"untreated key {key}") return np.vstack(x_data) @@ -156,48 +171,54 @@ def get_theta(self, x: np.ndarray) -> np.ndarray: if np.ndim(x) == 2: assert x.shape[1] == 1 C_flat = x[1 : 1 + self.n_rot * self.d**2] - return C_flat.reshape((self.n_rot * self.d, self.d)) + return C_flat.reshape((self.d, self.n_rot * self.d), order="F") elif self.level == "bm": + # Remember that x is composed of R_0.T, R_1.T, ..., R_N.T + # We want to return theta in the form [R*_1, R*_2, ..., R*_N] + # where each R*_i := R_0.T @ R_i + # d x d - R0 = x[: self.d, : self.d] - # nd x d + R0 = x[: self.d, : self.d].T + + # nd x d: [R_1.T; R_2.T; ...; R_N.T] Ri = np.array(x[self.d : (self.n_rot + 1) * self.d, : self.d]) - # below is a block-diagonal matrix - return np.kron(np.eye(self.n_rot), R0.T) @ Ri + + # return d x nd: [R*_1, R*_2, ..., R*_N] + Ri_world = R0.T @ Ri.T + return Ri_world else: raise ValueError(f"Unknown level {self.level} for RotationLifter") def add_relative_measurement(self, i, j, noise) -> np.ndarray: """ - _ - || R_i - R_ij @ R_j ||_F^2 - _ - measurements: R_ij = R_i @ R_j.T + || R_j - R_i @ R_ij ||_F^2 + + measurements: R_ij = R_i.T @ R_j """ - R_i = self.theta[i * self.d : (i + 1) * self.d, :] - R_j = self.theta[j * self.d : (j + 1) * self.d, :] - R_gt = R_i @ R_j.T + R_i = self.theta[:, i * self.d : (i + 1) * self.d] + R_j = self.theta[:, j * self.d : (j + 1) * self.d] + R_gt = R_i.T @ R_j # Generate a random small rotation as noise and apply it if noise > 0: noise_rotvec = np.random.normal(scale=noise, size=(self.d,)) Rnoise = ( - R.from_rotvec(noise_rotvec).as_matrix() + Rotation.from_rotvec(noise_rotvec).as_matrix() if self.d == 3 - else R.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] + else Rotation.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] ) return R_gt @ Rnoise else: return R_gt def add_absolute_measurement(self, i, noise, n_meas=1) -> List[np.ndarray]: - """_ - || R_i - R_i || + """ + || R_i - R_w @ R_wi || - measurements: _ - R_i = R_i @ noise + measurements: + R_wi = R_w.T @ R_i """ - R_gt = self.theta[i * self.d : (i + 1) * self.d, :] + R_gt = self.theta[:, i * self.d : (i + 1) * self.d] y = [] for _ in range(n_meas): # noise model: R_i = R.T @ Rnoise @@ -205,9 +226,9 @@ def add_absolute_measurement(self, i, noise, n_meas=1) -> List[np.ndarray]: # 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() + Rotation.from_rotvec(noise_rotvec).as_matrix() if self.d == 3 - else R.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] + else Rotation.from_euler("z", noise_rotvec[0]).as_matrix()[:2, :2] ) Ri = R_gt @ Rnoise else: @@ -224,7 +245,7 @@ def simulate_y(self, noise: float | None = None) -> dict: for i in range(self.n_rot): y[i] = self.add_absolute_measurement(i, noise, self.n_abs) else: - y[0] = self.add_absolute_measurement(0, 0.0, 1) + y[0] = self.add_absolute_measurement(0, 0.0, 1) # add prior if self.n_rel > 0: if self.sparsity == "chain": @@ -243,123 +264,97 @@ def get_Q(self, noise: float | None = None, output_poly: bool = False): return self.get_Q_from_y(self.y_, output_poly=output_poly) - def get_L(self, theta=None): - """This function is deprecated as we now introduce a homogeneous variable R0. - - Returns matrix L from the cost term, so that we can add non-quadratic terms. - F is the fixed rotation matrix - - || R - F ||_F = tr(R'R) - 2 tr(F'R) + tr(F'F) - = 2 tr(I) - 2 tr(F'R) - - # R is Nd x d - argmin "" = argmin -2 * tr(F'R_0) - = argmin -2 * vec(F).T @ vec(R_0) - - will add trace(L'R), so L is of shape Nd x d - """ - if theta is None: - theta = self.theta - R0 = theta[: self.d, : self.d] - - if self.level == "bm": - L = PolyMatrix(symmetric=False) - L["c_0", "width"] = -R0 - return L.get_matrix(variables=(self.var_dict, {"width": self.d})) - elif self.level == "no": - L = PolyMatrix(symmetric=False) - L["c_0", "width"] = -R0.flatten("C") - return L.get_matrix(variables=(self.var_dict, {"width": 1})) - else: - raise ValueError(f"Unknown level {self.level} for RotationLifter") - def get_Q_from_y(self, y, output_poly=False): """param y: list of noisy rotation matrices.""" Q = PolyMatrix() for key, R in y.items(): - # treat unary factors - # f(R) = sum_i || R - Ri ||_F^2 - # argmin f(R) = argmin sum_i tr((R - Ri)'(R - Ri)) - # = argmin sum_i -2 tr(Ri.T @ R) - # tr(A.T @ B) = vec(A).T @ vec(B) - # = argmin sum_i -2 vec(Ri).T @ vec(R) if isinstance(key, int): + # loop over all absolute measurements of this rotation. assert isinstance(R, list) - for Ri in R: + for Rk in R: if self.level == "no": - Q[self.HOM, f"c_{key}"] -= Ri.flatten("C")[None, :] - Q[self.HOM, self.HOM] += len(R) * self.d + # treat unary factors + # Rk is measured + # f(R) = sum_k || R - Rk ||_F^2 + # = sum_k 2tr(I) - 2tr(R'Rk) + Q_test = PolyMatrix() + Q_test[self.HOM, f"c_{key}"] -= Rk.flatten("F")[None, :] + # Q_test[self.HOM, self.HOM] += 2 * self.d + if DEBUG: + x = self.get_x() + cost_x = x.T @ Q_test.get_matrix(self.var_dict) @ x + Ri = self.theta[:, key * self.d : (key + 1) * self.d] + cost_R = np.linalg.norm(Ri - Rk) ** 2 - 2 * self.d + assert abs(cost_x - cost_R) < 1e-10 + Q += Q_test + elif self.level == "bm": - Q[self.HOM, f"c_{key}"] -= Ri + # in this case, we use self.HOM as a world frame. + # f(R) = sum_i || R - HOM @ Rk || + # compare with below: + # R corresponds to Rj, HOM corresponds to Ri + Q_test = PolyMatrix() + Q_test[self.HOM, f"c_{key}"] -= Rk + if DEBUG: + x = self.get_x() + cost_x = np.trace( + x.T @ Q_test.get_matrix(self.var_dict) @ x + ) + Ri = self.theta[:, key * self.d : (key + 1) * self.d] + cost_R = np.linalg.norm(Ri - Rk) ** 2 - 2 * self.d + assert abs(cost_x - cost_R) < 1e-10 + Q += Q_test # treat binary factors - # f(R) = sum_ij || Ri - Rij @ Rj ||_F^2 - # argmin f(R) = argmin sum_i -2 tr(Ri.T @ R_ij @ R_j) - # = tr(R_ij @ R_j @ Ri.T) - # tr(A.T @ C @ B) = vec(A).T @ (I kron C) @ vec(B) - # = argmin sum_i -2 tr((I kron R_ij) @ vec(R_j) vec(Ri).T) + # f(R) = sum_ij || Rj - Ri @ Rij ||_F^2 + # = sum_ij tr((Rj - Ri @ Rij)'(Rj - Ri @ Rij)) + # = sum_ij tr(Rj'Rj) - 2 tr(Rj'Ri Rij) + tr(Rij'Ri'RiRij) + # = sum_ij 2tr(I) - 2tr(Rij Rj' Ri) + # = sum_ij 2tr(I) - 2c_i' (Rij kron I) c_j elif isinstance(key, tuple): i, j = key if self.level == "no": - Q[f"c_{j}", f"c_{i}"] -= np.kron(np.eye(self.d), R) + Q_test = PolyMatrix() + # Q_test[self.HOM, self.HOM] += 2 * self.d + Q_test[f"c_{i}", f"c_{j}"] = -np.kron(R, np.eye(self.d)) + if DEBUG: + x = self.get_x() + assert ( + x[1:].T @ x[1:] - np.trace(self.theta.T @ self.theta) + ) < 1e-10 + Ri = self.theta[:, i * self.d : (i + 1) * self.d] + Rj = self.theta[:, j * self.d : (j + 1) * self.d] + c_j = x[1 + j * self.d**2 : 1 + (j + 1) * self.d**2] + c_i = x[1 + i * self.d**2 : 1 + (i + 1) * self.d**2] + np.testing.assert_allclose(Ri.flatten("F"), c_i, atol=1e-5) + np.testing.assert_allclose(Rj.flatten("F"), c_j, atol=1e-5) + + tr_R = np.trace(R @ Rj.T @ Ri) + tr_c = c_i.T @ (np.kron(R, np.eye(self.d)) @ c_j) + assert abs(tr_R - tr_c) < 1e-10 + + cost_x = x.T @ Q_test.get_matrix(self.var_dict) @ x + cost_R = np.linalg.norm(Rj - Ri @ R) ** 2 - 2 * self.d + assert abs(cost_x - cost_R) < 1e-10 + Q += Q_test elif self.level == "bm": - Q[f"c_{j}", f"c_{i}"] -= R + Q_test = PolyMatrix() + Q_test[f"c_{i}", f"c_{j}"] = -R + if DEBUG: + x = self.get_x() + Ri = self.theta[:, i * self.d : (i + 1) * self.d] + Rj = self.theta[:, j * self.d : (j + 1) * self.d] + + cost_x = np.trace(x.T @ Q_test.get_matrix(self.var_dict) @ x) + cost_R = np.linalg.norm(Rj - Ri @ R) ** 2 - 2 * self.d + assert abs(cost_x - cost_R) < 1e-9 + Q += Q_test if output_poly: return Q else: return Q.get_matrix(self.var_dict) - def local_solver_old( - self, t0, y, verbose=False, method=METHOD, solver_kwargs=SOLVER_KWARGS - ): - """Not used anymore, kept for reference. We now use the default - QCQP local solver.""" - import pymanopt - from pymanopt.manifolds import SpecialOrthogonalGroup - - if method == "CG": - from pymanopt.optimizers import ConjugateGradient as Optimizer # fastest - elif method == "SD": - from pymanopt.optimizers import SteepestDescent as Optimizer # slow - elif method == "TR": - from pymanopt.optimizers import TrustRegions as Optimizer # okay - else: - raise ValueError(method) - - if verbose: - solver_kwargs["verbosity"] = 2 - else: - solver_kwargs["verbosity"] = 0 - - manifold = SpecialOrthogonalGroup(self.d, k=1) - - @pymanopt.function.autograd(manifold) - def cost(R): - cost = 0 - for Ri in y: - cost += np.sum((R.T @ Ri - np.eye(self.d)) ** 2) - return cost - - euclidean_gradient = None - problem = pymanopt.Problem( - manifold, cost, euclidean_gradient=euclidean_gradient - ) - optimizer = Optimizer(**solver_kwargs) - - res = optimizer.run(problem, initial_point=t0) - theta_hat = res.point - - success = ("min step_size" in res.stopping_criterion) or ( - "min grad norm" in res.stopping_criterion - ) - info = { - "success": success, - "msg": res.stopping_criterion, - } - if success: - return theta_hat, info, cost - def test_and_add(self, A_list, Ai, output_poly, b_list=[], bi=0.0): x = self.get_x() Ai_sparse = Ai.get_matrix(self.var_dict) @@ -521,9 +516,9 @@ def __repr__(self): if __name__ == "__main__": import matplotlib.pyplot as plt import numpy as np - from cert_tools.linalg_tools import rank_project from cert_tools.sdp_solvers import solve_sdp + from popcor.utils.plotting_tools import plot_matrix level = "no" diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index 5fedd26..aa4f729 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -1,16 +1,23 @@ import numpy as np +import pytest from popcor.examples import RotationLifter -def test_rank1_vs_rankd(): - """make sure that the rank-1 and rank-d lifters give the same cost - for the same measurements""" +@pytest.mark.parametrize("d", [2, 3]) +def test_rank1_vs_rankd(d): + """ + Make sure that the rank-1 and rank-d lifters give the same cost + for the same measurements + """ + n_rot = 3 + n_abs = 2 + np.random.seed(0) - r1 = RotationLifter(level="no", n_abs=0, n_rot=3, d=2) + r1 = RotationLifter(level="no", n_abs=n_abs, n_rot=n_rot, d=d) np.random.seed(0) - r2 = RotationLifter(level="bm", n_abs=0, n_rot=3, d=2) + r2 = RotationLifter(level="bm", n_abs=n_abs, n_rot=n_rot, d=d) np.testing.assert_array_equal(r1.theta, r2.theta) @@ -19,42 +26,57 @@ def test_rank1_vs_rankd(): x1 = r1.get_x() x2 = r2.get_x() - Q1 = r1.get_Q_from_y(y) - Q2 = r2.get_Q_from_y(y) + Q1 = r1.get_Q_from_y(y, output_poly=False) + Q2 = r2.get_Q_from_y(y, output_poly=False) cost1 = x1.T @ Q1 @ x1 - cost2 = np.trace(x2.T @ Q2 @ x2) + cost2 = np.trace(x2.T @ Q2 @ x2) # + (n_rot + n_abs + 1) * d assert abs(cost1 - cost2) < 1e-10 # actual cost should be # 2 sum_ij tr(I) - 2 sum_ij tr(Ri.T@R_ij@R_j) # ==========our cost ========= - cost_optimal = cost1 + 2 * len(y) * r1.d + cost_optimal = cost1 + 2 * (n_abs * n_rot + n_rot - 1) * d assert abs(cost_optimal) < 1e-8 -def test_measurements(level="no"): - """make sure that the forward model for measurements is correct""" - lifter = RotationLifter(d=2, n_abs=0, n_rot=3, sparsity="chain", level=level) +@pytest.mark.parametrize("d", [2, 3]) +def test_measurements(d, level="no"): + """ + Make sure that the forward model for measurements is correct + """ + np.random.seed(0) + n_abs = 2 + n_rot = 3 + d = 3 + lifter = RotationLifter( + d=d, n_abs=n_abs, n_rot=n_rot, sparsity="chain", level=level + ) y = lifter.simulate_y(noise=1e-10) - for (i, j), R in y.items(): - R_i = lifter.theta[i * lifter.d : (i + 1) * lifter.d, :] - R_j = lifter.theta[j * lifter.d : (j + 1) * lifter.d, :] - np.testing.assert_allclose(R_i @ R_j.T, R, atol=1e-5) + for key, R in y.items(): + if isinstance(key, int): + # unary factor + for Rk in R: + R_i = lifter.theta[:, key * lifter.d : (key + 1) * lifter.d] + np.testing.assert_allclose(R_i, Rk, atol=1e-5) + elif isinstance(key, tuple): + # binary factor + i, j = key + R_i = lifter.theta[:, i * lifter.d : (i + 1) * lifter.d] + R_j = lifter.theta[:, j * lifter.d : (j + 1) * lifter.d] + np.testing.assert_allclose(R_i.T @ R_j, R, atol=1e-5) x = lifter.get_x() - rank = x.shape[1] if np.ndim(x) == 2 else 1 - theta = lifter.get_theta(x) - for (i, j), R in y.items(): - R_i = theta[i * lifter.d : (i + 1) * lifter.d, :] - R_j = theta[j * lifter.d : (j + 1) * lifter.d, :] - np.testing.assert_allclose(R_i @ R_j.T, R, atol=1e-5) + np.testing.assert_allclose(theta, lifter.theta) if __name__ == "__main__": - test_measurements(level="no") - test_measurements(level="bm") - test_rank1_vs_rankd() + test_measurements(d=2, level="no") + test_measurements(d=2, level="bm") + test_measurements(d=3, level="no") + test_measurements(d=3, level="bm") + test_rank1_vs_rankd(d=2) + test_rank1_vs_rankd(d=3) print("all tests passed") From dc16d6eb66c7b2f8df8cb8e0393e887ae9c49146 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 21 Aug 2025 16:48:03 +0200 Subject: [PATCH 17/21] Fix solver tests --- popcor/base_lifters/state_lifter.py | 9 ++++++++- popcor/examples/example_lifter.py | 4 ++-- popcor/examples/rotation_lifter.py | 7 +++++++ 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index 07f1257..0dc9503 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -158,7 +158,14 @@ def get_cost(self, theta, y: np.ndarray | None = None) -> float: else: Q = self.get_Q() x = self.get_x(theta=theta) - return float(np.trace(x.T @ Q @ x)) + if np.ndim(x) == 1: + return float(x.T @ Q @ x) + elif np.ndim(x) == 2: + return float(np.trace(x.T @ Q @ x)) + else: + raise ValueError( + f"Unexpected shape of x: {x.shape}. Must be 1D or 2D array." + ) def local_solver( self, diff --git a/popcor/examples/example_lifter.py b/popcor/examples/example_lifter.py index 2b5c59c..5daaea9 100644 --- a/popcor/examples/example_lifter.py +++ b/popcor/examples/example_lifter.py @@ -58,7 +58,7 @@ def get_x( return np.array(x_data) def sample_parameters(self, theta: np.ndarray | None = None) -> dict: - pass + return {} def sample_theta(self) -> np.ndarray: - pass + return np.array([]) diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index fac5c1d..7a35bf3 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -111,9 +111,11 @@ def __init__( @property def var_dict(self): if self.level == "no": + # self.HOM is the scalar homogenization variable var_dict = {self.HOM: 1} var_dict.update({f"c_{i}": self.d**2 for i in range(self.n_rot)}) else: + # self.HOM is the world frame, which is d x d var_dict = {self.HOM: self.d} var_dict.update({f"c_{i}": self.d for i in range(self.n_rot)}) return var_dict @@ -280,6 +282,8 @@ def get_Q_from_y(self, y, output_poly=False): # = sum_k 2tr(I) - 2tr(R'Rk) Q_test = PolyMatrix() Q_test[self.HOM, f"c_{key}"] -= Rk.flatten("F")[None, :] + # Not adding below to be consistent with "bm" case, where we cannot + # add a constant term to the cost. # Q_test[self.HOM, self.HOM] += 2 * self.d if DEBUG: x = self.get_x() @@ -315,6 +319,9 @@ def get_Q_from_y(self, y, output_poly=False): i, j = key if self.level == "no": Q_test = PolyMatrix() + + # Not adding below to be consistent with "bm" case, where we cannot + # add a constant term to the cost. # Q_test[self.HOM, self.HOM] += 2 * self.d Q_test[f"c_{i}", f"c_{j}"] = -np.kron(R, np.eye(self.d)) if DEBUG: From eda6dab1bedf93340fcdce4a446aa3ae664a432f Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 21 Aug 2025 20:32:31 +0200 Subject: [PATCH 18/21] Add solver tests for rotation averaging --- notebooks/rotation_averaging.ipynb | 80 +++++++++++++++------- popcor/base_lifters/state_lifter.py | 3 - tests/test_rotation_lifter.py | 100 ++++++++++++++++++++++++++++ 3 files changed, 157 insertions(+), 26 deletions(-) diff --git a/notebooks/rotation_averaging.ipynb b/notebooks/rotation_averaging.ipynb index c597ff4..88a3fd7 100644 --- a/notebooks/rotation_averaging.ipynb +++ b/notebooks/rotation_averaging.ipynb @@ -12,7 +12,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "ed5be9cc-ad44-421a-b852-d44fc37c42ed", "metadata": {}, "outputs": [], @@ -33,22 +33,10 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "id": "3c8c3215", "metadata": {}, - "outputs": [ - { - "ename": "TypeError", - "evalue": "RotationLifter.__init__() got an unexpected keyword argument 'n_meas'", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[2], line 7\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpopcor\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mexamples\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotationLifter\n\u001b[1;32m 6\u001b[0m np\u001b[38;5;241m.\u001b[39mrandom\u001b[38;5;241m.\u001b[39mseed(\u001b[38;5;241m2\u001b[39m)\n\u001b[0;32m----> 7\u001b[0m lifter \u001b[38;5;241m=\u001b[39m \u001b[43mRotationLifter\u001b[49m\u001b[43m(\u001b[49m\u001b[43md\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mn_meas\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m3\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m 9\u001b[0m y \u001b[38;5;241m=\u001b[39m lifter\u001b[38;5;241m.\u001b[39msimulate_y(noise\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0.2\u001b[39m)\n\u001b[1;32m 11\u001b[0m theta_gt, \u001b[38;5;241m*\u001b[39m_ \u001b[38;5;241m=\u001b[39m lifter\u001b[38;5;241m.\u001b[39mlocal_solver(lifter\u001b[38;5;241m.\u001b[39mtheta, y, verbose\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m)\n", - "\u001b[0;31mTypeError\u001b[0m: RotationLifter.__init__() got an unexpected keyword argument 'n_meas'" - ] - } - ], + "outputs": [], "source": [ "import matplotlib.pyplot as plt\n", "import numpy as np\n", @@ -56,7 +44,7 @@ "from popcor.examples import RotationLifter\n", "\n", "np.random.seed(2)\n", - "lifter = RotationLifter(d=3, n_meas=3)\n", + "lifter = RotationLifter(d=3, n_abs=1, n_rot=4)\n", "\n", "y = lifter.simulate_y(noise=0.2)\n", "\n", @@ -93,11 +81,12 @@ "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", - "fig = plot_matrix(Q.toarray(), ax=axs[i+1], title=\"Q\", colorbar=False)" + "A_plot = A_known[:5]\n", + "fig, axs = plt.subplots(1, len(A_plot) + 1)\n", + "fig.set_size_inches(3 * (len(A_plot) + 1), 3)\n", + "for i in range(len(A_plot)):\n", + " plot_matrix(A_plot[i].toarray(), ax=axs[i], title=f\"A{i} \", colorbar=False)\n", + "fig = plot_matrix(Q.toarray(), ax=axs[i + 1], title=\"Q\", colorbar=False)" ] }, { @@ -115,7 +104,7 @@ "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()[1:])\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)" @@ -128,8 +117,53 @@ "source": [ "## Conclusion\n", "\n", - "This problem is too easy! No redundant measurements are required for tightness. " + "This problem quite easy! No redundant measurements are required for tightness, even up to very high noise levels." ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "563a21f1-3705-4dc7-99cd-01b916d56184", + "metadata": {}, + "outputs": [], + "source": [ + "data = []\n", + "for noise in np.logspace(-3, 2, 6):\n", + " for i in range(10):\n", + " np.random.seed(i)\n", + " lifter = RotationLifter(d=3, n_abs=1, n_rot=4)\n", + " y = lifter.simulate_y(noise=noise)\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", + " X, info = solve_sdp(Q, constraints, verbose=False)\n", + " x, info_rank = rank_project(X, p=1)\n", + " data.append({\"seed\": i, \"noise\": noise, \"EVR\": info_rank[\"EVR\"]})" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "df6b3146", + "metadata": {}, + "outputs": [], + "source": [ + "import pandas as pd\n", + "import seaborn as sns\n", + "df = pd.DataFrame(data)\n", + "fig, ax =plt.subplots()\n", + "sns.scatterplot(df, x=\"noise\", y=\"EVR\", ax=ax)\n", + "ax.set_xscale(\"log\")\n", + "ax.set_yscale(\"log\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fbf60376", + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { diff --git a/popcor/base_lifters/state_lifter.py b/popcor/base_lifters/state_lifter.py index 0dc9503..0000f34 100644 --- a/popcor/base_lifters/state_lifter.py +++ b/popcor/base_lifters/state_lifter.py @@ -178,9 +178,6 @@ def local_solver( 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. """ - print( - "Warning: using default local_solver, which may be less efficient than a custom one." - ) if len(args): print(f"Warning: ignore args {args}") from cert_tools.sdp_solvers import solve_low_rank_sdp diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index aa4f729..dea6eb4 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -1,7 +1,27 @@ +import math + +import matplotlib.pyplot as plt import numpy as np import pytest +import scipy.sparse as sp +from cert_tools.linalg_tools import rank_project +from cert_tools.sdp_solvers import solve_sdp from popcor.examples import RotationLifter +from popcor.utils.plotting_tools import plot_matrix + + +def plot_matrices(A_known, Q): + n_cols = min(1 + len(A_known), 10) + n_rows = math.ceil((len(A_known) + 1) / n_cols) + fig, axs = plt.subplots(n_rows, n_cols) + fig.set_size_inches(n_rows * n_cols, 3 * n_rows) + axs = axs.flatten() + for i in range(len(A_known)): + assert isinstance(A_known[i], sp.csc_matrix) + plot_matrix(A_known[i].toarray(), ax=axs[i], title=f"A{i} ", colorbar=False) + fig = plot_matrix(Q.toarray(), ax=axs[i + 1], title="Q", colorbar=False) + [axs[j].axis("off") for j in range(i + 1, len(axs))] @pytest.mark.parametrize("d", [2, 3]) @@ -72,11 +92,91 @@ def test_measurements(d, level="no"): np.testing.assert_allclose(theta, lifter.theta) +def test_solve_local(): + + d = 3 + n_abs = 1 + n_rot = 4 + for noise in [0.0, 0.2]: + np.random.seed(2) + lifter = RotationLifter( + d=d, n_abs=n_abs, n_rot=n_rot, sparsity="chain", level="no" + ) + + y = lifter.simulate_y(noise=noise) + + theta_gt, *_ = lifter.local_solver(lifter.theta, y, verbose=False) + estimates = {"init gt": theta_gt} + for i in range(10): + theta_init = lifter.sample_theta() + theta_i, *_ = lifter.local_solver(theta_init, y, verbose=False) + + # make sure we are starting relatively far from ground truth + assert np.any(np.abs(theta_init - theta_gt) > 1e-1) + estimates[f"init random {i}"] = theta_i + + # make sure that we converge to the ground truth in zero-noise setting + if noise == 0.0: + np.testing.assert_allclose(theta_i, lifter.theta, atol=1e-5) + else: + # 0.5 is found empirically + np.testing.assert_allclose(theta_i, lifter.theta, atol=0.5) + + fig, ax = lifter.plot(estimates=estimates) + handles, labels = ax.get_legend_handles_labels() + ax.legend(handles[:3], labels[:3], loc="lower left", bbox_to_anchor=(0.0, 1.0)) + plt.show(block=False) + print("done") + + +def test_solve_sdp(): + d = 3 + n_abs = 1 + n_rot = 4 + estimates = {} + for noise in [0.0, 0.2]: + np.random.seed(2) + lifter = RotationLifter( + d=d, n_abs=n_abs, n_rot=n_rot, sparsity="chain", level="no" + ) + + y = lifter.simulate_y(noise=noise) + Q = lifter.get_Q_from_y(y=y, output_poly=False) + + assert isinstance(Q, sp.csc_matrix) + A_known = lifter.get_A_known() + constraints = lifter.get_A_b_list(lifter.get_A_known()) + + # if noise == 0: + # plot_matrices(A_known, Q) + + X, info = solve_sdp(Q, constraints, verbose=False) + + x, info_rank = rank_project(X, p=1) + print(f"EVR at noise {noise:.2f}: {info_rank['EVR']:.2e}") + if noise <= 1: + assert info_rank["EVR"] > 1e8 + + theta_sdp = lifter.get_theta(x.flatten()) + if noise == 0.0: + np.testing.assert_allclose(theta_sdp, lifter.theta, atol=1e-5) + + estimates.update({"init gt": lifter.theta, f"SDP noise {noise:.2f}": theta_sdp}) + fig, ax = lifter.plot(estimates=estimates) + plt.show(block=False) + return + + if __name__ == "__main__": + test_solve_sdp() + test_solve_local() + test_measurements(d=2, level="no") test_measurements(d=2, level="bm") test_measurements(d=3, level="no") test_measurements(d=3, level="bm") + test_rank1_vs_rankd(d=2) test_rank1_vs_rankd(d=3) + print("all tests passed") From feb6650716c1c993346cf865823ec49a178d4652 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Thu, 21 Aug 2025 20:32:45 +0200 Subject: [PATCH 19/21] Improve documentation instructions --- CONTRIBUTING.md | 8 ++++++ Makefile | 2 +- popcor/examples/rotation_lifter.py | 40 +++++++++++++++++++++++++++--- 3 files changed, 45 insertions(+), 5 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 1ab723a..4666351 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -74,3 +74,11 @@ curl --proto '=https' --tlsv1.2 -sSf https://raw.githubusercontent.com/nektos/ac ``` sudo ./bin/act ``` + +### Running documentation locally + +To generate a live-reloading preview of Sphinx docs while editing, you can run, from this root folder +of the repository: +``` +make doc +``` diff --git a/Makefile b/Makefile index 3a6a301..7892a45 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ doc: - sphinx-autobuild docs/source docs/build + sphinx-autobuild docs/source docs/build --watch popcor/ doctest: sphinx-build -b doctest docs/source docs/build/doctest diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py index 7a35bf3..ff7f5d2 100644 --- a/popcor/examples/rotation_lifter.py +++ b/popcor/examples/rotation_lifter.py @@ -68,9 +68,40 @@ def get_orthogonal_constraints(key, hom, d, level): class RotationLifter(StateLifter): """Rotation averaging problem. - - level "no" corresponds to the rank-1 version. - - level "bm" corresponds to the rank-d version (bm=Bourer Monteiro, for later extension). + We solve the following optimization problem: + .. math:: + f(\\theta) = \\min_{R_0, R_1, \\ldots, R_N \\in \\mathrm{SO}^d} + \\sum_{i,j \\in \\mathcal{E}} || R_i - R_j \\tilde{R}_{ij} ||_F^2 + + \\sum_{i=\\in\\mathcal{A}} || R_i - \\tilde{R}_i ||_F^2 + + where :math:`\\tilde{R}_{ij}` are the relative measurements, :math:`\\tilde{R}_{i}` are the absolute measurements, + and the unknowns are + + .. math:: + \\theta = \\begin{bmatrix} R_1 & R_2 & \\ldots & R_N \\end{bmatrix} + + + We can alternatively replace the absolute-measurement terms by + + .. math:: + || R_i - R_w \\tilde{R}_{i} ||_F^2 + + where :math:`R_w` is an arbitrary world frame that we can also optimize over, transforming the solutions + by :math:`R_w^{-1}R_i` after to move the world frame to the origin. Using this formulation, all + measurements are binary factors, which may simplify implementation. + + We consider two different formulations of the problem: + + - level "no" corresponds to the rank-1 version: + + .. math:: + x = \\begin{bmatrix} 1, \\mathrm{vec}(R_1), \\ldots, \\mathrm{vec}(R_N) \\end{bmatrix}^T + + - level "bm" corresponds to the rank-d version (bm=Burer-Monteiro). + + .. math:: + X = \\begin{bmatrix} R_1^\\top \\\\ \\vdots \\\\ R_N^\\top \\end{bmatrix} """ LEVELS = ["no", "bm"] @@ -172,6 +203,7 @@ def get_theta(self, x: np.ndarray) -> np.ndarray: if self.level == "no": if np.ndim(x) == 2: assert x.shape[1] == 1 + C_flat = x[1 : 1 + self.n_rot * self.d**2] return C_flat.reshape((self.d, self.n_rot * self.d), order="F") elif self.level == "bm": @@ -489,7 +521,7 @@ def plot(self, estimates={}): for i in range(self.n_rot): plot_frame( ax=ax, - theta=self.theta[i * self.d : (i + 1) * self.d, :], + theta=self.theta[:, i * self.d : (i + 1) * self.d], label=label, ls="-", scale=0.5, @@ -503,7 +535,7 @@ def plot(self, estimates={}): for i in range(self.n_rot): plot_frame( ax=ax, - theta=theta[i * self.d : (i + 1) * self.d, :], + theta=theta[:, i * self.d : (i + 1) * self.d], label=label, ls=next(linestyles), scale=1.0, From f4326e5d48ffa161d5a7eda830d60231a0494563 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 22 Aug 2025 10:45:50 +0200 Subject: [PATCH 20/21] Complete changelog and improve doc --- CHANGELOG.md | 26 ++++++++++++-------- popcor/base_lifters/_base_class.py | 4 ++-- tests/test_rotation_lifter.py | 38 ++++++++++++++++-------------- 3 files changed, 38 insertions(+), 30 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 87eae0e..cdd1862 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,23 +5,29 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). -## [Unreleased] -- 2025-06-13 +## [Unreleased] -- 2025-08-22 -(00-changelog)= +(0.0.0a)= ### Added -- RangeOnlyLifter: sample_landmarks_filling_space -- RotationLifter: support to plot 2d frames - -(01-changelog)= +- RotationLifter: support to plot 2d frames, improved documentation for rotation conventions +- RotationLifter: implemented rank-d ("bm") lifting +- RangeOnlyLifter: new landmark sampling methods to fill available space +- Documentation instructions in CONTRIBUTING.md +- Unit tests and notebook for RotationLifter +- StateLifter: started support for rank-d instead of rank-1 formulations + +(0.0.0c)= ### Changed +- RangeOnlyLifter: default sampling for landmarks in RO is now the one filling space +- RangeOnlyLifter: theta is sampled at least MIN_DIST away from landmarks -(02-changelog)= +(0.0.0a)= ### Fixed - Link fixes in documentation ## [0.0.2] - 2025-06-04 -(6-changelog)= +(0.0.2a)= ### Added - RangeOnlyLifter base class for 2D/3D range-only localization (base_lifters/range_only_lifter.py) @@ -29,12 +35,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - RangeOnlySqLifter: specialized lifter for squared-distance-based range-only localization - GitHub Pages deployment step to documentation.yml using peaceiris/actions-gh-pages for automatic documentation publishing -(5-changelog)= +(0.0.2r)= ### Removed - docs/build/ files -(4-changelog)= +(0.0.2f)= ### Fixed - Corrected return type annotations in get_grad and get_hess methods in state_lifter.py from float to np.ndarray diff --git a/popcor/base_lifters/_base_class.py b/popcor/base_lifters/_base_class.py index e282880..6342d0e 100644 --- a/popcor/base_lifters/_base_class.py +++ b/popcor/base_lifters/_base_class.py @@ -687,8 +687,8 @@ def get_A0(self, var_subset=None): def get_A_b_list(self, A_list, var_subset=None): """get equality constraint tuples (Ai, bi) s.t. x.t @ Ai @ bi, 0 - :param A_list: normally, this is just the list of equality constaints that equal zero. We will add the homogenization. - for certain cases, such as the RotationLifter with level="bm", this is already a tuple of (Ai, bi). + :param A_list: Normally, this is just the list of equality constaints that equal zero. We will add the homogenization. + For certain cases, such as the RotationLifter with level="bm", this is already a tuple of (Ai, bi). """ if var_subset is None: diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index dea6eb4..5e9a80e 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -1,3 +1,10 @@ +""" +Tests for RotationLifter behaviors including measurement generation, local solvers, +and semidefinite programming (SDP) relaxations. Covers consistency between rank-1 +and rank-d lifters, correctness of the forward measurement model, convergence of +a local solver under varying noise levels, and recovery via an SDP relaxation. +""" + import math import matplotlib.pyplot as plt @@ -15,7 +22,7 @@ def plot_matrices(A_known, Q): n_cols = min(1 + len(A_known), 10) n_rows = math.ceil((len(A_known) + 1) / n_cols) fig, axs = plt.subplots(n_rows, n_cols) - fig.set_size_inches(n_rows * n_cols, 3 * n_rows) + fig.set_size_inches(3 * n_cols, 3 * n_rows) axs = axs.flatten() for i in range(len(A_known)): assert isinstance(A_known[i], sp.csc_matrix) @@ -26,10 +33,7 @@ def plot_matrices(A_known, Q): @pytest.mark.parametrize("d", [2, 3]) def test_rank1_vs_rankd(d): - """ - Make sure that the rank-1 and rank-d lifters give the same cost - for the same measurements - """ + """Ensure rank-1 and rank-d RotationLifter formulations yield identical costs.""" n_rot = 3 n_abs = 2 @@ -50,21 +54,18 @@ def test_rank1_vs_rankd(d): Q2 = r2.get_Q_from_y(y, output_poly=False) cost1 = x1.T @ Q1 @ x1 - cost2 = np.trace(x2.T @ Q2 @ x2) # + (n_rot + n_abs + 1) * d + cost2 = np.trace(x2.T @ Q2 @ x2) assert abs(cost1 - cost2) < 1e-10 - # actual cost should be + # actual optimal cost should be # 2 sum_ij tr(I) - 2 sum_ij tr(Ri.T@R_ij@R_j) - # ==========our cost ========= cost_optimal = cost1 + 2 * (n_abs * n_rot + n_rot - 1) * d assert abs(cost_optimal) < 1e-8 @pytest.mark.parametrize("d", [2, 3]) def test_measurements(d, level="no"): - """ - Make sure that the forward model for measurements is correct - """ + """Verify that the simulated measurements match the lifter's internal rotations.""" np.random.seed(0) n_abs = 2 n_rot = 3 @@ -93,7 +94,7 @@ def test_measurements(d, level="no"): def test_solve_local(): - + """Test the local solver's convergence from different initializations and noise levels.""" d = 3 n_abs = 1 n_rot = 4 @@ -115,14 +116,14 @@ def test_solve_local(): assert np.any(np.abs(theta_init - theta_gt) > 1e-1) estimates[f"init random {i}"] = theta_i - # make sure that we converge to the ground truth in zero-noise setting + # check convergence tolerance depending on noise if noise == 0.0: np.testing.assert_allclose(theta_i, lifter.theta, atol=1e-5) else: # 0.5 is found empirically np.testing.assert_allclose(theta_i, lifter.theta, atol=0.5) - fig, ax = lifter.plot(estimates=estimates) + _, ax = lifter.plot(estimates=estimates) handles, labels = ax.get_legend_handles_labels() ax.legend(handles[:3], labels[:3], loc="lower left", bbox_to_anchor=(0.0, 1.0)) plt.show(block=False) @@ -130,6 +131,7 @@ def test_solve_local(): def test_solve_sdp(): + """Solve the SDP relaxation and compare the recovered rotations to ground truth.""" d = 3 n_abs = 1 n_rot = 4 @@ -145,10 +147,10 @@ def test_solve_sdp(): assert isinstance(Q, sp.csc_matrix) A_known = lifter.get_A_known() - constraints = lifter.get_A_b_list(lifter.get_A_known()) + constraints = lifter.get_A_b_list(A_known) - # if noise == 0: - # plot_matrices(A_known, Q) + if noise == 0: + plot_matrices(A_known, Q) X, info = solve_sdp(Q, constraints, verbose=False) @@ -156,13 +158,13 @@ def test_solve_sdp(): print(f"EVR at noise {noise:.2f}: {info_rank['EVR']:.2e}") if noise <= 1: assert info_rank["EVR"] > 1e8 - theta_sdp = lifter.get_theta(x.flatten()) if noise == 0.0: np.testing.assert_allclose(theta_sdp, lifter.theta, atol=1e-5) estimates.update({"init gt": lifter.theta, f"SDP noise {noise:.2f}": theta_sdp}) fig, ax = lifter.plot(estimates=estimates) + _, ax = lifter.plot(estimates=estimates) plt.show(block=False) return From 8b596a1b3c10c8d52c6c9bf41a3e9a2fbef8fe95 Mon Sep 17 00:00:00 2001 From: Frederike Duembgen Date: Fri, 22 Aug 2025 10:47:42 +0200 Subject: [PATCH 21/21] Make plot in test optional --- tests/test_rotation_lifter.py | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/tests/test_rotation_lifter.py b/tests/test_rotation_lifter.py index 5e9a80e..06e3039 100644 --- a/tests/test_rotation_lifter.py +++ b/tests/test_rotation_lifter.py @@ -17,6 +17,8 @@ from popcor.examples import RotationLifter from popcor.utils.plotting_tools import plot_matrix +PLOT = False + def plot_matrices(A_known, Q): n_cols = min(1 + len(A_known), 10) @@ -123,10 +125,13 @@ def test_solve_local(): # 0.5 is found empirically np.testing.assert_allclose(theta_i, lifter.theta, atol=0.5) - _, ax = lifter.plot(estimates=estimates) - handles, labels = ax.get_legend_handles_labels() - ax.legend(handles[:3], labels[:3], loc="lower left", bbox_to_anchor=(0.0, 1.0)) - plt.show(block=False) + if PLOT: + _, ax = lifter.plot(estimates=estimates) + handles, labels = ax.get_legend_handles_labels() + ax.legend( + handles[:3], labels[:3], loc="lower left", bbox_to_anchor=(0.0, 1.0) + ) + plt.show(block=False) print("done") @@ -149,8 +154,8 @@ def test_solve_sdp(): A_known = lifter.get_A_known() constraints = lifter.get_A_b_list(A_known) - if noise == 0: - plot_matrices(A_known, Q) + if noise == 0 and PLOT: + plot_matrices(A_known, Q) X, info = solve_sdp(Q, constraints, verbose=False) @@ -163,9 +168,11 @@ def test_solve_sdp(): np.testing.assert_allclose(theta_sdp, lifter.theta, atol=1e-5) estimates.update({"init gt": lifter.theta, f"SDP noise {noise:.2f}": theta_sdp}) - fig, ax = lifter.plot(estimates=estimates) - _, ax = lifter.plot(estimates=estimates) - plt.show(block=False) + + if PLOT: + fig, ax = lifter.plot(estimates=estimates) + _, ax = lifter.plot(estimates=estimates) + plt.show(block=False) return