Add Rotation Averaging example

CHANGELOG.md

@@ -5,33 +5,42 @@ 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-08-22
 
-(00-changelog)=
+(0.0.0a)=
 ### Added
-
-(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)
 - RangeOnlyNsqLifter: specialized lifter for range-only localization without squared distances (examples/ro_nsq_lifter.py)
 - 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

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

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

scripts/rotation_averaging.ipynb → notebooks/rotation_averaging.ipynb

@@ -44,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",
@@ -81,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)"
    ]
   },
   {
@@ -103,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)"
@@ -116,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": {

popcor/base_lifters/_base_class.py

@@ -685,7 +685,25 @@ 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)
+            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."""

popcor/base_lifters/state_lifter.py

@@ -153,27 +153,31 @@ 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)
+        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,
         t0,
-        y: np.ndarray | list | None = None,
+        y: np.ndarray | list | dict | 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."
-        )
         if len(args):
             print(f"Warning: ignore args {args}")
         from cert_tools.sdp_solvers import solve_low_rank_sdp
@@ -196,13 +200,16 @@ 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))
+
         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, **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 +246,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)]

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