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
```
diff --git a/README.md b/README.md
index 54265de..f5fbd7d 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,5 @@
[](https://github.com/duembgen/popr/actions/workflows/python-package-conda.yml)
[](https://duembgen.github.io/popcor)
-[](https://duembgen.github.io/popcor)
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" /> 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))
@@ -203,7 +233,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
@@ -246,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}")
@@ -322,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:
@@ -353,10 +392,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..6ae0c5c 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 :]
@@ -297,6 +298,7 @@ def local_solver(
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 6360ab6..df42d6a 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:
@@ -182,10 +189,15 @@ def local_solver(self, t0, y: np.ndarray | None = None, *args, **kwargs):
"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(
- 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
@@ -228,3 +240,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().flatten() 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)])
diff --git a/popcor/examples/rotation_lifter.py b/popcor/examples/rotation_lifter.py
index c6ac72b..9df3d14 100644
--- a/popcor/examples/rotation_lifter.py
+++ b/popcor/examples/rotation_lifter.py
@@ -44,7 +44,7 @@ def sample_theta(self):
C = R.from_euler("z", angle).as_matrix()[:2, :2]
elif self.d == 3:
C = R.random().as_matrix()
- return C.flatten("C")
+ return C
def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray:
"""Get the lifted vector x given theta and parameters."""
@@ -60,14 +60,15 @@ def get_x(self, theta=None, parameters=None, var_subset=None) -> np.ndarray:
if key == self.HOM:
x_data.append(1.0)
elif key == "c":
- x_data += list(theta)
+ x_data += list(theta.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
- return x[: 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:
if noise is None:
@@ -247,13 +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 __repr__(self):
- return f"rotation_lifter{self.d}d"
+ def plot(self, estimates={}):
+ import itertools
+
+ import matplotlib.pyplot as plt
+
+ 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="")
-if __name__ == "__main__":
+ linestyles = itertools.cycle(["--", "-.", ":"])
+ for label, theta in estimates.items():
+ plot_frame(
+ ax=ax,
+ theta=theta,
+ label=label,
+ ls=next(linestyles),
+ scale=1.0,
+ marker="",
+ )
- # adding this here because on save vscode sometimes adds duplicate lines
- pass
- # adding this here because on save vscode sometimes adds duplicate lines
- pass
+ ax.set_aspect("equal")
+ ax.legend()
+ return fig, ax
+
+ def __repr__(self):
+ return f"rotation_lifter{self.d}d"
diff --git a/popcor/utils/plotting_tools.py b/popcor/utils/plotting_tools.py
index 6890bc1..4adc893 100644
--- a/popcor/utils/plotting_tools.py
+++ b/popcor/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 Exception as e:
- 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/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
diff --git a/scripts/rotation_averaging.ipynb b/scripts/rotation_averaging.ipynb
new file mode 100644
index 0000000..dcd99d1
--- /dev/null
+++ b/scripts/rotation_averaging.ipynb
@@ -0,0 +1,144 @@
+{
+ "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": "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,
+ "id": "3c8c3215",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import matplotlib.pyplot as plt\n",
+ "import numpy as np\n",
+ "\n",
+ "from popcor.examples import RotationLifter\n",
+ "\n",
+ "np.random.seed(2)\n",
+ "lifter = RotationLifter(d=3, n_meas=3)\n",
+ "\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",
+ "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)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "da3c8b79-8d8e-497c-9761-21b678450217",
+ "metadata": {},
+ "source": [
+ "## Solve the rotation averaging with an SDP"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "64bbdf6b",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from popcor.utils.plotting_tools import plot_matrix\n",
+ "\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",
+ "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)"
+ ]
+ },
+ {
+ "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()[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": "Python 3 (ipykernel)",
+ "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.17"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}
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}"
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