Add more examples

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 
 
 ```

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)
 

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,34 +78,45 @@ 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),
             ]
         )
         return landmarks, theta
 
     @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)
         )
         # remove landmarks a bit from border for plotting reasons
-        landmarks = (landmarks + SCALE * 0.05) * SCALE * 0.9
-        theta = np.random.uniform(
-            [np.min(landmarks, axis=0)], [np.max(landmarks, axis=0)]
+        landmarks = (
+            (landmarks + RangeOnlyLifter.SCALE * 0.05) * RangeOnlyLifter.SCALE * 0.9
         )
-        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_
@@ -158,7 +171,24 @@ 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)."""
+        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))
@@ -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:

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
 

popcor/base_lifters/state_lifter.py

@@ -160,15 +160,22 @@ 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.
         """
         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)

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

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"