From a1bf03092cb1431c129f0d9e6c0e2ac0ccf017dd Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 08:46:55 +0200
Subject: [PATCH 01/36] Corrected types

---
 examples/lighthouse/multi_bs_geometry_estimation.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 4f6dc7c15..4bb3f8f5c 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -106,9 +106,9 @@ def ready_cb(averages):
     return result
 
 
-def record_angles_sequence(scf: SyncCrazyflie, recording_time_s: float) -> list[LhCfPoseSample]:
+def record_angles_sequence(scf: SyncCrazyflie, recording_time_s: float) -> list[LhMeasurement]:
     """Record angles and return a list of the samples"""
-    result: list[LhCfPoseSample] = []
+    result: list[LhMeasurement] = []
 
     bs_seen = set()
 
@@ -214,7 +214,7 @@ def write_to_file(name: str,
                   origin: LhCfPoseSample,
                   x_axis: list[LhCfPoseSample],
                   xy_plane: list[LhCfPoseSample],
-                  samples: list[LhCfPoseSample]):
+                  samples: list[LhMeasurement]):
     with open(name, 'wb') as handle:
         data = (origin, x_axis, xy_plane, samples)
         pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL)
@@ -228,7 +228,7 @@ def load_from_file(name: str):
 def estimate_geometry(origin: LhCfPoseSample,
                       x_axis: list[LhCfPoseSample],
                       xy_plane: list[LhCfPoseSample],
-                      samples: list[LhCfPoseSample]) -> dict[int, Pose]:
+                      samples: list[LhMeasurement]) -> dict[int, Pose]:
     """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
     matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
     initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(

From ddad27f69b006fa253d668268eaa140ee30eb8f8 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 09:16:51 +0200
Subject: [PATCH 02/36] Improve de-flipper to use all base stations

---
 .../localization/lighthouse_system_aligner.py | 18 +++++++++------
 .../test_lighthouse_system_aligner.py         | 23 +++++++++++++++++++
 2 files changed, 34 insertions(+), 7 deletions(-)

diff --git a/cflib/localization/lighthouse_system_aligner.py b/cflib/localization/lighthouse_system_aligner.py
index 3ec964a0a..158fc1903 100644
--- a/cflib/localization/lighthouse_system_aligner.py
+++ b/cflib/localization/lighthouse_system_aligner.py
@@ -109,21 +109,25 @@ def _Pose_from_params(cls, params: npt.ArrayLike) -> Pose:
     def _de_flip_transformation(cls, raw_transformation: Pose, x_axis: list[npt.ArrayLike],
                                 bs_poses: dict[int, Pose]) -> Pose:
         """
-        Investigats a transformation and flips it if needed. This method assumes that
-        1. all base stations are at Z>0
-        2. x_axis samples are taken at X>0
+        Examines a transformation and flips it if needed. This method assumes that
+        1. most base stations are at Z > 0
+        2. x_axis samples are taken at X > 0
         """
         transformation = raw_transformation
 
-        # X-axis poses should be on the positivie X-axis, check that the "mean" of the x-axis points ends up at X>0
+        # X-axis poses should be on the positive X-axis, check that the "mean" of the x-axis points ends up at X>0
         x_axis_mean = np.mean(x_axis, axis=0)
         if raw_transformation.rotate_translate(x_axis_mean)[0] < 0.0:
             flip_around_z_axis = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi))
             transformation = flip_around_z_axis.rotate_translate_pose(transformation)
 
-        # Base station poses should be above the floor, check the first one
-        bs_pose = list(bs_poses.values())[0]
-        if raw_transformation.rotate_translate(bs_pose.translation)[2] < 0.0:
+        # Assume base station poses should be above the floor. It is possible that the estimate of one or a few of them
+        # is slightly negative if they are placed on the floor, use an average of the z of all base stations.
+        def rotate_translate_get_z(bs_pose: Pose) -> float:
+            return raw_transformation.rotate_translate(bs_pose.translation)[2]
+
+        bs_z_mean = np.mean(list(map(rotate_translate_get_z, bs_poses.values())))
+        if bs_z_mean < 0.0:
             flip_around_x_axis = Pose.from_rot_vec(R_vec=(np.pi, 0.0, 0.0))
             transformation = flip_around_x_axis.rotate_translate_pose(transformation)
 
diff --git a/test/localization/test_lighthouse_system_aligner.py b/test/localization/test_lighthouse_system_aligner.py
index 0e5cea781..e5983164c 100644
--- a/test/localization/test_lighthouse_system_aligner.py
+++ b/test/localization/test_lighthouse_system_aligner.py
@@ -95,6 +95,29 @@ def test_that_solution_is_de_flipped(self):
         # Assert
         self.assertPosesAlmostEqual(expected, actual[bs_id])
 
+    def test_that_solution_is_de_flipped_with_first_bs_under_the_foor(self):
+        # Fixture
+        origin = (0.0, 0.0, 0.0)
+        x_axis = [(-1.0, 0.0, 0.0)]
+        xy_plane = [(2.0, 1.0, 0.0)]
+
+        bs_poses = {}
+
+        bs_id_1 = 7
+        bs_poses[bs_id_1] = Pose.from_rot_vec(t_vec=(0.0, 0.0, -0.1))
+        expected_1 = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(0.0, 0.0, -0.1))
+
+        bs_id_2 = 8
+        bs_poses[bs_id_2] = Pose.from_rot_vec(t_vec=(0.0, 0.0, 1.0))
+        expected_2 = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(0.0, 0.0, 1.0))
+
+        # Test
+        actual, transform = LighthouseSystemAligner.align(origin, x_axis, xy_plane, bs_poses)
+
+        # Assert
+        self.assertPosesAlmostEqual(expected_1, actual[bs_id_1])
+        self.assertPosesAlmostEqual(expected_2, actual[bs_id_2])
+
     def test_that_is_aligned_for_multiple_points_where_system_is_rotated_and_poins_are_fuzzy(self):
         # Fixture
         origin = (0.0, 0.0, 0.0)

From 6ff42e65aa235689b5b15203ac3685bbce216c58 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 13:52:41 +0200
Subject: [PATCH 03/36] Added IPPE solutions to LhCfPoseSample for reuse and
 speed up

---
 cflib/localization/ippe_cf.py                 |  2 +-
 cflib/localization/lighthouse_bs_vector.py    |  4 +-
 .../localization/lighthouse_cf_pose_sample.py | 31 +++++++
 .../lighthouse_geometry_solver.py             |  4 +-
 .../lighthouse_initial_estimator.py           | 84 +++++++++----------
 .../localization/lighthouse_sample_matcher.py |  2 +-
 .../localization/lighthouse_system_scaler.py  |  4 +-
 cflib/localization/lighthouse_types.py        | 19 -----
 .../multi_bs_geometry_estimation.py           |  4 +-
 .../test_lighthouse_geometry_solver.py        |  2 +-
 .../test_lighthouse_initial_estimator.py      |  4 +-
 11 files changed, 86 insertions(+), 74 deletions(-)
 create mode 100644 cflib/localization/lighthouse_cf_pose_sample.py

diff --git a/cflib/localization/ippe_cf.py b/cflib/localization/ippe_cf.py
index 51472dea0..c0bc306c1 100644
--- a/cflib/localization/ippe_cf.py
+++ b/cflib/localization/ippe_cf.py
@@ -65,7 +65,7 @@ def solve(U_cf: npt.ArrayLike, Q_cf: npt.ArrayLike) -> list[Solution]:
 
                      First param: Y (positive to the left)
                      Second param: Z (positive up)
-        :return: A list that contains 2 sets of pose solution from IPPE including rotation matrix
+        :return: A list that contains 2 sets of pose solutions from IPPE including rotation matrix
                  translation matrix, and reprojection error. The first solution in the list has
                  the smallest reprojection error.
         """
diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py
index 67e035964..0f29eb432 100644
--- a/cflib/localization/lighthouse_bs_vector.py
+++ b/cflib/localization/lighthouse_bs_vector.py
@@ -144,7 +144,7 @@ class LighthouseBsVectors(list):
 
     def projection_pair_list(self) -> npt.NDArray:
         """
-        Genereate a list of projection pairs for all vectors
+        Generate a list of projection pairs for all vectors
         """
         result = np.empty((len(self), 2), dtype=float)
         for i, vector in enumerate(self):
@@ -154,7 +154,7 @@ def projection_pair_list(self) -> npt.NDArray:
 
     def angle_list(self) -> npt.NDArray:
         """
-        Genereate a list of angles for all vectors, the order is horizontal, vertical, horizontal, vertical...
+        Generate a list of angles for all vectors, the order is horizontal, vertical, horizontal, vertical...
         """
         result = np.empty((len(self) * 2), dtype=float)
         for i, vector in enumerate(self):
diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
new file mode 100644
index 000000000..777063387
--- /dev/null
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -0,0 +1,31 @@
+from typing import NamedTuple
+
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+from cflib.localization.lighthouse_types import Pose
+
+
+class BsPairPoses(NamedTuple):
+    """A type representing the poses of a pair of base stations"""
+    bs1: Pose
+    bs2: Pose
+
+
+class LhCfPoseSample:
+    """ Represents a sample of a Crazyflie pose in space, it contains
+    various data related to the pose such as:
+    - lighthouse angles from one or more base stations
+    - The solutions found by IPPE, two solutions for each base station
+    """
+
+    def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, LighthouseBsVectors] = None) -> None:
+        self.timestamp: float = timestamp
+
+        # Angles measured by the Crazyflie and compensated using calibration data
+        # Stored in a dictionary using base station id as the key
+        self.angles_calibrated: dict[int, LighthouseBsVectors] = angles_calibrated
+        if self.angles_calibrated is None:
+            self.angles_calibrated = {}
+
+        # A dictionary from base station id to BsPairPoses, The poses represents the two possible poses of the base
+        # stations found by IPPE, in the crazyflie reference frame.
+        self.ippe_solutions: dict[int, BsPairPoses] = {}
diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index 949d70b50..ffc324159 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -25,9 +25,9 @@
 import numpy.typing as npt
 import scipy.optimize
 
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import Pose
 from cflib.localization.lighthouse_types import LhBsCfPoses
-from cflib.localization.lighthouse_types import LhCfPoseSample
-from cflib.localization.lighthouse_types import Pose
 
 
 class LighthouseGeometrySolution:
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 1853d415a..e17548ae9 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -27,8 +27,9 @@
 import numpy.typing as npt
 
 from .ippe_cf import IppeCf
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import BsPairPoses
 from cflib.localization.lighthouse_types import LhBsCfPoses
-from cflib.localization.lighthouse_types import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhException
 from cflib.localization.lighthouse_types import Pose
 
@@ -42,12 +43,6 @@ class BsPairIds(NamedTuple):
     bs2: int
 
 
-class BsPairPoses(NamedTuple):
-    """A type representing the poses of a pair of base stations"""
-    bs1: Pose
-    bs2: Pose
-
-
 class LighthouseInitialEstimator:
     """
     Make initial estimates of base station and CF poses using IPPE (analytical solution).
@@ -72,27 +67,17 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: Array
                  outliers are removed.
         """
 
-        bs_positions = cls._find_solutions(matched_samples, sensor_positions)
+        cls._add_ippe_solutions_to_samples(matched_samples, sensor_positions)
+
+        bs_positions = cls._find_bs_to_bs_poses(matched_samples, sensor_positions)
         # bs_positions is a map from bs-id-pair to position, where the position is the position of the second
         # bs, as seen from the first bs (in the first bs ref frame).
 
         bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(
             matched_samples, sensor_positions, bs_positions)
 
-        # Use the first CF pose as the global reference frame. The pose of the first base station (as estimated by ippe)
-        # is used as the "true" position (reference)
-        reference_bs_pose = None
-        for bs_pose_ref_cfs in bs_poses_ref_cfs:
-            if len(bs_pose_ref_cfs) > 0:
-                bs_id, reference_bs_pose = list(bs_pose_ref_cfs.items())[0]
-                break
-
-        if reference_bs_pose is None:
-            raise LhException('Too little data, no reference')
-        bs_poses: dict[int, Pose] = {bs_id: reference_bs_pose}
-
-        # Calculate the pose of the remaining base stations, based on the pose of the first CF
-        cls._estimate_remaining_bs_poses(bs_poses_ref_cfs, bs_poses)
+        # Calculate the pose of the base stations, based on the pose of one base station
+        bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
 
         # Now that we have estimated the base station poses, estimate the poses of the CF in all the samples
         cf_poses = cls._estimate_cf_poses(bs_poses_ref_cfs, bs_poses)
@@ -100,7 +85,20 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: Array
         return LhBsCfPoses(bs_poses, cf_poses), cleaned_matched_samples
 
     @classmethod
-    def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat
+    def _add_ippe_solutions_to_samples(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat):
+        for sample in matched_samples:
+            solutions: dict[int, BsPairPoses] = {}
+            for bs, angles in sample.angles_calibrated.items():
+                projections = angles.projection_pair_list()
+                estimates_ref_bs = IppeCf.solve(sensor_positions, projections)
+                estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs)
+                solutions[bs] = estimates_ref_cf
+
+                sample.ippe_solutions = solutions
+
+
+    @classmethod
+    def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat
                         ) -> dict[BsPairIds, ArrayFloat]:
         """
         Find the pose of all base stations, in the reference frame of other base stations.
@@ -121,14 +119,7 @@ def _find_solutions(cls, matched_samples: list[LhCfPoseSample], sensor_positions
 
         position_permutations: dict[BsPairIds, list[list[ArrayFloat]]] = {}
         for sample in matched_samples:
-            solutions: dict[int, BsPairPoses] = {}
-            for bs, angles in sample.angles_calibrated.items():
-                projections = angles.projection_pair_list()
-                estimates_ref_bs = IppeCf.solve(sensor_positions, projections)
-                estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs)
-                solutions[bs] = estimates_ref_cf
-
-            cls._add_solution_permutations(solutions, position_permutations)
+            cls._add_solution_permutations(sample.ippe_solutions, position_permutations)
 
         return cls._find_most_likely_positions(position_permutations)
 
@@ -174,7 +165,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position
         """
         Estimate the base station poses in the Crazyflie reference frames, for each sample.
 
-        Use Ippe again to find the possible poses of the bases stations and pick the one that best matches the position
+        Use Ippe again to find the possible poses of the base stations and pick the one that best matches the position
         in bs_positions.
 
         :param matched_samples: List of samples
@@ -188,12 +179,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position
         cleaned_matched_samples: list[LhCfPoseSample] = []
 
         for sample in matched_samples:
-            solutions: dict[int, BsPairPoses] = {}
-            for bs, angles in sample.angles_calibrated.items():
-                projections = angles.projection_pair_list()
-                estimates_ref_bs = IppeCf.solve(sensor_positions, projections)
-                estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs)
-                solutions[bs] = estimates_ref_cf
+            solutions = sample.ippe_solutions
 
             poses: dict[int, Pose] = {}
             ids = sorted(solutions.keys())
@@ -310,7 +296,7 @@ def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.
         return BsPairPoses(Pose(rot_1, t_1), Pose(rot_2, t_2))
 
     @classmethod
-    def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: dict[int, Pose]) -> None:
+    def _estimate_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]]) -> dict[int, Pose]:
         """
         Based on one base station pose, estimate the other base station poses.
 
@@ -318,6 +304,18 @@ def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], b
         have information of base station pairs (0, 2) and (2, 3), from this we can first derive the pose of 2 and after
         that the pose of 3.
         """
+        # Use the first CF pose as the global reference frame. The pose of the first base station (as estimated by ippe)
+        # is used as the "true" position (reference)
+        reference_bs_pose = None
+        for bs_pose_ref_cfs in bs_poses_ref_cfs:
+            if len(bs_pose_ref_cfs) > 0:
+                bs_id, reference_bs_pose = list(bs_pose_ref_cfs.items())[0]
+                break
+
+        if reference_bs_pose is None:
+            raise LhException('Too little data, no reference')
+        bs_poses: dict[int, Pose] = {bs_id: reference_bs_pose}
+
         # Find all base stations in the list
         all_bs = set()
         for initial_est_bs_poses in bs_poses_ref_cfs:
@@ -354,7 +352,7 @@ def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], b
 
             # Average over poses and add to bs_poses
             for bs_id, poses in buckets.items():
-                bs_poses[bs_id] = cls._avarage_poses(poses)
+                bs_poses[bs_id] = cls._average_poses(poses)
 
             to_find = all_bs - bs_poses.keys()
             if len(to_find) == 0:
@@ -365,8 +363,10 @@ def _estimate_remaining_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], b
 
             remaining = len(to_find)
 
+        return bs_poses
+
     @classmethod
-    def _avarage_poses(cls, poses: list[Pose]) -> Pose:
+    def _average_poses(cls, poses: list[Pose]) -> Pose:
         """
         Averaging of quaternions to get the "average" orientation of multiple samples.
         From https://stackoverflow.com/a/61013769
@@ -400,7 +400,7 @@ def _estimate_cf_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]], bs_poses: d
                 est_ref_global = cls._map_cf_pos_to_cf_pos(pose_global, pose_cf)
                 poses.append(est_ref_global)
 
-            cf_poses.append(cls._avarage_poses(poses))
+            cf_poses.append(cls._average_poses(poses))
 
         return cf_poses
 
diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
index afe26fb8f..7f80aca90 100644
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ b/cflib/localization/lighthouse_sample_matcher.py
@@ -21,7 +21,7 @@
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from __future__ import annotations
 
-from cflib.localization.lighthouse_types import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhMeasurement
 
 
diff --git a/cflib/localization/lighthouse_system_scaler.py b/cflib/localization/lighthouse_system_scaler.py
index a873dba03..8e9469328 100644
--- a/cflib/localization/lighthouse_system_scaler.py
+++ b/cflib/localization/lighthouse_system_scaler.py
@@ -27,8 +27,8 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
-from cflib.localization.lighthouse_types import LhCfPoseSample
-from cflib.localization.lighthouse_types import Pose
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import Pose
 
 
 class LighthouseSystemScaler:
diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py
index 941bc5e74..84ddb134d 100644
--- a/cflib/localization/lighthouse_types.py
+++ b/cflib/localization/lighthouse_types.py
@@ -149,25 +149,6 @@ class LhBsCfPoses(NamedTuple):
     cf_poses: list[Pose]
 
 
-class LhCfPoseSample:
-    """ Represents a sample of a Crazyflie pose in space, it contains
-    various data related to the pose such as:
-    - lighthouse angles from one or more base stations
-    - initial estimate of the pose
-    - refined estimate of the pose
-    - estimated errors
-    """
-
-    def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, LighthouseBsVectors] = None) -> None:
-        self.timestamp: float = timestamp
-
-        # Angles measured by the Crazyflie and compensated using calibration data
-        # Stored in a dictionary using base station id as the key
-        self.angles_calibrated: dict[int, LighthouseBsVectors] = angles_calibrated
-        if self.angles_calibrated is None:
-            self.angles_calibrated = {}
-
-
 class LhDeck4SensorPositions:
     """ Positions of the sensors on the Lighthouse 4 deck """
     # Sensor distances on the lighthouse deck
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 4bb3f8f5c..0b31c4314 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -63,10 +63,10 @@
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
-from cflib.localization.lighthouse_types import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import Pose
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhMeasurement
-from cflib.localization.lighthouse_types import Pose
 from cflib.utils import uri_helper
 
 REFERENCE_DIST = 1.0
diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index ad2f2fd29..275a897aa 100644
--- a/test/localization/test_lighthouse_geometry_solver.py
+++ b/test/localization/test_lighthouse_geometry_solver.py
@@ -24,7 +24,7 @@
 
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
-from cflib.localization.lighthouse_types import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 
 
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index b011558b5..abe0b7311 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -25,10 +25,10 @@
 import numpy as np
 
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
-from cflib.localization.lighthouse_types import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import Pose
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhException
-from cflib.localization.lighthouse_types import Pose
 
 
 class TestLighthouseInitialEstimator(LighthouseTestBase):

From f0e6a53b14eed34c9cd5e47d9ddc917b15a82f79 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 14:50:45 +0200
Subject: [PATCH 04/36] Refactor sample matcher

---
 .../localization/lighthouse_sample_matcher.py | 31 ++++++++++---------
 1 file changed, 16 insertions(+), 15 deletions(-)

diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
index 7f80aca90..ac46f5a35 100644
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ b/cflib/localization/lighthouse_sample_matcher.py
@@ -23,6 +23,7 @@
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhMeasurement
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 
 
 class LighthouseSampleMatcher:
@@ -35,30 +36,30 @@ class LighthouseSampleMatcher:
 
     @classmethod
     def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.020,
-              min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]:
+              min_nr_of_bs_in_match: int = 1) -> list[LhCfPoseSample]:
         """
         Aggregate samples close in time into lists
         """
 
         result = []
-        current: LhCfPoseSample = None
+        current_angles: dict[int, LighthouseBsVectors] = {}
+        current_ts = 0.0
 
         for sample in samples:
-            ts = sample.timestamp
+            if len(current_angles) > 0:
+                if sample.timestamp > (current_ts + max_time_diff):
+                    if len(current_angles) >= min_nr_of_bs_in_match:
+                        pose_sample = LhCfPoseSample(timestamp=current_ts, angles_calibrated=current_angles)
+                        result.append(pose_sample)
 
-            if current is None:
-                current = LhCfPoseSample(timestamp=ts)
+                    current_angles = {}
 
-            if ts > (current.timestamp + max_time_diff):
-                cls._append_result(current, result, min_nr_of_bs_in_match)
-                current = LhCfPoseSample(timestamp=ts)
+            if len(current_angles) == 0:
+                current_ts = sample.timestamp
+            current_angles[sample.base_station_id] = sample.angles
 
-            current.angles_calibrated[sample.base_station_id] = sample.angles
+        if len(current_angles) >= min_nr_of_bs_in_match:
+            pose_sample = LhCfPoseSample(timestamp=current_ts, angles_calibrated=current_angles)
+            result.append(pose_sample)
 
-        cls._append_result(current, result, min_nr_of_bs_in_match)
         return result
-
-    @classmethod
-    def _append_result(cls, current: LhCfPoseSample, result: list[LhCfPoseSample], min_nr_of_bs_in_match: int):
-        if current is not None and len(current.angles_calibrated) >= min_nr_of_bs_in_match:
-            result.append(current)

From 6534344cc960674422af1cde73af0e3d386cbee6 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 15:05:44 +0200
Subject: [PATCH 05/36] Removed unused parameters

---
 cflib/localization/lighthouse_initial_estimator.py | 13 ++++---------
 1 file changed, 4 insertions(+), 9 deletions(-)

diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index e17548ae9..a619166ab 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -69,12 +69,11 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: Array
 
         cls._add_ippe_solutions_to_samples(matched_samples, sensor_positions)
 
-        bs_positions = cls._find_bs_to_bs_poses(matched_samples, sensor_positions)
+        bs_positions = cls._find_bs_to_bs_poses(matched_samples)
         # bs_positions is a map from bs-id-pair to position, where the position is the position of the second
         # bs, as seen from the first bs (in the first bs ref frame).
 
-        bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(
-            matched_samples, sensor_positions, bs_positions)
+        bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(matched_samples, bs_positions)
 
         # Calculate the pose of the base stations, based on the pose of one base station
         bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
@@ -96,10 +95,8 @@ def _add_ippe_solutions_to_samples(cls, matched_samples: list[LhCfPoseSample], s
 
                 sample.ippe_solutions = solutions
 
-
     @classmethod
-    def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat
-                        ) -> dict[BsPairIds, ArrayFloat]:
+    def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample]) -> dict[BsPairIds, ArrayFloat]:
         """
         Find the pose of all base stations, in the reference frame of other base stations.
 
@@ -111,7 +108,6 @@ def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample], sensor_posi
            out in space, while the correct one will end up more or less in the same spot for all samples.
 
         :param matched_samples: List of matched samples
-        :param sensor_positions: list of sensor positions on the lighthouse deck, CF reference frame
         :return: Base stations poses in the reference frame of the other base stations. The data is organized as a
                  dictionary of tuples with base station id pairs, mapped to positions. For instance the entry with key
                  (2, 1) contains the position of base station 1, in the base station 2 reference frame.
@@ -159,7 +155,7 @@ def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses],
                                                    pose3.translation, pose4.translation])
 
     @classmethod
-    def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat,
+    def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
                          bs_positions: dict[BsPairIds, ArrayFloat]) -> tuple[list[dict[int, Pose]],
                                                                              list[LhCfPoseSample]]:
         """
@@ -169,7 +165,6 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], sensor_position
         in bs_positions.
 
         :param matched_samples: List of samples
-        :param sensor_positions: Positions of the sensors on the lighthouse deck (CF ref frame)
         :param bs_positions: Dictionary of base station positions (other base station ref frame)
         :return: A list of dictionaries from base station to Pose of all base stations, for each sample, as well as
                  a version of the matched_samples where outliers are removed

From ca419c5f9d7d74503053d479920ca988f2cf56a9 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 14 May 2025 20:31:20 +0200
Subject: [PATCH 06/36] Moved ippe estimation to LhCfPoseSample

---
 .../localization/lighthouse_cf_pose_sample.py | 46 ++++++++++++++++---
 .../lighthouse_initial_estimator.py           | 36 ++-------------
 .../localization/lighthouse_sample_matcher.py |  4 +-
 .../multi_bs_geometry_estimation.py           |  7 +--
 4 files changed, 48 insertions(+), 45 deletions(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 777063387..12b76ad99 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -1,8 +1,13 @@
 from typing import NamedTuple
+import numpy as np
+import numpy.typing as npt
 
+from .ippe_cf import IppeCf
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_types import Pose
 
+ArrayFloat = npt.NDArray[np.float_]
+
 
 class BsPairPoses(NamedTuple):
     """A type representing the poses of a pair of base stations"""
@@ -11,21 +16,48 @@ class BsPairPoses(NamedTuple):
 
 
 class LhCfPoseSample:
-    """ Represents a sample of a Crazyflie pose in space, it contains
-    various data related to the pose such as:
+    """ Represents a sample of a Crazyflie pose in space, it contains:
+    - a timestamp (if applicable)
     - lighthouse angles from one or more base stations
-    - The solutions found by IPPE, two solutions for each base station
+    - The the two solutions found by IPPE for each base station, in the cf ref frame.
+
+    The ippe solution is somewhat heavy and is only created on demand by calling augment_with_ippe()
     """
 
-    def __init__(self, timestamp: float = 0.0, angles_calibrated: dict[int, LighthouseBsVectors] = None) -> None:
+    def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp: float = 0.0) -> None:
         self.timestamp: float = timestamp
 
         # Angles measured by the Crazyflie and compensated using calibration data
         # Stored in a dictionary using base station id as the key
-        self.angles_calibrated: dict[int, LighthouseBsVectors] = angles_calibrated
-        if self.angles_calibrated is None:
-            self.angles_calibrated = {}
+        self.angles_calibrated = angles_calibrated
 
         # A dictionary from base station id to BsPairPoses, The poses represents the two possible poses of the base
         # stations found by IPPE, in the crazyflie reference frame.
         self.ippe_solutions: dict[int, BsPairPoses] = {}
+
+    def augment_with_ippe(self, sensor_positions: ArrayFloat) -> None:
+        self.ippe_solutions = self._find_ippe_solutions(self.angles_calibrated, sensor_positions)
+
+    def _find_ippe_solutions(self, angles_calibrated: dict[int, LighthouseBsVectors],
+                             sensor_positions: ArrayFloat) -> dict[int, BsPairPoses]:
+
+        solutions: dict[int, BsPairPoses] = {}
+        for bs, angles in angles_calibrated.items():
+            projections = angles.projection_pair_list()
+            estimates_ref_bs = IppeCf.solve(sensor_positions, projections)
+            estimates_ref_cf = self._convert_estimates_to_cf_reference_frame(estimates_ref_bs)
+            solutions[bs] = estimates_ref_cf
+
+        return solutions
+
+    def _convert_estimates_to_cf_reference_frame(self, estimates_ref_bs: list[IppeCf.Solution]) -> BsPairPoses:
+        """
+        Convert the two ippe solutions from the base station reference frame to the CF reference frame
+        """
+        rot_1 = estimates_ref_bs[0].R.transpose()
+        t_1 = np.dot(rot_1, -estimates_ref_bs[0].t)
+
+        rot_2 = estimates_ref_bs[1].R.transpose()
+        t_2 = np.dot(rot_2, -estimates_ref_bs[1].t)
+
+        return BsPairPoses(Pose(rot_1, t_1), Pose(rot_2, t_2))
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index a619166ab..b861d5f29 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -26,7 +26,6 @@
 import numpy as np
 import numpy.typing as npt
 
-from .ippe_cf import IppeCf
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import BsPairPoses
 from cflib.localization.lighthouse_types import LhBsCfPoses
@@ -53,8 +52,7 @@ class LighthouseInitialEstimator:
     OUTLIER_DETECTION_ERROR = 0.5
 
     @classmethod
-    def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat) -> tuple[
-            LhBsCfPoses, list[LhCfPoseSample]]:
+    def estimate(cls, matched_samples: list[LhCfPoseSample]) -> tuple[LhBsCfPoses, list[LhCfPoseSample]]:
         """
         Make a rough estimate of the poses of all base stations and CF poses found in the samples.
 
@@ -62,13 +60,10 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: Array
         global reference frame.
 
         :param matched_samples: A list of samples with lighthouse angles.
-        :param sensor_positions: An array with the sensor positions on the lighthouse deck (3D, CF ref frame)
         :return: an estimate of base station and Crazyflie poses, as well as a cleaned version of matched_samples where
                  outliers are removed.
         """
 
-        cls._add_ippe_solutions_to_samples(matched_samples, sensor_positions)
-
         bs_positions = cls._find_bs_to_bs_poses(matched_samples)
         # bs_positions is a map from bs-id-pair to position, where the position is the position of the second
         # bs, as seen from the first bs (in the first bs ref frame).
@@ -83,18 +78,6 @@ def estimate(cls, matched_samples: list[LhCfPoseSample], sensor_positions: Array
 
         return LhBsCfPoses(bs_poses, cf_poses), cleaned_matched_samples
 
-    @classmethod
-    def _add_ippe_solutions_to_samples(cls, matched_samples: list[LhCfPoseSample], sensor_positions: ArrayFloat):
-        for sample in matched_samples:
-            solutions: dict[int, BsPairPoses] = {}
-            for bs, angles in sample.angles_calibrated.items():
-                projections = angles.projection_pair_list()
-                estimates_ref_bs = IppeCf.solve(sensor_positions, projections)
-                estimates_ref_cf = cls._convert_estimates_to_cf_reference_frame(estimates_ref_bs)
-                solutions[bs] = estimates_ref_cf
-
-                sample.ippe_solutions = solutions
-
     @classmethod
     def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample]) -> dict[BsPairIds, ArrayFloat]:
         """
@@ -161,8 +144,8 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
         """
         Estimate the base station poses in the Crazyflie reference frames, for each sample.
 
-        Use Ippe again to find the possible poses of the base stations and pick the one that best matches the position
-        in bs_positions.
+        Again use the IPPE solutions to find the possible poses of the base stations and pick the one that best matches
+        the position in bs_positions.
 
         :param matched_samples: List of samples
         :param bs_positions: Dictionary of base station positions (other base station ref frame)
@@ -277,19 +260,6 @@ def _find_best_position_bucket(cls, buckets: list[list[ArrayFloat]]) -> ArrayFlo
         pos = np.mean(buckets[max_i], axis=0)
         return pos
 
-    @classmethod
-    def _convert_estimates_to_cf_reference_frame(cls, estimates_ref_bs: list[IppeCf.Solution]) -> BsPairPoses:
-        """
-        Convert the two ippe solutions from the base station reference frame to the CF reference frame
-        """
-        rot_1 = estimates_ref_bs[0].R.transpose()
-        t_1 = np.dot(rot_1, -estimates_ref_bs[0].t)
-
-        rot_2 = estimates_ref_bs[1].R.transpose()
-        t_2 = np.dot(rot_2, -estimates_ref_bs[1].t)
-
-        return BsPairPoses(Pose(rot_1, t_1), Pose(rot_2, t_2))
-
     @classmethod
     def _estimate_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]]) -> dict[int, Pose]:
         """
diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
index ac46f5a35..365e4e5dc 100644
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ b/cflib/localization/lighthouse_sample_matcher.py
@@ -49,7 +49,7 @@ def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.020,
             if len(current_angles) > 0:
                 if sample.timestamp > (current_ts + max_time_diff):
                     if len(current_angles) >= min_nr_of_bs_in_match:
-                        pose_sample = LhCfPoseSample(timestamp=current_ts, angles_calibrated=current_angles)
+                        pose_sample = LhCfPoseSample(current_angles, timestamp=current_ts)
                         result.append(pose_sample)
 
                     current_angles = {}
@@ -59,7 +59,7 @@ def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.020,
             current_angles[sample.base_station_id] = sample.angles
 
         if len(current_angles) >= min_nr_of_bs_in_match:
-            pose_sample = LhCfPoseSample(timestamp=current_ts, angles_calibrated=current_angles)
+            pose_sample = LhCfPoseSample(current_angles, timestamp=current_ts)
             result.append(pose_sample)
 
         return result
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 0b31c4314..b72e22637 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -94,7 +94,7 @@ def ready_cb(averages):
     for bs_id, data in recorded_angles.items():
         angles_calibrated[bs_id] = data[1]
 
-    result = LhCfPoseSample(angles_calibrated=angles_calibrated)
+    result = LhCfPoseSample(angles_calibrated)
 
     visible = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys()))
     print(f'  Position recorded, base station ids visible: {visible}')
@@ -231,8 +231,9 @@ def estimate_geometry(origin: LhCfPoseSample,
                       samples: list[LhMeasurement]) -> dict[int, Pose]:
     """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
     matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
-    initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(
-        matched_samples, LhDeck4SensorPositions.positions)
+    for sample in matched_samples:
+        sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+    initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
 
     print('Initial guess base stations at:')
     print_base_stations_poses(initial_guess.bs_poses)

From 5276b2a449ca89b7b55b4e3e18c4038c19899a0b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 15 May 2025 11:16:44 +0200
Subject: [PATCH 07/36] Added input data container

---
 .../localization/lighthouse_cf_pose_sample.py | 16 +++-
 .../lighthouse_geo_estimation_manager.py      | 93 +++++++++++++++++++
 .../lighthouse_sweep_angle_reader.py          | 39 ++++----
 .../multi_bs_geometry_estimation.py           | 63 ++++++-------
 4 files changed, 156 insertions(+), 55 deletions(-)
 create mode 100644 cflib/localization/lighthouse_geo_estimation_manager.py

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 12b76ad99..9f78493ec 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -29,14 +29,26 @@ def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp:
 
         # Angles measured by the Crazyflie and compensated using calibration data
         # Stored in a dictionary using base station id as the key
-        self.angles_calibrated = angles_calibrated
+        self.angles_calibrated: dict[int, LighthouseBsVectors] = angles_calibrated
 
         # A dictionary from base station id to BsPairPoses, The poses represents the two possible poses of the base
         # stations found by IPPE, in the crazyflie reference frame.
         self.ippe_solutions: dict[int, BsPairPoses] = {}
 
+        self.is_augmented = False
+
     def augment_with_ippe(self, sensor_positions: ArrayFloat) -> None:
-        self.ippe_solutions = self._find_ippe_solutions(self.angles_calibrated, sensor_positions)
+        if not self.is_augmented:
+            self.ippe_solutions = self._find_ippe_solutions(self.angles_calibrated, sensor_positions)
+            self.is_augmented = True
+
+    def is_empty(self) -> bool:
+        """Checks if no angles are set
+
+        Returns:
+            bool: True if no angles are set
+        """
+        return len(self.angles_calibrated) == 0
 
     def _find_ippe_solutions(self, angles_calibrated: dict[int, LighthouseBsVectors],
                              sensor_positions: ArrayFloat) -> dict[int, BsPairPoses]:
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
new file mode 100644
index 000000000..160f29726
--- /dev/null
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -0,0 +1,93 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2025 Bitcraze AB
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, in version 3.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <http://www.gnu.org/licenses/>.
+from __future__ import annotations
+
+import numpy as np
+import numpy.typing as npt
+
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_types import LhMeasurement
+from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
+
+
+ArrayFloat = npt.NDArray[np.float_]
+
+
+class LhGeoInputContainer():
+    """This class holds the input data required by the geometry estimation functionality.
+    """
+    def __init__(self, sensor_positions: ArrayFloat) -> None:
+        self.EMPTY_POSE_SAMPLE = LhCfPoseSample(angles_calibrated={})
+        self.sensor_positions = sensor_positions
+
+        self.origin: LhCfPoseSample = self.EMPTY_POSE_SAMPLE
+        self.x_axis: list[LhCfPoseSample] = []
+        self.xy_plane: list[LhCfPoseSample] = []
+        self.xyz_space: list[LhCfPoseSample] = []
+
+    def set_origin_sample(self, origin: LhCfPoseSample) -> None:
+        """Store/update the sample to be used for the origin
+
+        Args:
+            origin (LhCfPoseSample): the new origin
+        """
+        self.origin = origin
+        self.origin.augment_with_ippe(self.sensor_positions)
+
+    def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
+        """Store/update the sample to be used for the x_axis
+
+        Args:
+            x_axis (LhCfPoseSample): the new x-axis sample
+        """
+        self.x_axis = [x_axis]
+        self.x_axis[0].augment_with_ippe(self.sensor_positions)
+
+    def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
+        """Store/update the samples to be used for the xy-plane
+
+        Args:
+            xy_plane (list[LhCfPoseSample]): the new xy-plane samples
+        """
+        self.xy_plane = xy_plane
+        self._augment_samples(self.xy_plane)
+
+    def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
+        """Store/update the samples for the volume
+
+        Args:
+            samples (list[LhMeasurement]): the new samples
+        """
+        self.xyz_space = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
+        self._augment_samples(self.xyz_space)
+
+    def get_matched_samples(self) -> list[LhCfPoseSample]:
+        """Get all pose samples collected in a list
+
+        Returns:
+            list[LhCfPoseSample]: _description_
+        """
+        return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
+
+    def _augment_samples(self, samples: list[LhCfPoseSample]) -> None:
+        for sample in samples:
+            sample.augment_with_ippe(self.sensor_positions)
diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py
index 8c653c4a5..d421c323a 100644
--- a/cflib/localization/lighthouse_sweep_angle_reader.py
+++ b/cflib/localization/lighthouse_sweep_angle_reader.py
@@ -21,6 +21,8 @@
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from cflib.localization import LighthouseBsVector
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+from cflib.crazyflie import Crazyflie
+from collections.abc import Callable
 
 
 class LighthouseSweepAngleReader():
@@ -30,7 +32,7 @@ class LighthouseSweepAngleReader():
     ANGLE_STREAM_PARAM = 'locSrv.enLhAngleStream'
     NR_OF_SENSORS = 4
 
-    def __init__(self, cf, data_recevied_cb):
+    def __init__(self, cf: Crazyflie, data_recevied_cb):
         self._cf = cf
         self._cb = data_recevied_cb
         self._is_active = False
@@ -48,7 +50,7 @@ def stop(self):
             self._cf.loc.receivedLocationPacket.remove_callback(self._packet_received_cb)
             self._angle_stream_activate(False)
 
-    def _angle_stream_activate(self, is_active):
+    def _angle_stream_activate(self, is_active: bool):
         value = 0
         if is_active:
             value = 1
@@ -59,11 +61,11 @@ def _packet_received_cb(self, packet):
             return
 
         if self._cb:
-            base_station_id = packet.data['basestation']
-            horiz_angles = packet.data['x']
-            vert_angles = packet.data['y']
+            base_station_id: int = packet.data['basestation']
+            horiz_angles: float = packet.data['x']
+            vert_angles: float = packet.data['y']
 
-            result = []
+            result: list[LighthouseBsVector] = []
             for i in range(self.NR_OF_SENSORS):
                 result.append(LighthouseBsVector(horiz_angles[i], vert_angles[i]))
 
@@ -75,7 +77,7 @@ class LighthouseSweepAngleAverageReader():
     Helper class to make it easy read sweep angles for multiple base stations and average the result
     """
 
-    def __init__(self, cf, ready_cb):
+    def __init__(self, cf: Crazyflie, ready_cb: Callable[[dict[int, tuple[int, LighthouseBsVectors]]], None]):
         self._reader = LighthouseSweepAngleReader(cf, self._data_recevied_cb)
         self._ready_cb = ready_cb
         self.nr_of_samples_required = 50
@@ -84,7 +86,7 @@ def __init__(self, cf, ready_cb):
         # The storage is a dictionary keyed on the base station channel
         # Each entry is a list of 4 lists, one per sensor.
         # Each list contains LighthouseBsVector objects, representing the sampled sweep angles
-        self._sample_storage = None
+        self._sample_storage: dict[int, list[list[LighthouseBsVector]]] | None = None
 
     def start_angle_collection(self):
         """
@@ -103,7 +105,7 @@ def is_collecting(self):
         """True if data collection is in progress"""
         return self._sample_storage is not None
 
-    def _data_recevied_cb(self, base_station_id, bs_vectors):
+    def _data_recevied_cb(self, base_station_id: int, bs_vectors: list[LighthouseBsVector]):
         self._store_sample(base_station_id, bs_vectors, self._sample_storage)
         if self._has_collected_enough_data(self._sample_storage):
             self._reader.stop()
@@ -112,7 +114,8 @@ def _data_recevied_cb(self, base_station_id, bs_vectors):
                 self._ready_cb(averages)
             self._sample_storage = None
 
-    def _store_sample(self, base_station_id, bs_vectors, storage):
+    def _store_sample(self, base_station_id: int, bs_vectors: list[LighthouseBsVector],
+                      storage: dict[int, list[list[LighthouseBsVector]]]):
         if base_station_id not in storage:
             storage[base_station_id] = []
             for sensor in range(self._reader.NR_OF_SENSORS):
@@ -121,31 +124,31 @@ def _store_sample(self, base_station_id, bs_vectors, storage):
         for sensor in range(self._reader.NR_OF_SENSORS):
             storage[base_station_id][sensor].append(bs_vectors[sensor])
 
-    def _has_collected_enough_data(self, storage):
+    def _has_collected_enough_data(self, storage: dict[int, list[list[LighthouseBsVector]]]):
         for sample_list in storage.values():
             if len(sample_list[0]) >= self.nr_of_samples_required:
                 return True
         return False
 
-    def _average_all_lists(self, storage):
-        result = {}
+    def _average_all_lists(self, storage: dict[int, list[list[LighthouseBsVector]]]) -> dict[int, tuple[int, LighthouseBsVectors]]:
+        result: dict[int, tuple[int, LighthouseBsVectors]] = {}
 
-        for id, sample_lists in storage.items():
+        for bs_id, sample_lists in storage.items():
             averages = self._average_sample_lists(sample_lists)
             count = len(sample_lists[0])
-            result[id] = (count, averages)
+            result[bs_id] = (count, averages)
 
         return result
 
-    def _average_sample_lists(self, sample_lists):
-        result = []
+    def _average_sample_lists(self, sample_lists: list[list[LighthouseBsVector]]) -> LighthouseBsVectors:
+        result: list[LighthouseBsVector] = []
 
         for i in range(self._reader.NR_OF_SENSORS):
             result.append(self._average_sample_list(sample_lists[i]))
 
         return LighthouseBsVectors(result)
 
-    def _average_sample_list(self, sample_list):
+    def _average_sample_list(self, sample_list: list[LighthouseBsVector]) -> LighthouseBsVector:
         sum_horiz = 0.0
         sum_vert = 0.0
 
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index b72e22637..2705986b9 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -56,6 +56,7 @@
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
+from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
@@ -72,13 +73,13 @@
 REFERENCE_DIST = 1.0
 
 
-def record_angles_average(scf: SyncCrazyflie, timeout: float = 5.0) -> LhCfPoseSample:
+def record_angles_average(scf: SyncCrazyflie, timeout: float = 5.0) -> LhCfPoseSample | None:
     """Record angles and average over the samples to reduce noise"""
-    recorded_angles = None
+    recorded_angles: dict[int, tuple[int, LighthouseBsVectors]] | None = None
 
     is_ready = Event()
 
-    def ready_cb(averages):
+    def ready_cb(averages: dict[int, tuple[int, LighthouseBsVectors]]):
         nonlocal recorded_angles
         recorded_angles = averages
         is_ready.set()
@@ -90,7 +91,7 @@ def ready_cb(averages):
         print('Recording timed out.')
         return None
 
-    angles_calibrated = {}
+    angles_calibrated: dict[int, LighthouseBsVectors] = {}
     for bs_id, data in recorded_angles.items():
         angles_calibrated[bs_id] = data[1]
 
@@ -182,7 +183,7 @@ def visualize(cf_poses: list[Pose], bs_poses: list[Pose]):
     """Visualize positions of base stations and Crazyflie positions"""
     # Set to True to visualize positions
     # Requires PyPlot
-    visualize_positions = False
+    visualize_positions = True
     if visualize_positions:
         import matplotlib.pyplot as plt
 
@@ -210,29 +211,20 @@ def visualize(cf_poses: list[Pose], bs_poses: list[Pose]):
         plt.show()
 
 
-def write_to_file(name: str,
-                  origin: LhCfPoseSample,
-                  x_axis: list[LhCfPoseSample],
-                  xy_plane: list[LhCfPoseSample],
-                  samples: list[LhMeasurement]):
+def write_to_file(name: str, container: LhGeoInputContainer):
     with open(name, 'wb') as handle:
-        data = (origin, x_axis, xy_plane, samples)
+        data = container
         pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL)
 
 
-def load_from_file(name: str):
+def load_from_file(name: str) -> LhGeoInputContainer:
     with open(name, 'rb') as handle:
         return pickle.load(handle)
 
 
-def estimate_geometry(origin: LhCfPoseSample,
-                      x_axis: list[LhCfPoseSample],
-                      xy_plane: list[LhCfPoseSample],
-                      samples: list[LhMeasurement]) -> dict[int, Pose]:
+def estimate_geometry(container: LhGeoInputContainer) -> dict[int, Pose]:
     """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
-    matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
-    for sample in matched_samples:
-        sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+    matched_samples = container.get_matched_samples()
     initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
 
     print('Initial guess base stations at:')
@@ -241,16 +233,16 @@ def estimate_geometry(origin: LhCfPoseSample,
     print(f'{len(cleaned_matched_samples)} samples will be used')
     visualize(initial_guess.cf_poses, initial_guess.bs_poses.values())
 
-    solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions)
+    solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions)
     if not solution.success:
         print('Solution did not converge, it might not be good!')
 
     start_x_axis = 1
-    start_xy_plane = 1 + len(x_axis)
+    start_xy_plane = 1 + len(container.x_axis)
     origin_pos = solution.cf_poses[0].translation
-    x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(x_axis)]
+    x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(container.x_axis)]
     x_axis_pos = list(map(lambda x: x.translation, x_axis_poses))
-    xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(xy_plane)]
+    xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(container.xy_plane)]
     xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses))
 
     print('Raw solution:')
@@ -303,11 +295,11 @@ def data_written(_):
 
 
 def estimate_from_file(file_name: str):
-    origin, x_axis, xy_plane, samples = load_from_file(file_name)
-    estimate_geometry(origin, x_axis, xy_plane, samples)
+    container = load_from_file(file_name)
+    estimate_geometry(container)
 
 
-def get_recording(scf: SyncCrazyflie):
+def get_recording(scf: SyncCrazyflie) -> LhCfPoseSample:
     data = None
     while True:  # Infinite loop, will break on valid measurement
         input('Press return when ready. ')
@@ -322,8 +314,8 @@ def get_recording(scf: SyncCrazyflie):
     return data
 
 
-def get_multiple_recordings(scf: SyncCrazyflie):
-    data = []
+def get_multiple_recordings(scf: SyncCrazyflie) -> list[LhCfPoseSample]:
+    data: list[LhCfPoseSample] = []
     first_attempt = True
 
     while True:
@@ -354,21 +346,22 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
     """Connect to a Crazyflie, collect data and estimate the geometry of the system"""
     print(f'Step 1. Connecting to the Crazyflie on uri {uri}...')
     with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
         print('  Connected')
         print('')
         print('In the 3 following steps we will define the coordinate system.')
 
         print('Step 2. Put the Crazyflie where you want the origin of your coordinate system.')
 
-        origin = get_recording(scf)
+        container.set_origin_sample(get_recording(scf))
 
         print(f'Step 3. Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the origin. ' +
-              'This position defines the direction of the X-axis, but it is also used for scaling of the system.')
-        x_axis = [get_recording(scf)]
+              'This position defines the direction of the X-axis, but it is also used for scaling the system.')
+        container.set_x_axis_sample(get_recording(scf))
 
         print('Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.')
         print('Multiple samples can be recorded if you want to.')
-        xy_plane = get_multiple_recordings(scf)
+        container.set_xy_plane_samples(get_multiple_recordings(scf))
 
         print()
         print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' +
@@ -379,15 +372,15 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
                                'recording starts when you hit enter. ')
         recording_time_s = parse_recording_time(recording_time, default_time)
         print('  Recording started...')
-        samples = record_angles_sequence(scf, recording_time_s)
+        container.set_xyz_space_samples(record_angles_sequence(scf, recording_time_s))
         print('  Recording ended')
 
         if file_name:
-            write_to_file(file_name, origin, x_axis, xy_plane, samples)
+            write_to_file(file_name, container)
             print(f'Wrote data to file {file_name}')
 
         print('Step 6. Estimating geometry...')
-        bs_poses = estimate_geometry(origin, x_axis, xy_plane, samples)
+        bs_poses = estimate_geometry(container)
         print('  Geometry estimated')
 
         print('Step 7. Upload geometry to the Crazyflie')

From 75048632219500f6f78f1f9fd590e5a3564141f4 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 16 May 2025 00:55:35 +0200
Subject: [PATCH 08/36] Make sure mandatory samples are not discarded

---
 cflib/localization/lighthouse_cf_pose_sample.py  |  5 ++++-
 .../lighthouse_geo_estimation_manager.py         | 16 ++++++++++------
 .../localization/lighthouse_initial_estimator.py |  2 +-
 3 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 9f78493ec..d091bb73e 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -34,9 +34,12 @@ def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp:
         # A dictionary from base station id to BsPairPoses, The poses represents the two possible poses of the base
         # stations found by IPPE, in the crazyflie reference frame.
         self.ippe_solutions: dict[int, BsPairPoses] = {}
-
         self.is_augmented = False
 
+        # Some samples are mandatory and must not be removed, even if they appear to be outliers. For instance the
+        # the samples that define the origin or x-axis
+        self.is_mandatory = False
+
     def augment_with_ippe(self, sensor_positions: ArrayFloat) -> None:
         if not self.is_augmented:
             self.ippe_solutions = self._find_ippe_solutions(self.angles_calibrated, sensor_positions)
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 160f29726..6a35e050a 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -51,7 +51,7 @@ def set_origin_sample(self, origin: LhCfPoseSample) -> None:
             origin (LhCfPoseSample): the new origin
         """
         self.origin = origin
-        self.origin.augment_with_ippe(self.sensor_positions)
+        self._augment_sample(self.origin, True)
 
     def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         """Store/update the sample to be used for the x_axis
@@ -60,7 +60,7 @@ def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
             x_axis (LhCfPoseSample): the new x-axis sample
         """
         self.x_axis = [x_axis]
-        self.x_axis[0].augment_with_ippe(self.sensor_positions)
+        self._augment_samples(self.x_axis, True)
 
     def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         """Store/update the samples to be used for the xy-plane
@@ -69,7 +69,7 @@ def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
             xy_plane (list[LhCfPoseSample]): the new xy-plane samples
         """
         self.xy_plane = xy_plane
-        self._augment_samples(self.xy_plane)
+        self._augment_samples(self.xy_plane, True)
 
     def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
         """Store/update the samples for the volume
@@ -78,7 +78,7 @@ def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
             samples (list[LhMeasurement]): the new samples
         """
         self.xyz_space = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
-        self._augment_samples(self.xyz_space)
+        self._augment_samples(self.xyz_space, False)
 
     def get_matched_samples(self) -> list[LhCfPoseSample]:
         """Get all pose samples collected in a list
@@ -88,6 +88,10 @@ def get_matched_samples(self) -> list[LhCfPoseSample]:
         """
         return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
 
-    def _augment_samples(self, samples: list[LhCfPoseSample]) -> None:
+    def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
+        sample.augment_with_ippe(self.sensor_positions)
+        sample.is_mandatory = is_mandatory
+
+    def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) -> None:
         for sample in samples:
-            sample.augment_with_ippe(self.sensor_positions)
+            self._augment_sample(sample, is_mandatory)
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index b861d5f29..3d74c37f9 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -176,7 +176,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
                     is_sample_valid = False
                     break
 
-            if is_sample_valid:
+            if is_sample_valid or sample.is_mandatory:
                 result.append(poses)
                 cleaned_matched_samples.append(sample)
 

From 5d950925f74c6df1b211bfe45d3fe7161f1eebf3 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 16 May 2025 10:43:15 +0200
Subject: [PATCH 09/36] Moved scaling into estimation manager

---
 .../lighthouse_geo_estimation_manager.py      | 30 ++++++++++++-
 .../lighthouse_geometry_solver.py             | 13 +++---
 .../multi_bs_geometry_estimation.py           | 45 ++++++-------------
 3 files changed, 47 insertions(+), 41 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 6a35e050a..a6f17a101 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -25,13 +25,41 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
-from cflib.localization.lighthouse_types import LhMeasurement
+from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
+from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
+from cflib.localization.lighthouse_types import LhBsCfPoses, LhMeasurement
 from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
 
 
 ArrayFloat = npt.NDArray[np.float_]
 
 
+class LhGeoEstimationManager():
+    @classmethod
+    def align_and_scale_solution(cls, container: LhGeoInputContainer, poses: LhBsCfPoses,
+                                 reference_distance: float) -> LhBsCfPoses:
+        start_idx_x_axis = 1
+        start_idx_xy_plane = 1 + len(container.x_axis)
+
+        origin_pos = poses.cf_poses[0].translation
+        x_axis_poses = poses.cf_poses[start_idx_x_axis:start_idx_x_axis + len(container.x_axis)]
+        x_axis_pos = list(map(lambda x: x.translation, x_axis_poses))
+        xy_plane_poses = poses.cf_poses[start_idx_xy_plane:start_idx_xy_plane + len(container.xy_plane)]
+        xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses))
+
+        # Align the solution
+        bs_aligned_poses, trnsfrm = LighthouseSystemAligner.align(origin_pos, x_axis_pos, xy_plane_pos, poses.bs_poses)
+        cf_aligned_poses = list(map(trnsfrm.rotate_translate_pose, poses.cf_poses))
+
+        # Scale the solution
+        bs_scaled_poses, cf_scaled_poses, scale = LighthouseSystemScaler.scale_fixed_point(bs_aligned_poses,
+                                                                                           cf_aligned_poses,
+                                                                                           [reference_distance, 0, 0],
+                                                                                           cf_aligned_poses[1])
+
+        return LhBsCfPoses(bs_poses=bs_scaled_poses, cf_poses=cf_scaled_poses)
+
+
 class LhGeoInputContainer():
     """This class holds the input data required by the geometry estimation functionality.
     """
diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index ffc324159..5548b1d8a 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -66,11 +66,8 @@ def __init__(self) -> None:
 
         # The solution ######################
 
-        # The estimated poses of the base stations
-        self.bs_poses: dict[int, Pose] = {}
-
-        # The estimated poses of the CF samples
-        self.cf_poses: list[Pose] = []
+        # The estimated poses of the base stations and the CF samples
+        self.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
 
         # Estimated error for each base station in each sample
         self.estimated_errors: list[dict[int, float]] = []
@@ -402,14 +399,14 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution,
 
         # Extract CF pose estimates
         # First pose (origin) is not in the parameter list
-        solution.cf_poses.append(Pose())
+        solution.poses.cf_poses.append(Pose())
         for i in range(len(matched_samples) - 1):
-            solution.cf_poses.append(cls._params_to_pose(cf_poses[i], solution))
+            solution.poses.cf_poses.append(cls._params_to_pose(cf_poses[i], solution))
 
         # Extract base station pose estimates
         for index, pose in enumerate(bss):
             bs_id = solution.bs_index_to_id[index]
-            solution.bs_poses[bs_id] = cls._params_to_pose(pose, solution)
+            solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, solution)
 
         solution.success = lsq_result.success
 
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 2705986b9..5287fdeaa 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -56,18 +56,16 @@
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
-from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
+from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
-from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
-from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
-from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import Pose
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhMeasurement
+from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.utils import uri_helper
 
 REFERENCE_DIST = 1.0
@@ -179,7 +177,7 @@ def set_axes_equal(ax):
     ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
 
 
-def visualize(cf_poses: list[Pose], bs_poses: list[Pose]):
+def visualize(poses: LhBsCfPoses):
     """Visualize positions of base stations and Crazyflie positions"""
     # Set to True to visualize positions
     # Requires PyPlot
@@ -187,7 +185,7 @@ def visualize(cf_poses: list[Pose], bs_poses: list[Pose]):
     if visualize_positions:
         import matplotlib.pyplot as plt
 
-        positions = np.array(list(map(lambda x: x.translation, cf_poses)))
+        positions = np.array(list(map(lambda x: x.translation, poses.cf_poses)))
 
         fig = plt.figure()
         ax = fig.add_subplot(projection='3d')
@@ -198,7 +196,7 @@ def visualize(cf_poses: list[Pose], bs_poses: list[Pose]):
 
         ax.scatter(x_cf, y_cf, z_cf)
 
-        positions = np.array(list(map(lambda x: x.translation, bs_poses)))
+        positions = np.array(list(map(lambda x: x.translation, poses.bs_poses.values())))
 
         x_bs = positions[:, 0]
         y_bs = positions[:, 1]
@@ -226,52 +224,35 @@ def estimate_geometry(container: LhGeoInputContainer) -> dict[int, Pose]:
     """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
     matched_samples = container.get_matched_samples()
     initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
+    scaled_initial_guess = LhGeoEstimationManager.align_and_scale_solution(container, initial_guess, REFERENCE_DIST)
 
     print('Initial guess base stations at:')
     print_base_stations_poses(initial_guess.bs_poses)
 
     print(f'{len(cleaned_matched_samples)} samples will be used')
-    visualize(initial_guess.cf_poses, initial_guess.bs_poses.values())
+    visualize(scaled_initial_guess)
 
     solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions)
     if not solution.success:
-        print('Solution did not converge, it might not be good!')
+        print('WARNING: Solution did not converge, it might not be good!')
 
-    start_x_axis = 1
-    start_xy_plane = 1 + len(container.x_axis)
-    origin_pos = solution.cf_poses[0].translation
-    x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(container.x_axis)]
-    x_axis_pos = list(map(lambda x: x.translation, x_axis_poses))
-    xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(container.xy_plane)]
-    xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses))
+    scaled_solution = LhGeoEstimationManager.align_and_scale_solution(container, solution.poses, REFERENCE_DIST)
 
     print('Raw solution:')
     print('  Base stations at:')
-    print_base_stations_poses(solution.bs_poses)
+    print_base_stations_poses(solution.poses.bs_poses)
     print('  Solution match per base station:')
     for bs_id, value in solution.error_info['bs'].items():
         print(f'    {bs_id + 1}: {value}')
 
-    # Align the solution
-    bs_aligned_poses, transformation = LighthouseSystemAligner.align(
-        origin_pos, x_axis_pos, xy_plane_pos, solution.bs_poses)
-
-    cf_aligned_poses = list(map(transformation.rotate_translate_pose, solution.cf_poses))
-
-    # Scale the solution
-    bs_scaled_poses, cf_scaled_poses, scale = LighthouseSystemScaler.scale_fixed_point(bs_aligned_poses,
-                                                                                       cf_aligned_poses,
-                                                                                       [REFERENCE_DIST, 0, 0],
-                                                                                       cf_aligned_poses[1])
-
     print()
     print('Final solution:')
     print('  Base stations at:')
-    print_base_stations_poses(bs_scaled_poses)
+    print_base_stations_poses(scaled_solution.bs_poses)
 
-    visualize(cf_scaled_poses, bs_scaled_poses.values())
+    visualize(scaled_solution)
 
-    return bs_scaled_poses
+    return scaled_solution.bs_poses
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):

From f1ec5cc456f16e7ad80090516445dedbb2d1eb7b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 16 May 2025 13:38:31 +0200
Subject: [PATCH 10/36] Added functions for live sample matching

---
 .../localization/lighthouse_sample_matcher.py | 75 +++++++++++++++----
 1 file changed, 59 insertions(+), 16 deletions(-)

diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
index 365e4e5dc..5dfb48eb5 100644
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ b/cflib/localization/lighthouse_sample_matcher.py
@@ -34,32 +34,75 @@ class LighthouseSampleMatcher:
     a list of LhCfPoseSample. Matching is done using the timestamp and a maximum time span.
     """
 
+    def __init__(self, max_time_diff: float = 0.020, min_nr_of_bs_in_match: int = 1) -> None:
+        self.max_time_diff = max_time_diff
+        self.min_nr_of_bs_in_match = min_nr_of_bs_in_match
+
+        self._current_angles: dict[int, LighthouseBsVectors] = {}
+        self._current_ts = 0.0
+
+    def match_one(self, sample: LhMeasurement) -> LhCfPoseSample | None:
+        """Aggregate samples close in time.
+        This function is used to match samples from multiple base stations into a single LhCfPoseSample.
+        The function will return None if the number of base stations in the sample is less than
+        the minimum number of base stations required for a match.
+        Note that a pose sample is returned upon the next call to this function, that is when the maximum time diff of
+        the first sample in the group has been exceeded.
+
+        Args:
+            sample (LhMeasurement): angles from one base station
+
+        Returns:
+            LhCfPoseSample | None: a pose sample if available, otherwise None
+        """
+        result = None
+        if len(self._current_angles) > 0:
+            if sample.timestamp > (self._current_ts + self.max_time_diff):
+                if len(self._current_angles) >= self.min_nr_of_bs_in_match:
+                    result = LhCfPoseSample(self._current_angles, timestamp=self._current_ts)
+
+                self._current_angles = {}
+
+        if len(self._current_angles) == 0:
+            self._current_ts = sample.timestamp
+
+        self._current_angles[sample.base_station_id] = sample.angles
+
+        return result
+
+    def purge(self) -> LhCfPoseSample | None:
+        """Purge the current angles and return a pose sample if available.
+
+        Returns:
+            LhCfPoseSample | None: a pose sample if available, otherwise None
+        """
+        result = None
+
+        if len(self._current_angles) >= self.min_nr_of_bs_in_match:
+            result = LhCfPoseSample(self._current_angles, timestamp=self._current_ts)
+
+        self._current_angles = {}
+        self._current_ts = 0.0
+
+        return result
+
     @classmethod
     def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.020,
               min_nr_of_bs_in_match: int = 1) -> list[LhCfPoseSample]:
         """
-        Aggregate samples close in time into lists
+        Aggregate samples in a list
         """
 
         result = []
-        current_angles: dict[int, LighthouseBsVectors] = {}
-        current_ts = 0.0
+        matcher = cls(max_time_diff, min_nr_of_bs_in_match)
 
         for sample in samples:
-            if len(current_angles) > 0:
-                if sample.timestamp > (current_ts + max_time_diff):
-                    if len(current_angles) >= min_nr_of_bs_in_match:
-                        pose_sample = LhCfPoseSample(current_angles, timestamp=current_ts)
-                        result.append(pose_sample)
-
-                    current_angles = {}
-
-            if len(current_angles) == 0:
-                current_ts = sample.timestamp
-            current_angles[sample.base_station_id] = sample.angles
+            pose_sample = matcher.match_one(sample)
+            if pose_sample is not None:
+                result.append(pose_sample)
 
-        if len(current_angles) >= min_nr_of_bs_in_match:
-            pose_sample = LhCfPoseSample(current_angles, timestamp=current_ts)
+        pose_sample = matcher.purge()
+        if pose_sample is not None:
             result.append(pose_sample)
 
         return result

From 24de4a580ea8307fe5762f2810b3c45d3929c9a3 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 5 Jun 2025 18:44:07 +0200
Subject: [PATCH 11/36] Updated tests

---
 .../test_lighthouse_geometry_solver.py        | 10 ++++----
 .../test_lighthouse_initial_estimator.py      | 24 ++++++++++++++-----
 2 files changed, 24 insertions(+), 10 deletions(-)

diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index 275a897aa..7a816a368 100644
--- a/test/localization/test_lighthouse_geometry_solver.py
+++ b/test/localization/test_lighthouse_geometry_solver.py
@@ -43,9 +43,10 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self):
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
             }),
         ]
+        for sample in matched_samples:
+            sample.augment_with_ippe(LhDeck4SensorPositions.positions)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples,
-                                                                                     LhDeck4SensorPositions.positions)
+        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
 
         # Test
         actual = LighthouseGeometrySolver.solve(
@@ -77,9 +78,10 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self):
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
         ]
+        for sample in matched_samples:
+            sample.augment_with_ippe(LhDeck4SensorPositions.positions)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples,
-                                                                                     LhDeck4SensorPositions.positions)
+        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
 
         # Test
         actual = LighthouseGeometrySolver.solve(
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index abe0b7311..ee4aa7f2a 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -42,11 +42,12 @@ def test_that_one_bs_pose_raises_exception(self):
         samples = [
             LhCfPoseSample(angles_calibrated={bs_id: self.fixtures.angles_cf_origin_bs0}),
         ]
+        self.augment(samples)
 
         # Test
         # Assert
         with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+            LighthouseInitialEstimator.estimate(samples)
 
     def test_that_two_bs_poses_in_same_sample_are_found(self):
         # Fixture
@@ -59,9 +60,10 @@ def test_that_two_bs_poses_in_same_sample_are_found(self):
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
             }),
         ]
+        self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
@@ -88,9 +90,10 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self):
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
         ]
+        self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
@@ -119,9 +122,10 @@ def test_that_cf_poses_are_estimated(self):
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
         ]
+        self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, actual.cf_poses[0], places=3)
@@ -144,9 +148,10 @@ def test_that_the_global_ref_frame_is_used(self):
                 bs_id2: self.fixtures.angles_cf1_bs2,
             }),
         ]
+        self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
 
         # Assert
         self.assertPosesAlmostEqual(
@@ -172,8 +177,15 @@ def test_that_raises_for_isolated_bs(self):
                 bs_id3: self.fixtures.angles_cf2_bs2,
             }),
         ]
+        self.augment(samples)
 
         # Test
         # Assert
         with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples, LhDeck4SensorPositions.positions)
+            LighthouseInitialEstimator.estimate(samples)
+
+###### helpers
+
+    def augment(self, samples):
+        for sample in samples:
+            sample.augment_with_ippe(LhDeck4SensorPositions.positions)

From 7853d2b598dbbaf180d72ea39bcd284eff562eab Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 5 Jun 2025 19:18:15 +0200
Subject: [PATCH 12/36] Corrected test

---
 test/localization/test_lighthouse_geometry_solver.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index 7a816a368..585307f65 100644
--- a/test/localization/test_lighthouse_geometry_solver.py
+++ b/test/localization/test_lighthouse_geometry_solver.py
@@ -53,7 +53,7 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self):
             initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions)
 
         # Assert
-        bs_poses = actual.bs_poses
+        bs_poses = actual.poses.bs_poses
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3)
 
@@ -88,7 +88,7 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self):
             initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions)
 
         # Assert
-        bs_poses = actual.bs_poses
+        bs_poses = actual.poses.bs_poses
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, bs_poses[bs_id2], places=3)

From 3d7eb8e1715f4bc787017a39551bdb2ff086b178 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 5 Jun 2025 19:22:26 +0200
Subject: [PATCH 13/36] Styling

---
 cflib/localization/lighthouse_cf_pose_sample.py         | 1 +
 cflib/localization/lighthouse_geo_estimation_manager.py | 6 ++++--
 cflib/localization/lighthouse_initial_estimator.py      | 2 +-
 cflib/localization/lighthouse_sample_matcher.py         | 2 +-
 cflib/localization/lighthouse_sweep_angle_reader.py     | 8 +++++---
 test/localization/test_lighthouse_geometry_solver.py    | 2 +-
 test/localization/test_lighthouse_initial_estimator.py  | 4 ++--
 7 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index d091bb73e..ef95ecfb1 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -1,4 +1,5 @@
 from typing import NamedTuple
+
 import numpy as np
 import numpy.typing as npt
 
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index a6f17a101..f69f5c5d6 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -25,10 +25,11 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
-from cflib.localization.lighthouse_types import LhBsCfPoses, LhMeasurement
-from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
+from cflib.localization.lighthouse_types import LhBsCfPoses
+from cflib.localization.lighthouse_types import LhMeasurement
 
 
 ArrayFloat = npt.NDArray[np.float_]
@@ -63,6 +64,7 @@ def align_and_scale_solution(cls, container: LhGeoInputContainer, poses: LhBsCfP
 class LhGeoInputContainer():
     """This class holds the input data required by the geometry estimation functionality.
     """
+
     def __init__(self, sensor_positions: ArrayFloat) -> None:
         self.EMPTY_POSE_SAMPLE = LhCfPoseSample(angles_calibrated={})
         self.sensor_positions = sensor_positions
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 3d74c37f9..134fea47b 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -26,8 +26,8 @@
 import numpy as np
 import numpy.typing as npt
 
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import BsPairPoses
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.localization.lighthouse_types import LhException
 from cflib.localization.lighthouse_types import Pose
diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
index 5dfb48eb5..ccc9c94d4 100644
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ b/cflib/localization/lighthouse_sample_matcher.py
@@ -21,9 +21,9 @@
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from __future__ import annotations
 
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhMeasurement
-from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 
 
 class LighthouseSampleMatcher:
diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py
index d421c323a..5de9ff6e4 100644
--- a/cflib/localization/lighthouse_sweep_angle_reader.py
+++ b/cflib/localization/lighthouse_sweep_angle_reader.py
@@ -19,10 +19,11 @@
 #
 # You should have received a copy of the GNU General Public License
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
+from collections.abc import Callable
+
+from cflib.crazyflie import Crazyflie
 from cflib.localization import LighthouseBsVector
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
-from cflib.crazyflie import Crazyflie
-from collections.abc import Callable
 
 
 class LighthouseSweepAngleReader():
@@ -130,7 +131,8 @@ def _has_collected_enough_data(self, storage: dict[int, list[list[LighthouseBsVe
                 return True
         return False
 
-    def _average_all_lists(self, storage: dict[int, list[list[LighthouseBsVector]]]) -> dict[int, tuple[int, LighthouseBsVectors]]:
+    def _average_all_lists(self, storage: dict[int, list[list[LighthouseBsVector]]]
+                           ) -> dict[int, tuple[int, LighthouseBsVectors]]:
         result: dict[int, tuple[int, LighthouseBsVectors]] = {}
 
         for bs_id, sample_lists in storage.items():
diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index 585307f65..ea2987ed1 100644
--- a/test/localization/test_lighthouse_geometry_solver.py
+++ b/test/localization/test_lighthouse_geometry_solver.py
@@ -22,9 +22,9 @@
 from test.localization.lighthouse_fixtures import LighthouseFixtures
 from test.localization.lighthouse_test_base import LighthouseTestBase
 
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 
 
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index ee4aa7f2a..8d25a17a4 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -24,9 +24,9 @@
 
 import numpy as np
 
-from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import Pose
+from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhException
 
@@ -184,7 +184,7 @@ def test_that_raises_for_isolated_bs(self):
         with self.assertRaises(LhException):
             LighthouseInitialEstimator.estimate(samples)
 
-###### helpers
+# helpers
 
     def augment(self, samples):
         for sample in samples:

From 6fe04c392c3e520d759eb26c15786dfad11020a5 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 5 Jun 2025 19:34:03 +0200
Subject: [PATCH 14/36] styling

---
 examples/lighthouse/multi_bs_geometry_estimation.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 5287fdeaa..93bc1219c 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -55,17 +55,18 @@
 from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import Pose
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
-from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer, LhGeoEstimationManager
+from cflib.localization.lighthouse_geo_estimation_manager import LhGeoEstimationManager
+from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
-from cflib.localization.lighthouse_cf_pose_sample import Pose
+from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhMeasurement
-from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.utils import uri_helper
 
 REFERENCE_DIST = 1.0

From 1e81175f2ffaa369fbdefb94952fe70c706b798f Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 12 Jun 2025 15:35:05 +0200
Subject: [PATCH 15/36] basic continuous geo estimation

---
 .../lighthouse_geo_estimation_manager.py      | 158 +++++++++++++++---
 .../lighthouse_geometry_solver.py             |  13 +-
 .../multi_bs_geometry_estimation.py           |  95 +++++------
 3 files changed, 184 insertions(+), 82 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index f69f5c5d6..373be9b62 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -21,10 +21,16 @@
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from __future__ import annotations
 
+import copy
+import threading
+
 import numpy as np
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolution
+from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
+from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
@@ -36,16 +42,25 @@
 
 
 class LhGeoEstimationManager():
+    REFERENCE_DIST = 1.0  # Reference distance used for scaling the solution
+
     @classmethod
-    def align_and_scale_solution(cls, container: LhGeoInputContainer, poses: LhBsCfPoses,
+    def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhBsCfPoses,
                                  reference_distance: float) -> LhBsCfPoses:
+
+        if len(container.x_axis) == 0 or len(container.xy_plane) == 0:
+            # Return unaligned solution for now
+            # TODO krri Add information that the solution is not aligned
+            return LhBsCfPoses(bs_poses=poses.bs_poses, cf_poses=poses.cf_poses)
+
         start_idx_x_axis = 1
         start_idx_xy_plane = 1 + len(container.x_axis)
+        start_idx_xyz_space = start_idx_xy_plane + len(container.xy_plane)
 
         origin_pos = poses.cf_poses[0].translation
         x_axis_poses = poses.cf_poses[start_idx_x_axis:start_idx_x_axis + len(container.x_axis)]
         x_axis_pos = list(map(lambda x: x.translation, x_axis_poses))
-        xy_plane_poses = poses.cf_poses[start_idx_xy_plane:start_idx_xy_plane + len(container.xy_plane)]
+        xy_plane_poses = poses.cf_poses[start_idx_xy_plane:start_idx_xyz_space]
         xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses))
 
         # Align the solution
@@ -60,11 +75,74 @@ def align_and_scale_solution(cls, container: LhGeoInputContainer, poses: LhBsCfP
 
         return LhBsCfPoses(bs_poses=bs_scaled_poses, cf_poses=cf_scaled_poses)
 
+    @classmethod
+    def estimate_geometry(cls, container: LhGeoInputContainerData) -> tuple[LighthouseGeometrySolution, LhBsCfPoses]:
+        """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
+        matched_samples = container.get_matched_samples()
+        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
 
-class LhGeoInputContainer():
-    """This class holds the input data required by the geometry estimation functionality.
-    """
+        solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions)
+        scaled_solution = cls.align_and_scale_solution(container, solution.poses, cls.REFERENCE_DIST)
+
+        return solution, scaled_solution
+
+    class SolverThread(threading.Thread):
+        """This class runs the geometry solver in a separate thread.
+        It is used to provide continuous updates of the solution as well as updating the geometry in the Crazyflie.
+        """
+
+        def __init__(self, container: LhGeoInputContainer, is_done_cb) -> None:
+            """This constructor initializes the solver thread and starts it.
+            It takes a container with the input data and an callback that is called when the solution is done.
+            The thread will run the geometry solver and return the solution in the callback as soon as the data in the
+            container is modified.
+            Args:
+                container (LhGeoInputContainer): A container with the input data for the geometry estimation.
+                is_done_cb: Callback function that is called when the solution is done.
+            """
+            threading.Thread.__init__(self, name='LhGeoEstimationManager.SolverThread')
+
+            self.container = container
+            self.latest_solved_data_version = container._data.version
 
+            self.is_done_cb = is_done_cb
+
+            self.is_running = False
+            self.is_done = False
+            self.time_to_stop = False
+
+        def run(self):
+            """Run the geometry solver in a separate thread"""
+            self.is_running = True
+
+            with self.container.is_modified_condition:
+                while True:
+                    if self.time_to_stop:
+                        break
+
+                    if self.container._data.version > self.latest_solved_data_version:
+                        self.is_done = False
+
+                        # Copy the container as the original container may be modified while the solver is running
+                        container_copy = copy.deepcopy(self.container._data)
+                        solution, scaled_solution = LhGeoEstimationManager.estimate_geometry(container_copy)
+                        self.latest_solved_data_version = container_copy.version
+
+                        self.is_done = True
+                        self.is_done_cb(scaled_solution)
+
+                    self.container.is_modified_condition.wait(timeout=0.1)
+
+            self.is_running = False
+
+        def stop(self):
+            """Stop the solver thread"""
+            self.time_to_stop = True
+            if self.is_running:
+                self.join()
+
+
+class LhGeoInputContainerData():
     def __init__(self, sensor_positions: ArrayFloat) -> None:
         self.EMPTY_POSE_SAMPLE = LhCfPoseSample(angles_calibrated={})
         self.sensor_positions = sensor_positions
@@ -74,14 +152,34 @@ def __init__(self, sensor_positions: ArrayFloat) -> None:
         self.xy_plane: list[LhCfPoseSample] = []
         self.xyz_space: list[LhCfPoseSample] = []
 
+        self.version = 0
+
+    def get_matched_samples(self) -> list[LhCfPoseSample]:
+        """Get all pose samples collected in a list
+
+        Returns:
+            list[LhCfPoseSample]: _description_
+        """
+        return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
+
+
+class LhGeoInputContainer():
+    """This class holds the input data required by the geometry estimation functionality.
+    """
+
+    def __init__(self, sensor_positions: ArrayFloat) -> None:
+        self._data = LhGeoInputContainerData(sensor_positions)
+        self.is_modified_condition = threading.Condition()
+
     def set_origin_sample(self, origin: LhCfPoseSample) -> None:
         """Store/update the sample to be used for the origin
 
         Args:
             origin (LhCfPoseSample): the new origin
         """
-        self.origin = origin
-        self._augment_sample(self.origin, True)
+        self._data.origin = origin
+        self._augment_sample(self._data.origin, True)
+        self._update_version()
 
     def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         """Store/update the sample to be used for the x_axis
@@ -89,8 +187,9 @@ def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         Args:
             x_axis (LhCfPoseSample): the new x-axis sample
         """
-        self.x_axis = [x_axis]
-        self._augment_samples(self.x_axis, True)
+        self._data.x_axis = [x_axis]
+        self._augment_samples(self._data.x_axis, True)
+        self._update_version()
 
     def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         """Store/update the samples to be used for the xy-plane
@@ -98,8 +197,19 @@ def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         Args:
             xy_plane (list[LhCfPoseSample]): the new xy-plane samples
         """
-        self.xy_plane = xy_plane
-        self._augment_samples(self.xy_plane, True)
+        self._data.xy_plane = xy_plane
+        self._augment_samples(self._data.xy_plane, True)
+        self._update_version()
+
+    def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
+        """append to the samples to be used for the xy-plane
+
+        Args:
+            xy_plane (LhCfPoseSample): the new xy-plane sample
+        """
+        self._augment_sample(xy_plane, True)
+        self._data.xy_plane.append(xy_plane)
+        self._update_version()
 
     def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
         """Store/update the samples for the volume
@@ -107,21 +217,31 @@ def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
         Args:
             samples (list[LhMeasurement]): the new samples
         """
-        self.xyz_space = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
-        self._augment_samples(self.xyz_space, False)
+        self._data.xyz_space = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
+        self._augment_samples(self._data.xyz_space, False)
+        self._update_version()
 
-    def get_matched_samples(self) -> list[LhCfPoseSample]:
-        """Get all pose samples collected in a list
+    def append_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
+        """Append to the samples for the volume
 
-        Returns:
-            list[LhCfPoseSample]: _description_
+        Args:
+            samples (LhMeasurement): the new samples
         """
-        return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
+        new_samples = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
+        self._augment_samples(new_samples, False)
+        self._data.xyz_space += new_samples
+        self._update_version()
 
     def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
-        sample.augment_with_ippe(self.sensor_positions)
+        sample.augment_with_ippe(self._data.sensor_positions)
         sample.is_mandatory = is_mandatory
 
     def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) -> None:
         for sample in samples:
             self._augment_sample(sample, is_mandatory)
+
+    def _update_version(self) -> None:
+        """Update the data version and notify the waiting thread"""
+        with self.is_modified_condition:
+            self._data.version += 1
+            self.is_modified_condition.notify()
diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index 5548b1d8a..e84646238 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -45,7 +45,7 @@ def __init__(self) -> None:
 
         # Nr of base stations
         self.n_bss: int = None
-        # Nr of parametrs per base station
+        # Nr of parameters per base station
         self.n_params_per_bs = self.len_pose
 
         # Nr of sampled Crazyflie poses
@@ -76,8 +76,8 @@ def __init__(self) -> None:
         self.error_info = {}
 
         # Indicates if the solution converged (True).
-        # If it did not converge, the solution is probably not good enough to use
-        self.success = False
+        # If it did not converge, the solution is possibly not good enough to use
+        self.has_converged = False
 
 
 class LighthouseGeometrySolver:
@@ -137,8 +137,9 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
         Solve for the pose of base stations and CF samples.
         The pose of the CF in sample 0 defines the global reference frame.
 
-        Iteration is terminated acceptable solution is found. If no solution is found after a fixed number of iterations
-        the solver is terminated. The success member of the result will indicate if a solution was found or not.
+        Iteration is terminated when an acceptable solution is found. If no solution is found after a fixed number of
+        iterations the solver is terminated. The has_converged member of the result will indicate if a solution was
+        found or not. Note: the solution may still be good enough to use even if it did not converge.
 
         :param initial_guess: Initial guess for the base stations and CF sample poses
         :param matched_samples: List of matched samples.
@@ -408,7 +409,7 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution,
             bs_id = solution.bs_index_to_id[index]
             solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, solution)
 
-        solution.success = lsq_result.success
+        solution.has_converged = lsq_result.success
 
         # Extract the error for each CF pose
         residuals = lsq_result.fun
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 93bc1219c..7b348c957 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -60,8 +60,6 @@
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoEstimationManager
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
-from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
-from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
 from cflib.localization.lighthouse_types import LhBsCfPoses
@@ -221,39 +219,12 @@ def load_from_file(name: str) -> LhGeoInputContainer:
         return pickle.load(handle)
 
 
-def estimate_geometry(container: LhGeoInputContainer) -> dict[int, Pose]:
-    """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
-    matched_samples = container.get_matched_samples()
-    initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
-    scaled_initial_guess = LhGeoEstimationManager.align_and_scale_solution(container, initial_guess, REFERENCE_DIST)
-
-    print('Initial guess base stations at:')
-    print_base_stations_poses(initial_guess.bs_poses)
-
-    print(f'{len(cleaned_matched_samples)} samples will be used')
-    visualize(scaled_initial_guess)
-
-    solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions)
-    if not solution.success:
-        print('WARNING: Solution did not converge, it might not be good!')
-
-    scaled_solution = LhGeoEstimationManager.align_and_scale_solution(container, solution.poses, REFERENCE_DIST)
-
-    print('Raw solution:')
-    print('  Base stations at:')
-    print_base_stations_poses(solution.poses.bs_poses)
-    print('  Solution match per base station:')
-    for bs_id, value in solution.error_info['bs'].items():
-        print(f'    {bs_id + 1}: {value}')
-
-    print()
-    print('Final solution:')
+def solution_handler(scaled_solution: LhBsCfPoses):
+    print('Solution ready:')
     print('  Base stations at:')
     print_base_stations_poses(scaled_solution.bs_poses)
-
-    visualize(scaled_solution)
-
-    return scaled_solution.bs_poses
+    # visualize(thread.scaled_solution)
+    # upload_geometry(thread.container.scf, thread.scaled_solution.bs_poses)
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):
@@ -278,7 +249,10 @@ def data_written(_):
 
 def estimate_from_file(file_name: str):
     container = load_from_file(file_name)
-    estimate_geometry(container)
+    thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=solution_handler)
+    thread.start()
+    time.sleep(1)
+    thread.stop()
 
 
 def get_recording(scf: SyncCrazyflie) -> LhCfPoseSample:
@@ -329,6 +303,10 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
     print(f'Step 1. Connecting to the Crazyflie on uri {uri}...')
     with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
         container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
+        print('Starting geometry estimation thread...')
+        thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=solution_handler)
+        thread.start()
+
         print('  Connected')
         print('')
         print('In the 3 following steps we will define the coordinate system.')
@@ -341,34 +319,37 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
               'This position defines the direction of the X-axis, but it is also used for scaling the system.')
         container.set_x_axis_sample(get_recording(scf))
 
-        print('Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.')
+        print('Step 4. Put the Crazyflie somewhere in the XY-plane, but not on the X-axis.')
         print('Multiple samples can be recorded if you want to.')
         container.set_xy_plane_samples(get_multiple_recordings(scf))
 
         print()
         print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' +
-              'geometry based on this data. Move the Crazyflie around, try to cover all of the space, make sure ' +
-              'all the base stations are received and do not move too fast.')
-        default_time = 20
-        recording_time = input(f'Enter the number of seconds you want to record ({default_time} by default), ' +
-                               'recording starts when you hit enter. ')
-        recording_time_s = parse_recording_time(recording_time, default_time)
-        print('  Recording started...')
-        container.set_xyz_space_samples(record_angles_sequence(scf, recording_time_s))
-        print('  Recording ended')
-
-        if file_name:
-            write_to_file(file_name, container)
-            print(f'Wrote data to file {file_name}')
-
-        print('Step 6. Estimating geometry...')
-        bs_poses = estimate_geometry(container)
-        print('  Geometry estimated')
-
-        print('Step 7. Upload geometry to the Crazyflie')
-        input('Press enter to upload geometry. ')
-        upload_geometry(scf, bs_poses)
-        print('Geometry uploaded')
+              'geometry based on this data.')
+        recording_time_s = 1.0
+        first_attempt = True
+
+        while True:
+            if first_attempt:
+                user_input = input('Press return to record a measurement: ').lower()
+                first_attempt = False
+            else:
+                user_input = input('Press return to record another measurement, or "q" to continue: ').lower()
+
+            if user_input == 'q':
+                break
+
+            measurement = record_angles_sequence(scf, recording_time_s)
+            if measurement is not None:
+                container.append_xyz_space_samples(measurement)
+            else:
+                print('No data recorded, please try again.')
+
+        thread.stop()
+
+        # if file_name:
+        #     write_to_file(file_name, container)
+        #     print(f'Wrote data to file {file_name}')
 
 
 # Only output errors from the logging framework

From a5245356e714d4b0105ac3295fe00e2ad708c3c8 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 13 Jun 2025 15:56:39 +0200
Subject: [PATCH 16/36] Unified solution data and added human readable
 information

---
 .../lighthouse_geo_estimation_manager.py      | 136 +++++++++++++++---
 .../lighthouse_geometry_solution.py           |  52 +++++++
 .../lighthouse_geometry_solver.py             |  80 +++++------
 .../lighthouse_initial_estimator.py           |  25 +++-
 .../multi_bs_geometry_estimation.py           |  17 ++-
 .../test_lighthouse_geometry_solver.py        |  18 +--
 .../test_lighthouse_initial_estimator.py      |  14 +-
 7 files changed, 255 insertions(+), 87 deletions(-)
 create mode 100644 cflib/localization/lighthouse_geometry_solution.py

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 373be9b62..174761d44 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -28,7 +28,7 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
-from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolution
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
@@ -47,12 +47,6 @@ class LhGeoEstimationManager():
     @classmethod
     def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhBsCfPoses,
                                  reference_distance: float) -> LhBsCfPoses:
-
-        if len(container.x_axis) == 0 or len(container.xy_plane) == 0:
-            # Return unaligned solution for now
-            # TODO krri Add information that the solution is not aligned
-            return LhBsCfPoses(bs_poses=poses.bs_poses, cf_poses=poses.cf_poses)
-
         start_idx_x_axis = 1
         start_idx_xy_plane = 1 + len(container.x_axis)
         start_idx_xyz_space = start_idx_xy_plane + len(container.xy_plane)
@@ -76,15 +70,109 @@ def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhB
         return LhBsCfPoses(bs_poses=bs_scaled_poses, cf_poses=cf_scaled_poses)
 
     @classmethod
-    def estimate_geometry(cls, container: LhGeoInputContainerData) -> tuple[LighthouseGeometrySolution, LhBsCfPoses]:
+    def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeometrySolution:
         """Estimate the geometry of the system based on samples recorded by a Crazyflie"""
+        solution = LighthouseGeometrySolution()
+
         matched_samples = container.get_matched_samples()
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
+        solution.progress_info = 'Data validation'
+        validated_matched_samples = cls._data_validation(matched_samples, container, solution)
+        if solution.progress_is_ok:
+            solution.progress_info = 'Initial estimation of geometry'
+            initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(validated_matched_samples,
+                                                                                         solution)
+            solution.poses = initial_guess
 
-        solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions)
-        scaled_solution = cls.align_and_scale_solution(container, solution.poses, cls.REFERENCE_DIST)
+            if solution.progress_is_ok:
+                solution.progress_info = 'Refining geometry solution'
+                LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions,
+                                               solution)
+                solution.progress_info = 'Align and scale solution'
+                scaled_solution = cls.align_and_scale_solution(container, solution.poses, cls.REFERENCE_DIST)
+                solution.poses = scaled_solution
 
-        return solution, scaled_solution
+        cls._humanize_error_info(solution, container)
+        # TODO krri indicate in the solution if there is a geometry
+
+        # TODO krri create linkage map
+
+        return solution
+
+    @classmethod
+    def _data_validation(cls, matched_samples: list[LhCfPoseSample], container: LhGeoInputContainerData,
+                         solution: LighthouseGeometrySolution) -> list[LhCfPoseSample]:
+        """Validate the data collected by the Crazyflie and update the solution object with the results"""
+
+        result = []
+
+        NO_DATA = 'No data'
+        TOO_FEW_BS = 'Too few base stations recorded'
+
+        # Check the origin sample
+        origin = container.origin
+        if len(origin.angles_calibrated) == 0:
+            solution.append_mandatory_issue_sample(origin, NO_DATA)
+        elif len(origin.angles_calibrated) == 1:
+            solution.append_mandatory_issue_sample(origin, TOO_FEW_BS)
+
+        # Check the x-axis samples
+        if len(container.x_axis) == 0:
+            solution.is_x_axis_samples_valid = False
+            solution.x_axis_samples_info = NO_DATA
+            solution.progress_is_ok = False
+
+        if len(container.xy_plane) == 0:
+            solution.is_xy_plane_samples_valid = False
+            solution.xy_plane_samples_info = NO_DATA
+            solution.progress_is_ok = False
+
+        if len(container.xyz_space) == 0:
+            solution.xyz_space_samples_info = NO_DATA
+
+        # Samples must contain at least two base stations
+        for sample in matched_samples:
+            if sample == container.origin:
+                continue  # The origin sample is already checked
+
+            if len(sample.angles_calibrated) >= 2:
+                result.append(sample)
+            else:
+                # If the sample is mandatory, we cannot remove it, but we can add an issue to the solution
+                if sample.is_mandatory:
+                    solution.append_mandatory_issue_sample(sample, TOO_FEW_BS)
+                else:
+                    # If the sample is not mandatory, we can ignore it
+                    solution.xyz_space_samples_info = 'Sample(s) with too few base stations skipped'
+                    continue
+
+        return result
+
+    @classmethod
+    def _humanize_error_info(cls, solution: LighthouseGeometrySolution, container: LhGeoInputContainerData) -> None:
+        """Humanize the error info in the solution object"""
+        if solution.is_origin_sample_valid:
+            solution.is_origin_sample_valid, solution.origin_sample_info = cls._error_info_for(solution,
+                                                                                               [container.origin])
+        if solution.is_x_axis_samples_valid:
+            solution.is_x_axis_samples_valid, solution.x_axis_samples_info = cls._error_info_for(solution,
+                                                                                                 container.x_axis)
+        if solution.is_xy_plane_samples_valid:
+            solution.is_xy_plane_samples_valid, solution.xy_plane_samples_info = cls._error_info_for(solution,
+                                                                                                     container.xy_plane)
+
+    @classmethod
+    def _error_info_for(cls, solution: LighthouseGeometrySolution, samples: list[LhCfPoseSample]) -> tuple[bool, str]:
+        """Check if any issue sample is registered and return a human readable error message"""
+        info_strings = []
+        for sample in samples:
+            for issue_sample, issue in solution.mandatory_issue_samples:
+                if sample == issue_sample:
+                    info_strings.append(issue)
+
+        if len(info_strings) > 0:
+            return False, ', '.join(info_strings)
+        else:
+            return True, ''
 
     class SolverThread(threading.Thread):
         """This class runs the geometry solver in a separate thread.
@@ -120,16 +208,16 @@ def run(self):
                     if self.time_to_stop:
                         break
 
-                    if self.container._data.version > self.latest_solved_data_version:
+                    if self.container.get_data_version() > self.latest_solved_data_version:
                         self.is_done = False
 
                         # Copy the container as the original container may be modified while the solver is running
-                        container_copy = copy.deepcopy(self.container._data)
-                        solution, scaled_solution = LhGeoEstimationManager.estimate_geometry(container_copy)
+                        container_copy = self.container.get_data_copy()
+                        solution = LhGeoEstimationManager.estimate_geometry(container_copy)
                         self.latest_solved_data_version = container_copy.version
 
                         self.is_done = True
-                        self.is_done_cb(scaled_solution)
+                        self.is_done_cb(solution)
 
                     self.container.is_modified_condition.wait(timeout=0.1)
 
@@ -240,6 +328,22 @@ def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) ->
         for sample in samples:
             self._augment_sample(sample, is_mandatory)
 
+    def get_data_version(self) -> int:
+        """Get the current data version
+
+        Returns:
+            int: The current data version
+        """
+        return self._data.version
+
+    def get_data_copy(self) -> LhGeoInputContainerData:
+        """Get a copy of the data in the container
+
+        Returns:
+            LhGeoInputContainerData: A copy of the data in the container
+        """
+        return copy.deepcopy(self._data)
+
     def _update_version(self) -> None:
         """Update the data version and notify the waiting thread"""
         with self.is_modified_condition:
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
new file mode 100644
index 000000000..83a0399bc
--- /dev/null
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -0,0 +1,52 @@
+
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_types import LhBsCfPoses
+
+
+class LighthouseGeometrySolution:
+    """
+    A class to represent the solution of a lighthouse geometry problem.
+    """
+
+    def __init__(self):
+        # The estimated poses of the base stations and the CF samples
+        self.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
+
+        # Information about errors in the solution
+        # TODO krri This data is not well structured
+        self.error_info = {}
+
+        # Indicates if the solution converged (True).
+        # If it did not converge, the solution is possibly not good enough to use
+        self.has_converged = False
+
+        # Progress information stating how far in the solution process we got
+        self.progress_info = ""
+
+        # Indicates that all previous steps in the solution process were successful and that the next step
+        # can be executed. This is used to determine if the solution process can continue.
+        self.progress_is_ok = True
+
+        # Issue descriptions
+        self.is_origin_sample_valid = True
+        self.origin_sample_info = ''
+        self.is_x_axis_samples_valid = True
+        self.x_axis_samples_info = ''
+        self.is_xy_plane_samples_valid = True
+        self.xy_plane_samples_info = ''
+        # For the xyz space, there are not any stopping errors, this string may contain information for the user though
+        self.xyz_space_samples_info = ''
+
+        # Samples that are mandatory for the solution but where problems were encountered. The tuples contain the sample
+        # and a description of the issue. This list is used to extract issue descriptions for the user interface.
+        self.mandatory_issue_samples: list[tuple[LhCfPoseSample, str]] = []
+
+    def append_mandatory_issue_sample(self, sample: LhCfPoseSample, issue: str):
+        """
+        Append a sample with an issue to the list of mandatory issue samples.
+
+        :param sample: The CF pose sample that has an issue.
+        :param issue: A description of the issue with the sample.
+        """
+        self.mandatory_issue_samples.append((sample, issue))
+        self.progress_is_ok = False
diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index e84646238..f0f5ae43a 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -27,10 +27,11 @@
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import Pose
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_types import LhBsCfPoses
 
 
-class LighthouseGeometrySolution:
+class SolverData:
     """
     Represents a solution from the geometry solver.
 
@@ -64,21 +65,6 @@ def __init__(self) -> None:
         self.bs_id_to_index: dict[int, int] = {}
         self.bs_index_to_id: dict[int, int] = {}
 
-        # The solution ######################
-
-        # The estimated poses of the base stations and the CF samples
-        self.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
-
-        # Estimated error for each base station in each sample
-        self.estimated_errors: list[dict[int, float]] = []
-
-        # Information about errors in the solution
-        self.error_info = {}
-
-        # Indicates if the solution converged (True).
-        # If it did not converge, the solution is possibly not good enough to use
-        self.has_converged = False
-
 
 class LighthouseGeometrySolver:
     """
@@ -132,7 +118,7 @@ class LighthouseGeometrySolver:
 
     @classmethod
     def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample],
-              sensor_positions: npt.ArrayLike) -> LighthouseGeometrySolution:
+              sensor_positions: npt.ArrayLike, solution: LighthouseGeometrySolution) -> None:
         """
         Solve for the pose of base stations and CF samples.
         The pose of the CF in sample 0 defines the global reference frame.
@@ -144,23 +130,23 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
         :param initial_guess: Initial guess for the base stations and CF sample poses
         :param matched_samples: List of matched samples.
         :param sensor_positions: Sensor positions (3D), in the CF reference frame
-        :return: an instance of LighthouseGeometrySolution
+        :param solution: an instance of LighthouseGeometrySolution that is filled with the result
         """
-        solution = LighthouseGeometrySolution()
+        defs = SolverData()
 
-        solution.n_bss = len(initial_guess.bs_poses)
-        solution.n_cfs = len(matched_samples)
-        solution.n_cfs_in_params = len(matched_samples) - 1
-        solution.n_sensors = len(sensor_positions)
-        solution.bs_id_to_index, solution.bs_index_to_id = cls._create_bs_map(initial_guess.bs_poses)
+        defs.n_bss = len(initial_guess.bs_poses)
+        defs.n_cfs = len(matched_samples)
+        defs.n_cfs_in_params = len(matched_samples) - 1
+        defs.n_sensors = len(sensor_positions)
+        defs.bs_id_to_index, defs.bs_index_to_id = cls._create_bs_map(initial_guess.bs_poses)
 
         target_angles = cls._populate_target_angles(matched_samples)
         idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, jac_sparsity = cls._populate_indexes_and_jacobian(
-            matched_samples, solution)
-        params_bs, params_cfs = cls._populate_initial_guess(initial_guess, solution)
+            matched_samples, defs)
+        params_bs, params_cfs = cls._populate_initial_guess(initial_guess, defs)
 
         # Extra arguments passed on to calc_residual()
-        args = (solution, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions)
+        args = (defs, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions)
 
         # Vector to optimize. Composed of base station parameters followed by cf parameters
         x0 = np.hstack((params_bs.ravel(), params_cfs.ravel()))
@@ -172,11 +158,10 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
                                               x_scale='jac',
                                               ftol=1e-8,
                                               method='trf',
-                                              max_nfev=solution.max_nr_iter,
+                                              max_nfev=defs.max_nr_iter,
                                               args=args)
 
-        cls._condense_results(result, solution, matched_samples)
-        return solution
+        cls._condense_results(result, defs, matched_samples, solution)
 
     @classmethod
     def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample]) -> npt.NDArray:
@@ -191,7 +176,7 @@ def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample]) -> npt.N
         return np.array(result)
 
     @classmethod
-    def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], defs: LighthouseGeometrySolution
+    def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], defs: SolverData
                                        ) -> tuple[npt.NDArray, npt.NDArray, npt.NDArray, npt.NDArray]:
         """
         To speed up calculations all operations in the iteration phase are done on np.arrays of equal length (ish),
@@ -252,7 +237,7 @@ def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], d
 
     @classmethod
     def _populate_initial_guess(cls, initial_guess: LhBsCfPoses,
-                                defs: LighthouseGeometrySolution) -> tuple[npt.NDArray, npt.NDArray]:
+                                defs: SolverData) -> tuple[npt.NDArray, npt.NDArray]:
         """
         Generate parameters for base stations and CFs, this is the initial guess we start to iterate from.
         """
@@ -268,7 +253,7 @@ def _populate_initial_guess(cls, initial_guess: LhBsCfPoses,
         return params_bs, params_cfs
 
     @classmethod
-    def _params_to_struct(cls, params, defs: LighthouseGeometrySolution):
+    def _params_to_struct(cls, params, defs: SolverData):
         """
         Convert the params list to two arrays, one for base stations and one for CFs
         """
@@ -280,7 +265,7 @@ def _params_to_struct(cls, params, defs: LighthouseGeometrySolution):
         return params_bs_poses, params_cf_poses
 
     @classmethod
-    def _calc_residual(cls, params, defs: LighthouseGeometrySolution, index_angle_pair_to_bs, index_angle_pair_to_cf,
+    def _calc_residual(cls, params, defs: SolverData, index_angle_pair_to_bs, index_angle_pair_to_cf,
                        index_angle_pair_to_sensor_base, target_angles, sensor_positions):
         """
         Calculate the residual for a set of parameters. The residual is defined as the distance from a sensor to the
@@ -315,13 +300,13 @@ def _calc_residual(cls, params, defs: LighthouseGeometrySolution, index_angle_pa
 
     @classmethod
     def _poses_to_angle_pairs(cls, bss, cf_poses, sensor_base_pos, index_angle_pair_to_bs, index_angle_pair_to_cf,
-                              index_angle_pair_to_sensor_base, defs: LighthouseGeometrySolution):
+                              index_angle_pair_to_sensor_base, defs: SolverData):
         pairs = cls._calc_angle_pairs(bss[index_angle_pair_to_bs], cf_poses[index_angle_pair_to_cf],
                                       sensor_base_pos[index_angle_pair_to_sensor_base], defs)
         return pairs
 
     @classmethod
-    def _calc_angle_pairs(cls, bs_p_a, cf_p_a, sens_pos_p_a, defs: LighthouseGeometrySolution):
+    def _calc_angle_pairs(cls, bs_p_a, cf_p_a, sens_pos_p_a, defs: SolverData):
         """
         Calculate angle pairs based on base station poses, cf poses and sensor positions
 
@@ -366,7 +351,7 @@ def _pose_to_params(cls, pose: Pose) -> npt.NDArray:
         return np.concatenate((pose.rot_vec, pose.translation))
 
     @classmethod
-    def _params_to_pose(cls, params: npt.ArrayLike, defs: LighthouseGeometrySolution) -> Pose:
+    def _params_to_pose(cls, params: npt.ArrayLike, defs: SolverData) -> Pose:
         """
         Convert from the array format used in the solver to Pose
         """
@@ -394,34 +379,37 @@ def _create_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[i
         return bs_id_to_index, bs_index_to_id
 
     @classmethod
-    def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution,
-                          matched_samples: list[LhCfPoseSample]) -> None:
-        bss, cf_poses = cls._params_to_struct(lsq_result.x, solution)
+    def _condense_results(cls, lsq_result, defs: SolverData, matched_samples: list[LhCfPoseSample],
+                          solution: LighthouseGeometrySolution) -> None:
+        bss, cf_poses = cls._params_to_struct(lsq_result.x, defs)
 
         # Extract CF pose estimates
         # First pose (origin) is not in the parameter list
         solution.poses.cf_poses.append(Pose())
         for i in range(len(matched_samples) - 1):
-            solution.poses.cf_poses.append(cls._params_to_pose(cf_poses[i], solution))
+            solution.poses.cf_poses.append(cls._params_to_pose(cf_poses[i], defs))
 
         # Extract base station pose estimates
         for index, pose in enumerate(bss):
-            bs_id = solution.bs_index_to_id[index]
-            solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, solution)
+            bs_id = defs.bs_index_to_id[index]
+            solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, defs)
 
         solution.has_converged = lsq_result.success
 
         # Extract the error for each CF pose
         residuals = lsq_result.fun
         i = 0
+        # Estimated error for each base station in each sample
+        estimated_errors: list[dict[int, float]] = []
+
         for sample in matched_samples:
             sample_errors = {}
             for bs_id in sorted(sample.angles_calibrated.keys()):
                 sample_errors[bs_id] = np.linalg.norm(residuals[i:i + 2])
-                i += solution.n_sensors * 2
-            solution.estimated_errors.append(sample_errors)
+                i += defs.n_sensors * 2
+            estimated_errors.append(sample_errors)
 
-        solution.error_info = cls._aggregate_error_info(solution.estimated_errors)
+        solution.error_info = cls._aggregate_error_info(estimated_errors)
 
     @classmethod
     def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]) -> dict[str, float]:
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 134fea47b..5572b65c5 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -28,6 +28,7 @@
 
 from cflib.localization.lighthouse_cf_pose_sample import BsPairPoses
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.localization.lighthouse_types import LhException
 from cflib.localization.lighthouse_types import Pose
@@ -52,14 +53,17 @@ class LighthouseInitialEstimator:
     OUTLIER_DETECTION_ERROR = 0.5
 
     @classmethod
-    def estimate(cls, matched_samples: list[LhCfPoseSample]) -> tuple[LhBsCfPoses, list[LhCfPoseSample]]:
+    def estimate(cls, matched_samples: list[LhCfPoseSample],
+                 solution: LighthouseGeometrySolution) -> tuple[LhBsCfPoses, list[LhCfPoseSample]]:
         """
         Make a rough estimate of the poses of all base stations and CF poses found in the samples.
 
         The pose of the Crazyflie in the first sample is used as a reference and will define the
         global reference frame.
 
-        :param matched_samples: A list of samples with lighthouse angles.
+        :param matched_samples: A list of samples with lighthouse angles. It is assumed that all samples have data for
+                                two or more base stations.
+        :param solution: A LighthouseGeometrySolution object to store progress information and issues in
         :return: an estimate of base station and Crazyflie poses, as well as a cleaned version of matched_samples where
                  outliers are removed.
         """
@@ -68,9 +72,12 @@ def estimate(cls, matched_samples: list[LhCfPoseSample]) -> tuple[LhBsCfPoses, l
         # bs_positions is a map from bs-id-pair to position, where the position is the position of the second
         # bs, as seen from the first bs (in the first bs ref frame).
 
-        bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(matched_samples, bs_positions)
+        bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(matched_samples, bs_positions, solution)
+        if not solution.progress_is_ok:
+            return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
         # Calculate the pose of the base stations, based on the pose of one base station
+        # TODO krri _estimate_bs_poses() may raise an exception, handle
         bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
 
         # Now that we have estimated the base station poses, estimate the poses of the CF in all the samples
@@ -138,9 +145,8 @@ def _add_solution_permutations(cls, solutions: dict[int, BsPairPoses],
                                                    pose3.translation, pose4.translation])
 
     @classmethod
-    def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
-                         bs_positions: dict[BsPairIds, ArrayFloat]) -> tuple[list[dict[int, Pose]],
-                                                                             list[LhCfPoseSample]]:
+    def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], bs_positions: dict[BsPairIds, ArrayFloat],
+                         solution: LighthouseGeometrySolution) -> tuple[list[dict[int, Pose]], list[LhCfPoseSample]]:
         """
         Estimate the base station poses in the Crazyflie reference frames, for each sample.
 
@@ -149,8 +155,9 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
 
         :param matched_samples: List of samples
         :param bs_positions: Dictionary of base station positions (other base station ref frame)
+        :param solution: A LighthouseGeometrySolution object to store issues in
         :return: A list of dictionaries from base station to Pose of all base stations, for each sample, as well as
-                 a version of the matched_samples where outliers are removed
+                 a version of the matched_samples where outliers are removed.
         """
         result: list[dict[int, Pose]] = []
 
@@ -174,6 +181,10 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample],
                     poses[pair_ids.bs2] = pair_poses.bs2
                 else:
                     is_sample_valid = False
+                    if sample.is_mandatory:
+                        solution.append_mandatory_issue_sample(sample, 'Outlier detected')
+                    else:
+                        solution.xyz_space_samples_info = 'Sample(s) with outliers skipped'
                     break
 
             if is_sample_valid or sample.is_mandatory:
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 7b348c957..32a08e7dc 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -60,6 +60,7 @@
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoEstimationManager
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
 from cflib.localization.lighthouse_types import LhBsCfPoses
@@ -219,12 +220,20 @@ def load_from_file(name: str) -> LhGeoInputContainer:
         return pickle.load(handle)
 
 
-def solution_handler(scaled_solution: LhBsCfPoses):
+def solution_handler(solution: LighthouseGeometrySolution):
     print('Solution ready:')
     print('  Base stations at:')
-    print_base_stations_poses(scaled_solution.bs_poses)
-    # visualize(thread.scaled_solution)
-    # upload_geometry(thread.container.scf, thread.scaled_solution.bs_poses)
+    bs_poses = solution.poses.bs_poses
+    print_base_stations_poses(bs_poses)
+    print(f'Converged: {solution.has_converged}')
+    print(f'Progress info: {solution.progress_info}')
+    print(f'Progress is ok: {solution.progress_is_ok}')
+    print(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}')
+    print(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}')
+    print(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}')
+    print(f'XYZ space: {solution.xyz_space_samples_info}')
+    # visualize(bs_poses)
+    # upload_geometry(thread.container.scf, bs_poses)
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):
diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index ea2987ed1..cf4fef036 100644
--- a/test/localization/test_lighthouse_geometry_solver.py
+++ b/test/localization/test_lighthouse_geometry_solver.py
@@ -23,6 +23,7 @@
 from test.localization.lighthouse_test_base import LighthouseTestBase
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
@@ -31,6 +32,7 @@
 class TestLighthouseGeometrySolver(LighthouseTestBase):
     def setUp(self):
         self.fixtures = LighthouseFixtures()
+        self.solution = LighthouseGeometrySolution()
 
     def test_that_two_bs_poses_in_one_sample_are_estimated(self):
         # Fixture
@@ -46,14 +48,14 @@ def test_that_two_bs_poses_in_one_sample_are_estimated(self):
         for sample in matched_samples:
             sample.augment_with_ippe(LhDeck4SensorPositions.positions)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
+        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, self.solution)
 
         # Test
-        actual = LighthouseGeometrySolver.solve(
-            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions)
+        LighthouseGeometrySolver.solve(
+            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions, self.solution)
 
         # Assert
-        bs_poses = actual.poses.bs_poses
+        bs_poses = self.solution.poses.bs_poses
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3)
 
@@ -81,14 +83,14 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self):
         for sample in matched_samples:
             sample.augment_with_ippe(LhDeck4SensorPositions.positions)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples)
+        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, self.solution)
 
         # Test
-        actual = LighthouseGeometrySolver.solve(
-            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions)
+        LighthouseGeometrySolver.solve(
+            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions, self.solution)
 
         # Assert
-        bs_poses = actual.poses.bs_poses
+        bs_poses = self.solution.poses.bs_poses
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, bs_poses[bs_id0], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, bs_poses[bs_id1], places=3)
         self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, bs_poses[bs_id2], places=3)
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index 8d25a17a4..94f1afce8 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -26,6 +26,7 @@
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_cf_pose_sample import Pose
+from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhException
@@ -34,6 +35,7 @@
 class TestLighthouseInitialEstimator(LighthouseTestBase):
     def setUp(self):
         self.fixtures = LighthouseFixtures()
+        self.solution = LighthouseGeometrySolution()
 
     def test_that_one_bs_pose_raises_exception(self):
         # Fixture
@@ -47,7 +49,7 @@ def test_that_one_bs_pose_raises_exception(self):
         # Test
         # Assert
         with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples)
+            LighthouseInitialEstimator.estimate(samples, self.solution)
 
     def test_that_two_bs_poses_in_same_sample_are_found(self):
         # Fixture
@@ -63,7 +65,7 @@ def test_that_two_bs_poses_in_same_sample_are_found(self):
         self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
@@ -93,7 +95,7 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self):
         self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
@@ -125,7 +127,7 @@ def test_that_cf_poses_are_estimated(self):
         self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
 
         # Assert
         self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, actual.cf_poses[0], places=3)
@@ -151,7 +153,7 @@ def test_that_the_global_ref_frame_is_used(self):
         self.augment(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples)
+        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
 
         # Assert
         self.assertPosesAlmostEqual(
@@ -182,7 +184,7 @@ def test_that_raises_for_isolated_bs(self):
         # Test
         # Assert
         with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples)
+            LighthouseInitialEstimator.estimate(samples, self.solution)
 
 # helpers
 

From 2b813e18244d5331c19fdb90556637416b20aa2d Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sat, 14 Jun 2025 08:22:20 +0200
Subject: [PATCH 17/36] Fix problem in sweep angle reader

---
 .../localization/lighthouse_sweep_angle_reader.py | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py
index 5de9ff6e4..59acf8157 100644
--- a/cflib/localization/lighthouse_sweep_angle_reader.py
+++ b/cflib/localization/lighthouse_sweep_angle_reader.py
@@ -107,13 +107,14 @@ def is_collecting(self):
         return self._sample_storage is not None
 
     def _data_recevied_cb(self, base_station_id: int, bs_vectors: list[LighthouseBsVector]):
-        self._store_sample(base_station_id, bs_vectors, self._sample_storage)
-        if self._has_collected_enough_data(self._sample_storage):
-            self._reader.stop()
-            if self._ready_cb:
-                averages = self._average_all_lists(self._sample_storage)
-                self._ready_cb(averages)
-            self._sample_storage = None
+        if self._sample_storage is not None:
+            self._store_sample(base_station_id, bs_vectors, self._sample_storage)
+            if self._has_collected_enough_data(self._sample_storage):
+                self._reader.stop()
+                if self._ready_cb:
+                    averages = self._average_all_lists(self._sample_storage)
+                    self._ready_cb(averages)
+                self._sample_storage = None
 
     def _store_sample(self, base_station_id: int, bs_vectors: list[LighthouseBsVector],
                       storage: dict[int, list[list[LighthouseBsVector]]]):

From da212626e8ec5c523fca5c4324fdc9d8849c6e44 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 16 Jun 2025 10:43:50 +0200
Subject: [PATCH 18/36] Added  base station link map

---
 .../lighthouse_geo_estimation_manager.py      |  2 -
 .../lighthouse_geometry_solution.py           | 11 ++-
 .../lighthouse_initial_estimator.py           | 67 +++++++++++++++----
 .../test_lighthouse_initial_estimator.py      | 15 +++--
 4 files changed, 71 insertions(+), 24 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 174761d44..9518b41ce 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -94,8 +94,6 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
         cls._humanize_error_info(solution, container)
         # TODO krri indicate in the solution if there is a geometry
 
-        # TODO krri create linkage map
-
         return solution
 
     @classmethod
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index 83a0399bc..a37a66585 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -1,4 +1,3 @@
-
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhBsCfPoses
 
@@ -21,7 +20,7 @@ def __init__(self):
         self.has_converged = False
 
         # Progress information stating how far in the solution process we got
-        self.progress_info = ""
+        self.progress_info = ''
 
         # Indicates that all previous steps in the solution process were successful and that the next step
         # can be executed. This is used to determine if the solution process can continue.
@@ -41,6 +40,14 @@ def __init__(self):
         # and a description of the issue. This list is used to extract issue descriptions for the user interface.
         self.mandatory_issue_samples: list[tuple[LhCfPoseSample, str]] = []
 
+        # General failure information if the problem is not related to a specific sample
+        self.general_failure_info = ''
+
+        # The number of links between base stations. The data is organized as a dictionary with base station ids as
+        # keys, mapped to a dictionary of base station ids and the number of links to other base stations.
+        # For example: link_count[1][2] = 3 means that base station 1 has 3 links to base station 2.
+        self.link_count: dict[int, dict[int, int]] = {}
+
     def append_mandatory_issue_sample(self, sample: LhCfPoseSample, issue: str):
         """
         Append a sample with an issue to the list of mandatory issue samples.
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 5572b65c5..eb772e351 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -76,15 +76,51 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
         if not solution.progress_is_ok:
             return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
-        # Calculate the pose of the base stations, based on the pose of one base station
-        # TODO krri _estimate_bs_poses() may raise an exception, handle
-        bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
+        cls._build_link_stats(matched_samples, solution)
+        # TODO krri: This step should check that we have enough links between base stations and fail with good
+        # user information.
+        # We could also filter out base stations that are not linked instead of failing the solution (in
+        # _estimate_bs_poses()).
+        if not solution.progress_is_ok:
+            return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
+
+        # Calculate the pose of all base stations, based on the pose of one base station
+        try:
+            bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
+        except LhException as e:
+            # At this point we might have too few base stations or we have islands of non-linked base stations.
+            solution.progress_is_ok = False
+            solution.general_failure_info = str(e)
+            return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
         # Now that we have estimated the base station poses, estimate the poses of the CF in all the samples
         cf_poses = cls._estimate_cf_poses(bs_poses_ref_cfs, bs_poses)
 
         return LhBsCfPoses(bs_poses, cf_poses), cleaned_matched_samples
 
+    @classmethod
+    def _build_link_stats(cls, matched_samples: list[LhCfPoseSample], solution: LighthouseGeometrySolution) -> None:
+        """
+        Build statistics about the number of links between base stations, based on the matched samples.
+        :param matched_samples: List of matched samples
+        :param solution: A LighthouseGeometry Solution object to store issues in
+        """
+
+        def increase_link_count(bs1: int, bs2: int):
+            """Increase the link count between two base stations"""
+            if bs1 not in solution.link_count:
+                solution.link_count[bs1] = {}
+            if bs2 not in solution.link_count[bs1]:
+                solution.link_count[bs1][bs2] = 0
+            solution.link_count[bs1][bs2] += 1
+
+        for sample in matched_samples:
+            bs_in_sample = sample.angles_calibrated.keys()
+            for bs1 in bs_in_sample:
+                for bs2 in bs_in_sample:
+                    if bs1 != bs2:
+                        increase_link_count(bs1, bs2)
+
     @classmethod
     def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample]) -> dict[BsPairIds, ArrayFloat]:
         """
@@ -281,7 +317,7 @@ def _estimate_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]]) -> dict[int
         that the pose of 3.
         """
         # Use the first CF pose as the global reference frame. The pose of the first base station (as estimated by ippe)
-        # is used as the "true" position (reference)
+        # is used as the pose that all other base stations are mapped to.
         reference_bs_pose = None
         for bs_pose_ref_cfs in bs_poses_ref_cfs:
             if len(bs_pose_ref_cfs) > 0:
@@ -301,9 +337,11 @@ def _estimate_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]]) -> dict[int
         to_find = all_bs - bs_poses.keys()
 
         # run through the list of samples until we manage to find them all
-        remaining = len(to_find)
-        while remaining > 0:
-            buckets: dict[int, list[Pose]] = {}
+        # The process is like peeling an onion, from the inside out. In each iteration we find the poses of
+        # the base stations that are closest to the ones we already have, until we have found all poses.
+        remaining_to_find = len(to_find)
+        while remaining_to_find > 0:
+            averaging_storage: dict[int, list[Pose]] = {}
             for bs_poses_in_sample in bs_poses_ref_cfs:
                 unknown = to_find.intersection(bs_poses_in_sample.keys())
                 known = set(bs_poses.keys()).intersection(bs_poses_in_sample.keys())
@@ -322,22 +360,25 @@ def _estimate_bs_poses(cls, bs_poses_ref_cfs: list[dict[int, Pose]]) -> dict[int
                         unknown_cf = bs_poses_in_sample[bs_id]
                         # Finally we can calculate the BS pose in the global reference frame
                         bs_pose = cls._map_pose_to_ref_frame(known_global, known_cf, unknown_cf)
-                        if bs_id not in buckets:
-                            buckets[bs_id] = []
-                        buckets[bs_id].append(bs_pose)
+                        if bs_id not in averaging_storage:
+                            averaging_storage[bs_id] = []
+                        averaging_storage[bs_id].append(bs_pose)
 
             # Average over poses and add to bs_poses
-            for bs_id, poses in buckets.items():
+            for bs_id, poses in averaging_storage.items():
                 bs_poses[bs_id] = cls._average_poses(poses)
 
+            # Remove the newly found base stations from the set of base stations to find
             to_find = all_bs - bs_poses.keys()
             if len(to_find) == 0:
                 break
 
-            if len(to_find) == remaining:
+            if len(to_find) == remaining_to_find:
+                # We could not map any more poses, but some still remain to be found. This means that there are not
+                # links to all base stations.
                 raise LhException('Can not link positions between all base stations')
 
-            remaining = len(to_find)
+            remaining_to_find = len(to_find)
 
         return bs_poses
 
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index 94f1afce8..f77415f2b 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -29,7 +29,6 @@
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
-from cflib.localization.lighthouse_types import LhException
 
 
 class TestLighthouseInitialEstimator(LighthouseTestBase):
@@ -37,7 +36,7 @@ def setUp(self):
         self.fixtures = LighthouseFixtures()
         self.solution = LighthouseGeometrySolution()
 
-    def test_that_one_bs_pose_raises_exception(self):
+    def test_that_one_bs_pose_failes_solution(self):
         # Fixture
         # CF_ORIGIN is used in the first sample and will define the global reference frame
         bs_id = 3
@@ -47,9 +46,10 @@ def test_that_one_bs_pose_raises_exception(self):
         self.augment(samples)
 
         # Test
+        LighthouseInitialEstimator.estimate(samples, self.solution)
+
         # Assert
-        with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples, self.solution)
+        assert self.solution.progress_is_ok is False
 
     def test_that_two_bs_poses_in_same_sample_are_found(self):
         # Fixture
@@ -163,7 +163,7 @@ def test_that_the_global_ref_frame_is_used(self):
         self.assertPosesAlmostEqual(
             Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 1.0, 3.0)), actual.bs_poses[bs_id2], places=3)
 
-    def test_that_raises_for_isolated_bs(self):
+    def test_that_solution_failes_for_isolated_bs(self):
         # Fixture
         bs_id0 = 3
         bs_id1 = 1
@@ -182,9 +182,10 @@ def test_that_raises_for_isolated_bs(self):
         self.augment(samples)
 
         # Test
+        LighthouseInitialEstimator.estimate(samples, self.solution)
+
         # Assert
-        with self.assertRaises(LhException):
-            LighthouseInitialEstimator.estimate(samples, self.solution)
+        assert self.solution.progress_is_ok is False
 
 # helpers
 

From 9094f92bf4b99d43a4d4706a2ecbc7f40246e59f Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 16 Jun 2025 11:02:59 +0200
Subject: [PATCH 19/36] Added unit test

---
 .../test_lighthouse_initial_estimator.py      | 35 +++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index f77415f2b..cff39835f 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -187,6 +187,41 @@ def test_that_solution_failes_for_isolated_bs(self):
         # Assert
         assert self.solution.progress_is_ok is False
 
+    def test_that_link_count_is_right(self):
+        # Fixture
+        bs_id0 = 3
+        bs_id1 = 1
+        bs_id2 = 2
+        bs_id3 = 4
+        samples = [
+            LhCfPoseSample(angles_calibrated={
+                bs_id0: self.fixtures.angles_cf_origin_bs0,
+                bs_id1: self.fixtures.angles_cf_origin_bs1,
+            }),
+            LhCfPoseSample(angles_calibrated={
+                bs_id2: self.fixtures.angles_cf1_bs1,
+                bs_id3: self.fixtures.angles_cf1_bs2,
+            }),
+            LhCfPoseSample(angles_calibrated={
+                bs_id0: self.fixtures.angles_cf2_bs0,
+                bs_id1: self.fixtures.angles_cf2_bs1,
+                bs_id2: self.fixtures.angles_cf2_bs2,
+                bs_id3: self.fixtures.angles_cf2_bs3,
+            }),
+        ]
+        self.augment(samples)
+
+        # Test
+        LighthouseInitialEstimator.estimate(samples, self.solution)
+
+        # Assert
+        assert self.solution.link_count == {
+            bs_id0: {bs_id1: 2, bs_id2: 1, bs_id3: 1},
+            bs_id1: {bs_id0: 2, bs_id2: 1, bs_id3: 1},
+            bs_id2: {bs_id0: 1, bs_id1: 1, bs_id3: 2},
+            bs_id3: {bs_id0: 1, bs_id1: 1, bs_id2: 2},
+        }
+
 # helpers
 
     def augment(self, samples):

From eb6c6231d2bfd140d4c8bcb4326ef36a8e6175e4 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 17 Jun 2025 13:56:34 +0200
Subject: [PATCH 20/36] Added matched lighthouse sample stream

---
 cflib/crazyflie/localization.py               | 24 ++++++
 cflib/localization/__init__.py                |  2 +
 .../lighthouse_sweep_angle_reader.py          | 84 +++++++++++++++++++
 3 files changed, 110 insertions(+)

diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py
index 1d50d81d9..0d609789d 100644
--- a/cflib/crazyflie/localization.py
+++ b/cflib/crazyflie/localization.py
@@ -66,6 +66,7 @@ class Localization():
     EXT_POSE_PACKED = 9
     LH_ANGLE_STREAM = 10
     LH_PERSIST_DATA = 11
+    LH_MATCHED_ANGLE_STREAM = 12
 
     def __init__(self, crazyflie=None):
         """
@@ -105,6 +106,8 @@ def _incoming(self, packet):
             decoded_data = bool(data[0])
         elif pk_type == self.LH_ANGLE_STREAM:
             decoded_data = self._decode_lh_angle(data)
+        elif pk_type == self.LH_MATCHED_ANGLE_STREAM:
+            decoded_data = self._decode_matched_lh_angle(data)
 
         pk = LocalizationPacket(pk_type, data, decoded_data)
         self.receivedLocationPacket.call(pk)
@@ -128,6 +131,27 @@ def _decode_lh_angle(self, data):
 
         return decoded_data
 
+    def _decode_matched_lh_angle(self, data):
+        decoded_data = {}
+
+        raw_data = struct.unpack('<BfhhhfhhhB', data)
+
+        decoded_data['basestation'] = raw_data[0]
+        decoded_data['x'] = [0, 0, 0, 0]
+        decoded_data['x'][0] = raw_data[1]
+        decoded_data['x'][1] = raw_data[1] - fp16_to_float(raw_data[2])
+        decoded_data['x'][2] = raw_data[1] - fp16_to_float(raw_data[3])
+        decoded_data['x'][3] = raw_data[1] - fp16_to_float(raw_data[4])
+        decoded_data['y'] = [0, 0, 0, 0]
+        decoded_data['y'][0] = raw_data[5]
+        decoded_data['y'][1] = raw_data[5] - fp16_to_float(raw_data[6])
+        decoded_data['y'][2] = raw_data[5] - fp16_to_float(raw_data[7])
+        decoded_data['y'][3] = raw_data[5] - fp16_to_float(raw_data[8])
+        decoded_data['group_id'] = raw_data[9] >> 4
+        decoded_data['bs_count'] = raw_data[9] & 0x0F
+
+        return decoded_data
+
     def send_extpos(self, pos):
         """
         Send the current Crazyflie X, Y, Z position. This is going to be
diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py
index 6f4252c3e..d48fde667 100644
--- a/cflib/localization/__init__.py
+++ b/cflib/localization/__init__.py
@@ -25,6 +25,7 @@
 from .lighthouse_config_manager import LighthouseConfigWriter
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
+from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .param_io import ParamFileManager
 
 __all__ = [
@@ -32,6 +33,7 @@
     'LighthouseBsVector',
     'LighthouseSweepAngleAverageReader',
     'LighthouseSweepAngleReader',
+    'LighthouseMatchedSweepAngleReader',
     'LighthouseConfigFileManager',
     'LighthouseConfigWriter',
     'ParamFileManager']
diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py
index 59acf8157..824a8448f 100644
--- a/cflib/localization/lighthouse_sweep_angle_reader.py
+++ b/cflib/localization/lighthouse_sweep_angle_reader.py
@@ -24,6 +24,7 @@
 from cflib.crazyflie import Crazyflie
 from cflib.localization import LighthouseBsVector
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 
 
 class LighthouseSweepAngleReader():
@@ -161,3 +162,86 @@ def _average_sample_list(self, sample_list: list[LighthouseBsVector]) -> Lightho
 
         count = len(sample_list)
         return LighthouseBsVector(sum_horiz / count, sum_vert / count)
+
+
+class LighthouseMatchedSweepAngleReader():
+    """
+    Wrapper to simplify reading of matched lighthouse sweep angles from the locSrv stream
+    """
+    MATCHED_STREAM_PARAM = 'locSrv.enLhMtchStm'
+    MATCHED_STREAM_MIN_BS_PARAM = 'locSrv.minBsLhMtchStm'
+    MATCHED_STREAM_MAX_TIME_PARAM = 'locSrv.maxTimeLhMtchStm'
+    NR_OF_SENSORS = 4
+
+    def __init__(self, cf: Crazyflie, data_recevied_cb, sample_count: int = 1, min_bs: int = 2, max_time_ms: int = 25):
+        self._cf = cf
+        self._cb = data_recevied_cb
+        self._is_active = False
+        self._sample_count = sample_count
+
+        # The maximum number of base stations is limited in the CF due to memory considerations.
+        if min_bs > 4:
+            raise ValueError('Minimum base station count must be 4 or less')
+        self._min_bs = min_bs
+
+        self._max_time_ms = max_time_ms
+
+        self._current_group_id = 0
+        self._angles: dict[int, LighthouseBsVectors] = {}
+
+    def start(self):
+        """Start reading sweep angles"""
+        self._cf.loc.receivedLocationPacket.add_callback(self._packet_received_cb)
+        self._is_active = True
+        self._angle_stream_activate(True)
+
+    def stop(self):
+        """Stop reading sweep angles"""
+        if self._is_active:
+            self._is_active = False
+            self._cf.loc.receivedLocationPacket.remove_callback(self._packet_received_cb)
+            self._angle_stream_activate(False)
+
+    def _angle_stream_activate(self, is_active: bool):
+        value = 0
+        if is_active:
+            value = self._sample_count
+        self._cf.param.set_value(self.MATCHED_STREAM_PARAM, value)
+
+        self._cf.param.set_value(self.MATCHED_STREAM_MIN_BS_PARAM, self._min_bs)
+        self._cf.param.set_value(self.MATCHED_STREAM_MAX_TIME_PARAM, self._max_time_ms)
+
+    def _packet_received_cb(self, packet):
+        if self._is_active:
+            if packet.type != self._cf.loc.LH_MATCHED_ANGLE_STREAM:
+                return
+
+            base_station_id: int = packet.data['basestation']
+            horiz_angles: float = packet.data['x']
+            vert_angles: float = packet.data['y']
+            group_id: int = packet.data['group_id']
+            bs_count: int = packet.data['bs_count']
+
+            if group_id != self._current_group_id:
+                if len(self._angles) >= self._min_bs:
+                    # We have enough angles in the previous group even though all angles were not received
+                    # Lost a packet?
+                    self._call_callback()
+
+                # Reset
+                self._current_group_id = group_id
+                self._angles = {}
+
+            vectors: list[LighthouseBsVector] = []
+            for i in range(self.NR_OF_SENSORS):
+                vectors.append(LighthouseBsVector(horiz_angles[i], vert_angles[i]))
+            self._angles[base_station_id] = LighthouseBsVectors(vectors)
+
+            if len(self._angles) == bs_count:
+                # We have received all angles for this group, call the callback
+                self._call_callback()
+
+    def _call_callback(self):
+        if self._cb:
+            self._cb(LhCfPoseSample(self._angles))
+            self._angles = {}

From 088c691d1f235f13ca0b7a51ffc443a97e722e25 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 18 Jun 2025 14:07:08 +0200
Subject: [PATCH 21/36] Added user action detector

---
 cflib/localization/__init__.py                |   2 +-
 .../lighthouse_geo_estimation_manager.py      |  13 +-
 cflib/localization/user_action_detector.py    | 123 ++++++++++++++++++
 .../multi_bs_geometry_estimation.py           |  45 ++++---
 4 files changed, 155 insertions(+), 28 deletions(-)
 create mode 100644 cflib/localization/user_action_detector.py

diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py
index d48fde667..9d4ee81db 100644
--- a/cflib/localization/__init__.py
+++ b/cflib/localization/__init__.py
@@ -23,9 +23,9 @@
 from .lighthouse_bs_vector import LighthouseBsVector
 from .lighthouse_config_manager import LighthouseConfigFileManager
 from .lighthouse_config_manager import LighthouseConfigWriter
+from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
-from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .param_io import ParamFileManager
 
 __all__ = [
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 9518b41ce..b9cd943ad 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -31,11 +31,9 @@
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
-from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
 from cflib.localization.lighthouse_types import LhBsCfPoses
-from cflib.localization.lighthouse_types import LhMeasurement
 
 
 ArrayFloat = npt.NDArray[np.float_]
@@ -297,23 +295,22 @@ def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
         self._data.xy_plane.append(xy_plane)
         self._update_version()
 
-    def set_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
+    def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """Store/update the samples for the volume
 
         Args:
             samples (list[LhMeasurement]): the new samples
         """
-        self._data.xyz_space = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
-        self._augment_samples(self._data.xyz_space, False)
-        self._update_version()
+        self._data.xyz_space = []
+        self.append_xyz_space_samples(samples)
 
-    def append_xyz_space_samples(self, samples: list[LhMeasurement]) -> None:
+    def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """Append to the samples for the volume
 
         Args:
             samples (LhMeasurement): the new samples
         """
-        new_samples = LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2)
+        new_samples = samples
         self._augment_samples(new_samples, False)
         self._data.xyz_space += new_samples
         self._update_version()
diff --git a/cflib/localization/user_action_detector.py b/cflib/localization/user_action_detector.py
new file mode 100644
index 000000000..f8a089371
--- /dev/null
+++ b/cflib/localization/user_action_detector.py
@@ -0,0 +1,123 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2025 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+"""
+Functionality to get user input by shaking the Crazyflie.
+"""
+import time
+
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+
+class UserActionDetector:
+    """ This class is used as an user interface that lets the user trigger an event by using the Crazyflie as the
+    input device. The class listens to the z component of the gyro and detects a quick left or right rotation followed
+    by period of no motion. If such a sequence is detected, it calls the callback function provided in the constructor.
+    """
+
+    def __init__(self, cf: Crazyflie, cb=None):
+        self._is_active = False
+        self._reset()
+        self._cf = cf
+        self._cb = cb
+        self._lg_config = None
+
+        self.left_event_threshold_time = 0.0
+        self.left_event_time = 0.0
+        self.right_event_threshold_time = 0.0
+        self.right_event_time = 0
+        self.still_event_threshold_time = 0.0
+        self.still_event_time = 0.0
+
+    def start(self):
+        if not self._is_active:
+            self._is_active = True
+            self._reset()
+            self._cf.disconnected.add_callback(self.stop)
+
+            self._lg_config = LogConfig(name='lighthouse_geo_estimator', period_in_ms=25)
+            self._lg_config.add_variable('gyro.z', 'float')
+            self._cf.log.add_config(self._lg_config)
+            self._lg_config.data_received_cb.add_callback(self._log_callback)
+            self._lg_config.start()
+
+    def stop(self):
+        if self._is_active:
+            if self._lg_config is not None:
+                self._lg_config.stop()
+                self._lg_config.delete()
+                self._lg_config.data_received_cb.remove_callback(self._log_callback)
+                self._lg_config = None
+            self._cf.disconnected.remove_callback(self.stop)
+            self._is_active = False
+
+    def _log_callback(self, ts, data, logblock):
+        if self._is_active:
+            gyro_z = data['gyro.z']
+            self.process_rot(gyro_z)
+
+    def _reset(self):
+        self.left_event_threshold_time = 0.0
+        self.left_event_time = 0.0
+
+        self.right_event_threshold_time = 0.0
+        self.right_event_time = 0
+
+        self.still_event_threshold_time = 0.0
+        self.still_event_time = 0.0
+
+    def process_rot(self, gyro_z):
+        now = time.time()
+
+        MAX_DURATION_OF_EVENT_PEEK = 0.1
+        MIN_DURATION_OF_STILL_EVENT = 0.5
+        MAX_TIME_BETWEEN_LEFT_RIGHT_EVENTS = 0.3
+        MAX_TIME_BETWEEN_FIRST_ROTATION_AND_STILL_EVENT = 1.0
+
+        if gyro_z > 0:
+            self.left_event_threshold_time = now
+        if gyro_z < -300 and now - self.left_event_threshold_time < MAX_DURATION_OF_EVENT_PEEK:
+            self.left_event_time = now
+
+        if gyro_z < 0:
+            self.right_event_threshold_time = now
+        if gyro_z > 300 and now - self.right_event_threshold_time < MAX_DURATION_OF_EVENT_PEEK:
+            self.right_event_time = now
+
+        if abs(gyro_z) > 50:
+            self.still_event_threshold_time = now
+        if abs(gyro_z) < 30 and now - self.still_event_threshold_time > MIN_DURATION_OF_STILL_EVENT:
+            self.still_event_time = now
+
+        dt_left_right = self.left_event_time - self.right_event_time
+        first_left_right = min(self.left_event_time, self.right_event_time)
+        dt_first_still = self.still_event_time - first_left_right
+
+        if self.left_event_time > 0 and self.right_event_time > 0 and self.still_event_time > 0:
+            if (abs(dt_left_right) < MAX_TIME_BETWEEN_LEFT_RIGHT_EVENTS and
+                dt_first_still > 0 and
+                    dt_first_still < MAX_TIME_BETWEEN_FIRST_ROTATION_AND_STILL_EVENT):
+                self._reset()
+                if self._cb is not None:
+                    self._cb()
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 32a08e7dc..b2c143025 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -61,11 +61,13 @@
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoEstimationManager
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
+from cflib.localization.lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
 from cflib.localization.lighthouse_types import LhBsCfPoses
 from cflib.localization.lighthouse_types import LhDeck4SensorPositions
 from cflib.localization.lighthouse_types import LhMeasurement
+from cflib.localization.user_action_detector import UserActionDetector
 from cflib.utils import uri_helper
 
 REFERENCE_DIST = 1.0
@@ -221,10 +223,11 @@ def load_from_file(name: str) -> LhGeoInputContainer:
 
 
 def solution_handler(solution: LighthouseGeometrySolution):
-    print('Solution ready:')
+    print('Solution ready --------------------------------------')
     print('  Base stations at:')
     bs_poses = solution.poses.bs_poses
     print_base_stations_poses(bs_poses)
+
     print(f'Converged: {solution.has_converged}')
     print(f'Progress info: {solution.progress_info}')
     print(f'Progress is ok: {solution.progress_is_ok}')
@@ -232,8 +235,7 @@ def solution_handler(solution: LighthouseGeometrySolution):
     print(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}')
     print(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}')
     print(f'XYZ space: {solution.xyz_space_samples_info}')
-    # visualize(bs_poses)
-    # upload_geometry(thread.container.scf, bs_poses)
+    print(f'General info: {solution.general_failure_info}')
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):
@@ -313,6 +315,13 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
     with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
         container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
         print('Starting geometry estimation thread...')
+
+        def _local_solution_handler(solution: LighthouseGeometrySolution):
+            solution_handler(solution)
+            if solution.progress_is_ok:
+                upload_geometry(scf, solution.poses.bs_poses)
+                print('Geometry uploaded to Crazyflie.')
+
         thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=solution_handler)
         thread.start()
 
@@ -334,26 +343,24 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
 
         print()
         print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' +
-              'geometry based on this data.')
-        recording_time_s = 1.0
-        first_attempt = True
+              'geometry based on this data. Sample a position by quickly rotating the Crazyflie ' +
+              'around the Z-axis. This will trigger a measurement of the base station angles. ')
 
-        while True:
-            if first_attempt:
-                user_input = input('Press return to record a measurement: ').lower()
-                first_attempt = False
-            else:
-                user_input = input('Press return to record another measurement, or "q" to continue: ').lower()
+        def matched_angles_cb(sample: LhCfPoseSample):
+            print('Sampled position')
+            container.append_xyz_space_samples([sample])
+        angle_reader = LighthouseMatchedSweepAngleReader(scf.cf, matched_angles_cb)
 
-            if user_input == 'q':
-                break
+        def user_action_cb():
+            angle_reader.start()
+        detector = UserActionDetector(scf.cf, cb=user_action_cb)
 
-            measurement = record_angles_sequence(scf, recording_time_s)
-            if measurement is not None:
-                container.append_xyz_space_samples(measurement)
-            else:
-                print('No data recorded, please try again.')
+        detector.start()
+        while True:
+            time.sleep(0.5)
+            # TODO krri
 
+        detector.stop()
         thread.stop()
 
         # if file_name:

From 6cb0c2721e95d2def3b88fefdce275845e06d7f8 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 23 Jun 2025 10:24:22 +0200
Subject: [PATCH 22/36] Make thread daemon

---
 cflib/localization/lighthouse_geo_estimation_manager.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index b9cd943ad..f4c03dfc4 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -185,6 +185,7 @@ def __init__(self, container: LhGeoInputContainer, is_done_cb) -> None:
                 is_done_cb: Callback function that is called when the solution is done.
             """
             threading.Thread.__init__(self, name='LhGeoEstimationManager.SolverThread')
+            self.daemon = True
 
             self.container = container
             self.latest_solved_data_version = container._data.version

From 08ba567492821e0bf1b18df64279aa2cffedf17b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 23 Jun 2025 11:33:52 +0200
Subject: [PATCH 23/36] Added missing sample from the origin

---
 cflib/localization/lighthouse_geo_estimation_manager.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index f4c03dfc4..bdc64fe74 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -46,7 +46,7 @@ class LhGeoEstimationManager():
     def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhBsCfPoses,
                                  reference_distance: float) -> LhBsCfPoses:
         start_idx_x_axis = 1
-        start_idx_xy_plane = 1 + len(container.x_axis)
+        start_idx_xy_plane = start_idx_x_axis + len(container.x_axis)
         start_idx_xyz_space = start_idx_xy_plane + len(container.xy_plane)
 
         origin_pos = poses.cf_poses[0].translation
@@ -128,6 +128,7 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSample], container: LhGe
         # Samples must contain at least two base stations
         for sample in matched_samples:
             if sample == container.origin:
+                result.append(sample)
                 continue  # The origin sample is already checked
 
             if len(sample.angles_calibrated) >= 2:

From 482542c599fec7104d1c4673a7284dc608ae3286 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 23 Jun 2025 11:36:11 +0200
Subject: [PATCH 24/36] Restored save/load functionality

---
 .../multi_bs_geometry_estimation.py           | 34 +++++++++----------
 1 file changed, 17 insertions(+), 17 deletions(-)

diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index b2c143025..f93ee48d9 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -60,6 +60,7 @@
 from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoEstimationManager
 from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainer
+from cflib.localization.lighthouse_geo_estimation_manager import LhGeoInputContainerData
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
@@ -211,18 +212,19 @@ def visualize(poses: LhBsCfPoses):
         plt.show()
 
 
-def write_to_file(name: str, container: LhGeoInputContainer):
-    with open(name, 'wb') as handle:
-        data = container
-        pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL)
+def write_to_file(name: str | None, container: LhGeoInputContainer):
+    if name:
+        with open(name, 'wb') as handle:
+            data = container.get_data_copy()
+            pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL)
 
 
-def load_from_file(name: str) -> LhGeoInputContainer:
+def load_from_file(name: str) -> LhGeoInputContainerData:
     with open(name, 'rb') as handle:
         return pickle.load(handle)
 
 
-def solution_handler(solution: LighthouseGeometrySolution):
+def print_solution(solution: LighthouseGeometrySolution):
     print('Solution ready --------------------------------------')
     print('  Base stations at:')
     bs_poses = solution.poses.bs_poses
@@ -259,11 +261,9 @@ def data_written(_):
 
 
 def estimate_from_file(file_name: str):
-    container = load_from_file(file_name)
-    thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=solution_handler)
-    thread.start()
-    time.sleep(1)
-    thread.stop()
+    container_data = load_from_file(file_name)
+    solution = LhGeoEstimationManager.estimate_geometry(container_data)
+    print_solution(solution)
 
 
 def get_recording(scf: SyncCrazyflie) -> LhCfPoseSample:
@@ -317,12 +317,12 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
         print('Starting geometry estimation thread...')
 
         def _local_solution_handler(solution: LighthouseGeometrySolution):
-            solution_handler(solution)
+            print_solution(solution)
             if solution.progress_is_ok:
                 upload_geometry(scf, solution.poses.bs_poses)
                 print('Geometry uploaded to Crazyflie.')
 
-        thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=solution_handler)
+        thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=print_solution)
         thread.start()
 
         print('  Connected')
@@ -332,14 +332,17 @@ def _local_solution_handler(solution: LighthouseGeometrySolution):
         print('Step 2. Put the Crazyflie where you want the origin of your coordinate system.')
 
         container.set_origin_sample(get_recording(scf))
+        write_to_file(file_name, container)
 
         print(f'Step 3. Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the origin. ' +
               'This position defines the direction of the X-axis, but it is also used for scaling the system.')
         container.set_x_axis_sample(get_recording(scf))
+        write_to_file(file_name, container)
 
         print('Step 4. Put the Crazyflie somewhere in the XY-plane, but not on the X-axis.')
         print('Multiple samples can be recorded if you want to.')
         container.set_xy_plane_samples(get_multiple_recordings(scf))
+        write_to_file(file_name, container)
 
         print()
         print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' +
@@ -349,6 +352,7 @@ def _local_solution_handler(solution: LighthouseGeometrySolution):
         def matched_angles_cb(sample: LhCfPoseSample):
             print('Sampled position')
             container.append_xyz_space_samples([sample])
+            write_to_file(file_name, container)
         angle_reader = LighthouseMatchedSweepAngleReader(scf.cf, matched_angles_cb)
 
         def user_action_cb():
@@ -363,10 +367,6 @@ def user_action_cb():
         detector.stop()
         thread.stop()
 
-        # if file_name:
-        #     write_to_file(file_name, container)
-        #     print(f'Wrote data to file {file_name}')
-
 
 # Only output errors from the logging framework
 logging.basicConfig(level=logging.ERROR)

From b28584b99b98bdeee2fdeea9e6e4c2a20f3d8168 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 24 Jun 2025 16:38:22 +0200
Subject: [PATCH 25/36] Join optional in solver thread

---
 cflib/localization/lighthouse_geo_estimation_manager.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index bdc64fe74..38641ef00 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -221,11 +221,13 @@ def run(self):
 
             self.is_running = False
 
-        def stop(self):
+        def stop(self, do_join: bool = True):
             """Stop the solver thread"""
             self.time_to_stop = True
-            if self.is_running:
-                self.join()
+            if do_join:
+                # Wait for the thread to finish
+                if self.is_running:
+                    self.join()
 
 
 class LhGeoInputContainerData():

From 711dea23373fedd47618946ed68c7ea9ddd44920 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 25 Jun 2025 11:33:18 +0200
Subject: [PATCH 26/36] Try to solve when the solver thread is started

---
 .../lighthouse_geo_estimation_manager.py          | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 38641ef00..171fe9d3c 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -90,7 +90,8 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
                 solution.poses = scaled_solution
 
         cls._humanize_error_info(solution, container)
-        # TODO krri indicate in the solution if there is a geometry
+
+        # TODO krri indicate in the solution if there is a geometry. progress_is_ok is not a good indicator
 
         return solution
 
@@ -147,6 +148,8 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSample], container: LhGe
     @classmethod
     def _humanize_error_info(cls, solution: LighthouseGeometrySolution, container: LhGeoInputContainerData) -> None:
         """Humanize the error info in the solution object"""
+
+        # There might already be an error reported earlier, so only check if we think the sample is valid
         if solution.is_origin_sample_valid:
             solution.is_origin_sample_valid, solution.origin_sample_info = cls._error_info_for(solution,
                                                                                                [container.origin])
@@ -189,7 +192,7 @@ def __init__(self, container: LhGeoInputContainer, is_done_cb) -> None:
             self.daemon = True
 
             self.container = container
-            self.latest_solved_data_version = container._data.version
+            self.latest_solved_data_version = -1
 
             self.is_done_cb = is_done_cb
 
@@ -319,6 +322,14 @@ def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         self._data.xyz_space += new_samples
         self._update_version()
 
+    def clear_all_samples(self) -> None:
+        """Clear all samples in the container"""
+        self._data.origin = self._data.EMPTY_POSE_SAMPLE
+        self._data.x_axis = []
+        self._data.xy_plane = []
+        self._data.xyz_space = []
+        self._update_version()
+
     def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
         sample.augment_with_ippe(self._data.sensor_positions)
         sample.is_mandatory = is_mandatory

From 1cedb1d454d09831eed5b2aaf03accc9b48806df Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Jun 2025 09:06:19 +0200
Subject: [PATCH 27/36] Added sample count methods

---
 .../lighthouse_geo_estimation_manager.py         | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 171fe9d3c..59d98b952 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -302,6 +302,14 @@ def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
         self._data.xy_plane.append(xy_plane)
         self._update_version()
 
+    def xy_plane_sample_count(self) -> int:
+        """Get the number of samples in the xy-plane
+
+        Returns:
+            int: The number of samples in the xy-plane
+        """
+        return len(self._data.xy_plane)
+
     def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """Store/update the samples for the volume
 
@@ -322,6 +330,14 @@ def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         self._data.xyz_space += new_samples
         self._update_version()
 
+    def xyz_space_sample_count(self) -> int:
+        """Get the number of samples in the xyz space
+
+        Returns:
+            int: The number of samples in the xyz space
+        """
+        return len(self._data.xyz_space)
+
     def clear_all_samples(self) -> None:
         """Clear all samples in the container"""
         self._data.origin = self._data.EMPTY_POSE_SAMPLE

From 7c084abbfd5ed5d3254ef16ace537b62b2273c4d Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Jun 2025 09:06:38 +0200
Subject: [PATCH 28/36] Corrected callback

---
 cflib/localization/user_action_detector.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/cflib/localization/user_action_detector.py b/cflib/localization/user_action_detector.py
index f8a089371..278b2a543 100644
--- a/cflib/localization/user_action_detector.py
+++ b/cflib/localization/user_action_detector.py
@@ -54,7 +54,7 @@ def start(self):
         if not self._is_active:
             self._is_active = True
             self._reset()
-            self._cf.disconnected.add_callback(self.stop)
+            self._cf.disconnected.add_callback(self._disconnected_callback)
 
             self._lg_config = LogConfig(name='lighthouse_geo_estimator', period_in_ms=25)
             self._lg_config.add_variable('gyro.z', 'float')
@@ -69,9 +69,12 @@ def stop(self):
                 self._lg_config.delete()
                 self._lg_config.data_received_cb.remove_callback(self._log_callback)
                 self._lg_config = None
-            self._cf.disconnected.remove_callback(self.stop)
+            self._cf.disconnected.remove_callback(self._disconnected_callback)
             self._is_active = False
 
+    def _disconnected_callback(self, uri):
+        self.stop()
+
     def _log_callback(self, ts, data, logblock):
         if self._is_active:
             gyro_z = data['gyro.z']

From a03c1200d851c657510518c1cfdd53eb2ea23d65 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 27 Jun 2025 09:49:31 +0200
Subject: [PATCH 29/36] Build link stats as early as possible

---
 cflib/localization/lighthouse_initial_estimator.py | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index eb772e351..7b99d6ef3 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -73,16 +73,14 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
         # bs, as seen from the first bs (in the first bs ref frame).
 
         bs_poses_ref_cfs, cleaned_matched_samples = cls._angles_to_poses(matched_samples, bs_positions, solution)
+        cls._build_link_stats(cleaned_matched_samples, solution)
         if not solution.progress_is_ok:
             return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
-        cls._build_link_stats(matched_samples, solution)
-        # TODO krri: This step should check that we have enough links between base stations and fail with good
-        # user information.
+        # TODO krri: We should check that we have enough links between base stations and fail with good
+        # user information if not.
         # We could also filter out base stations that are not linked instead of failing the solution (in
         # _estimate_bs_poses()).
-        if not solution.progress_is_ok:
-            return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
         # Calculate the pose of all base stations, based on the pose of one base station
         try:

From 97a012df52d9c5b1f42ad4d12610c529bb6ef7ed Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 1 Jul 2025 11:29:40 +0200
Subject: [PATCH 30/36] Added script for uploading geometries

---
 examples/lighthouse/upload_geos.py | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)
 create mode 100644 examples/lighthouse/upload_geos.py

diff --git a/examples/lighthouse/upload_geos.py b/examples/lighthouse/upload_geos.py
new file mode 100644
index 000000000..df38c0587
--- /dev/null
+++ b/examples/lighthouse/upload_geos.py
@@ -0,0 +1,23 @@
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.utils import uri_helper
+from cflib.localization import LighthouseConfigFileManager, LighthouseConfigWriter
+
+
+# Upload a geometry to one or more Crazyflies.
+
+mgr = LighthouseConfigFileManager()
+geos, calibs, type = mgr.read('/path/to/your/geo.yaml')
+
+uri_list = [
+    "radio://0/70/2M/E7E7E7E770"
+]
+
+# Initialize the low-level drivers
+cflib.crtp.init_drivers()
+
+for uri in uri_list:
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        writer = LighthouseConfigWriter(scf.cf)
+        writer.write_and_store_config(data_stored_cb=None, geos=geos, calibs=calibs)

From 17e800d17e8023381f3d69484bff4f672585f925 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 2 Jul 2025 13:45:19 +0200
Subject: [PATCH 31/36] Added user notification platform service

---
 cflib/crazyflie/platformservice.py | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/cflib/crazyflie/platformservice.py b/cflib/crazyflie/platformservice.py
index 262587a25..36421511c 100644
--- a/cflib/crazyflie/platformservice.py
+++ b/cflib/crazyflie/platformservice.py
@@ -42,6 +42,7 @@
 PLATFORM_SET_CONT_WAVE = 0
 PLATFORM_REQUEST_ARMING = 1
 PLATFORM_REQUEST_CRASH_RECOVERY = 2
+PLATFORM_REQUEST_USER_NOTIFICATION = 3
 
 VERSION_GET_PROTOCOL = 0
 VERSION_GET_FIRMWARE = 1
@@ -110,6 +111,17 @@ def send_crash_recovery_request(self):
         pk.data = (PLATFORM_REQUEST_CRASH_RECOVERY, )
         self._cf.send_packet(pk)
 
+    def send_user_notification(self, success: bool = True):
+        """
+        Send a user notification to the Crazyflie. This is used to notify a user of some sort of event by using the
+        means available on the Crazyflie.
+        """
+        pk = CRTPPacket()
+        pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
+        notification_type = 1 if success else 0
+        pk.data = (PLATFORM_REQUEST_USER_NOTIFICATION, notification_type)
+        self._cf.send_packet(pk)
+
     def get_protocol_version(self):
         """
         Return version of the CRTP protocol

From ab5cfcc40427fdc101b67301926fd64dceb739e4 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 2 Jul 2025 17:17:54 +0200
Subject: [PATCH 32/36] Added timeout to sweep angle reader

---
 .../lighthouse_sweep_angle_reader.py          | 50 +++++++++++++++----
 1 file changed, 41 insertions(+), 9 deletions(-)

diff --git a/cflib/localization/lighthouse_sweep_angle_reader.py b/cflib/localization/lighthouse_sweep_angle_reader.py
index 824a8448f..aa0652000 100644
--- a/cflib/localization/lighthouse_sweep_angle_reader.py
+++ b/cflib/localization/lighthouse_sweep_angle_reader.py
@@ -20,6 +20,7 @@
 # You should have received a copy of the GNU General Public License
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from collections.abc import Callable
+from threading import Timer
 
 from cflib.crazyflie import Crazyflie
 from cflib.localization import LighthouseBsVector
@@ -173,11 +174,14 @@ class LighthouseMatchedSweepAngleReader():
     MATCHED_STREAM_MAX_TIME_PARAM = 'locSrv.maxTimeLhMtchStm'
     NR_OF_SENSORS = 4
 
-    def __init__(self, cf: Crazyflie, data_recevied_cb, sample_count: int = 1, min_bs: int = 2, max_time_ms: int = 25):
+    def __init__(self, cf: Crazyflie, data_recevied_cb, timeout_cb=None, sample_count: int = 1, min_bs: int = 2,
+                 max_time_ms: int = 25):
         self._cf = cf
-        self._cb = data_recevied_cb
+        self._data_cb = data_recevied_cb
+        self._timeout_cb = timeout_cb
         self._is_active = False
         self._sample_count = sample_count
+        self._sample_count_remaining = 0
 
         # The maximum number of base stations is limited in the CF due to memory considerations.
         if min_bs > 4:
@@ -189,19 +193,41 @@ def __init__(self, cf: Crazyflie, data_recevied_cb, sample_count: int = 1, min_b
         self._current_group_id = 0
         self._angles: dict[int, LighthouseBsVectors] = {}
 
-    def start(self):
-        """Start reading sweep angles"""
+        self._timeout_timer = None
+
+    def start(self, timeout: float = 0.0):
+        """Start reading sweep angles
+
+        Args:
+            timeout (float): timeout in seconds, 0.0 means no timeout
+        """
         self._cf.loc.receivedLocationPacket.add_callback(self._packet_received_cb)
         self._is_active = True
         self._angle_stream_activate(True)
+        self._sample_count_remaining = self._sample_count
+
+        self._clear_timer()
+        self._timeout_timer = Timer(timeout, self._timer_done_cb)
+        self._timeout_timer.start()
 
     def stop(self):
         """Stop reading sweep angles"""
         if self._is_active:
             self._is_active = False
+            self._clear_timer()
             self._cf.loc.receivedLocationPacket.remove_callback(self._packet_received_cb)
             self._angle_stream_activate(False)
 
+    def _clear_timer(self):
+        if self._timeout_timer is not None:
+            self._timeout_timer.cancel()
+            self._timeout_timer = None
+
+    def _timer_done_cb(self):
+        self.stop()
+        if self._timeout_cb:
+            self._timeout_cb()
+
     def _angle_stream_activate(self, is_active: bool):
         value = 0
         if is_active:
@@ -226,7 +252,7 @@ def _packet_received_cb(self, packet):
                 if len(self._angles) >= self._min_bs:
                     # We have enough angles in the previous group even though all angles were not received
                     # Lost a packet?
-                    self._call_callback()
+                    self._call_data_callback()
 
                 # Reset
                 self._current_group_id = group_id
@@ -239,9 +265,15 @@ def _packet_received_cb(self, packet):
 
             if len(self._angles) == bs_count:
                 # We have received all angles for this group, call the callback
-                self._call_callback()
+                self._call_data_callback()
 
-    def _call_callback(self):
-        if self._cb:
-            self._cb(LhCfPoseSample(self._angles))
+            if self._sample_count_remaining <= 0:
+                # We have received enough samples, stop the reader
+                self.stop()
+
+    def _call_data_callback(self):
+        self._sample_count_remaining -= 1
+
+        if self._data_cb:
+            self._data_cb(LhCfPoseSample(self._angles))
             self._angles = {}

From bd6576fc8e645ffb5f77223aaaf6822f415bd5ee Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 3 Jul 2025 11:18:20 +0200
Subject: [PATCH 33/36] Touch up of multi bs estimation script

---
 .../multi_bs_geometry_estimation.py           | 51 +++++++++++--------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index f93ee48d9..31842ade0 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -144,11 +144,11 @@ def parse_recording_time(recording_time: str, default: int) -> int:
         return default
 
 
-def print_base_stations_poses(base_stations: dict[int, Pose]):
+def print_base_stations_poses(base_stations: dict[int, Pose], printer=print):
     """Pretty print of base stations pose"""
     for bs_id, pose in sorted(base_stations.items()):
         pos = pose.translation
-        print(f'    {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})')
+        printer(f'    {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})')
 
 
 def set_axes_equal(ax):
@@ -225,19 +225,21 @@ def load_from_file(name: str) -> LhGeoInputContainerData:
 
 
 def print_solution(solution: LighthouseGeometrySolution):
-    print('Solution ready --------------------------------------')
-    print('  Base stations at:')
+    def _print(msg: str):
+        print(f'      * {msg}')
+    _print('Solution ready --------------------------------------')
+    _print('  Base stations at:')
     bs_poses = solution.poses.bs_poses
-    print_base_stations_poses(bs_poses)
+    print_base_stations_poses(bs_poses, printer=_print)
 
-    print(f'Converged: {solution.has_converged}')
-    print(f'Progress info: {solution.progress_info}')
-    print(f'Progress is ok: {solution.progress_is_ok}')
-    print(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}')
-    print(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}')
-    print(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}')
-    print(f'XYZ space: {solution.xyz_space_samples_info}')
-    print(f'General info: {solution.general_failure_info}')
+    _print(f'Converged: {solution.has_converged}')
+    _print(f'Progress info: {solution.progress_info}')
+    _print(f'Progress is ok: {solution.progress_is_ok}')
+    _print(f'Origin: {solution.is_origin_sample_valid}, {solution.origin_sample_info}')
+    _print(f'X-axis: {solution.is_x_axis_samples_valid}, {solution.x_axis_samples_info}')
+    _print(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}')
+    _print(f'XYZ space: {solution.xyz_space_samples_info}')
+    _print(f'General info: {solution.general_failure_info}')
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):
@@ -274,8 +276,10 @@ def get_recording(scf: SyncCrazyflie) -> LhCfPoseSample:
         measurement = record_angles_average(scf)
         if measurement is not None:
             data = measurement
+            scf.cf.platform.send_user_notification(True)
             break  # Exit the loop if a valid measurement is obtained
         else:
+            scf.cf.platform.send_user_notification(False)
             time.sleep(1)
             print('Invalid measurement, please try again.')
     return data
@@ -301,8 +305,10 @@ def get_multiple_recordings(scf: SyncCrazyflie) -> list[LhCfPoseSample]:
         print('  Recording...')
         measurement = record_angles_average(scf)
         if measurement is not None:
+            scf.cf.platform.send_user_notification(True)
             data.append(measurement)
         else:
+            scf.cf.platform.send_user_notification(False)
             time.sleep(1)
             print('Invalid measurement, please try again.')
 
@@ -322,7 +328,7 @@ def _local_solution_handler(solution: LighthouseGeometrySolution):
                 upload_geometry(scf, solution.poses.bs_poses)
                 print('Geometry uploaded to Crazyflie.')
 
-        thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=print_solution)
+        thread = LhGeoEstimationManager.SolverThread(container, is_done_cb=_local_solution_handler)
         thread.start()
 
         print('  Connected')
@@ -350,19 +356,24 @@ def _local_solution_handler(solution: LighthouseGeometrySolution):
               'around the Z-axis. This will trigger a measurement of the base station angles. ')
 
         def matched_angles_cb(sample: LhCfPoseSample):
-            print('Sampled position')
+            print('Position stored')
+            scf.cf.platform.send_user_notification(True)
             container.append_xyz_space_samples([sample])
+            scf.cf.platform.send_user_notification()
             write_to_file(file_name, container)
-        angle_reader = LighthouseMatchedSweepAngleReader(scf.cf, matched_angles_cb)
+
+        def timeout_cb():
+            print('Timeout, no angles received. Please try again.')
+            scf.cf.platform.send_user_notification(False)
+        angle_reader = LighthouseMatchedSweepAngleReader(scf.cf, matched_angles_cb, timeout_cb=timeout_cb)
 
         def user_action_cb():
-            angle_reader.start()
+            print("Sampling...")
+            angle_reader.start(timeout=1.0)
         detector = UserActionDetector(scf.cf, cb=user_action_cb)
 
         detector.start()
-        while True:
-            time.sleep(0.5)
-            # TODO krri
+        input('Press return to terminate the script when all required positions have been sampled.')
 
         detector.stop()
         thread.stop()

From 6d2b1ef8a1ecaf19feb0ed8a1f98fe2235593080 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 3 Jul 2025 15:53:07 +0200
Subject: [PATCH 34/36] Use crossing beam to calculate error of solution

---
 cflib/localization/__init__.py                |   4 +-
 .../lighthouse_geo_estimation_manager.py      |  24 ++-
 .../lighthouse_geometry_solution.py           |  31 ++-
 .../lighthouse_geometry_solver.py             |  40 ----
 .../lighthouse_initial_estimator.py           |   5 -
 cflib/localization/lighthouse_utils.py        | 190 ++++++++++++++++++
 .../multi_bs_geometry_estimation.py           |   3 +-
 7 files changed, 247 insertions(+), 50 deletions(-)
 create mode 100644 cflib/localization/lighthouse_utils.py

diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py
index 9d4ee81db..0d873d0d7 100644
--- a/cflib/localization/__init__.py
+++ b/cflib/localization/__init__.py
@@ -26,6 +26,7 @@
 from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
+from .lighthouse_utils import LighthouseCrossingBeam
 from .param_io import ParamFileManager
 
 __all__ = [
@@ -36,4 +37,5 @@
     'LighthouseMatchedSweepAngleReader',
     'LighthouseConfigFileManager',
     'LighthouseConfigWriter',
-    'ParamFileManager']
+    'ParamFileManager',
+    'LighthouseCrossingBeam']
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 59d98b952..26a31c3e5 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -34,6 +34,7 @@
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
 from cflib.localization.lighthouse_types import LhBsCfPoses
+from cflib.localization.lighthouse_utils import LighthouseCrossingBeam
 
 
 ArrayFloat = npt.NDArray[np.float_]
@@ -80,7 +81,6 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
             initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(validated_matched_samples,
                                                                                          solution)
             solution.poses = initial_guess
-
             if solution.progress_is_ok:
                 solution.progress_info = 'Refining geometry solution'
                 LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions,
@@ -89,6 +89,8 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
                 scaled_solution = cls.align_and_scale_solution(container, solution.poses, cls.REFERENCE_DIST)
                 solution.poses = scaled_solution
 
+                cls._create_solution_stats(validated_matched_samples, solution)
+
         cls._humanize_error_info(solution, container)
 
         # TODO krri indicate in the solution if there is a geometry. progress_is_ok is not a good indicator
@@ -174,6 +176,26 @@ def _error_info_for(cls, solution: LighthouseGeometrySolution, samples: list[LhC
         else:
             return True, ''
 
+    @classmethod
+    def _create_solution_stats(cls, matched_samples: list[LhCfPoseSample], solution: LighthouseGeometrySolution) -> None:
+        """Calculate statistics about the solution and store them in the solution object"""
+
+        # Estimated worst error for each sample based on crossing beams
+        estimated_errors: list[float] = []
+
+        for sample in matched_samples:
+            bs_ids = list(sample.angles_calibrated.keys())
+
+            bs_angle_list = [(solution.poses.bs_poses[bs_id], sample.angles_calibrated[bs_id]) for bs_id in bs_ids]
+            sample_error = LighthouseCrossingBeam.max_distance_all_permutations(bs_angle_list)
+            estimated_errors.append(sample_error)
+
+        solution.error_stats = LighthouseGeometrySolution.ErrorStats(
+            mean=np.mean(estimated_errors),
+            max=np.max(estimated_errors),
+            std=np.std(estimated_errors)
+        )
+
     class SolverThread(threading.Thread):
         """This class runs the geometry solver in a separate thread.
         It is used to provide continuous updates of the solution as well as updating the geometry in the Crazyflie.
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index a37a66585..bbc531365 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -1,3 +1,29 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2025 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+from collections import namedtuple
+
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_types import LhBsCfPoses
 
@@ -7,13 +33,14 @@ class LighthouseGeometrySolution:
     A class to represent the solution of a lighthouse geometry problem.
     """
 
+    ErrorStats = namedtuple('ErrorStats', ['mean', 'max', 'std'])
+
     def __init__(self):
         # The estimated poses of the base stations and the CF samples
         self.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
 
         # Information about errors in the solution
-        # TODO krri This data is not well structured
-        self.error_info = {}
+        self.error_stats = self.ErrorStats(0.0, 0.0, 0.0)
 
         # Indicates if the solution converged (True).
         # If it did not converge, the solution is possibly not good enough to use
diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index f0f5ae43a..ffa0e3e6e 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -395,43 +395,3 @@ def _condense_results(cls, lsq_result, defs: SolverData, matched_samples: list[L
             solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, defs)
 
         solution.has_converged = lsq_result.success
-
-        # Extract the error for each CF pose
-        residuals = lsq_result.fun
-        i = 0
-        # Estimated error for each base station in each sample
-        estimated_errors: list[dict[int, float]] = []
-
-        for sample in matched_samples:
-            sample_errors = {}
-            for bs_id in sorted(sample.angles_calibrated.keys()):
-                sample_errors[bs_id] = np.linalg.norm(residuals[i:i + 2])
-                i += defs.n_sensors * 2
-            estimated_errors.append(sample_errors)
-
-        solution.error_info = cls._aggregate_error_info(estimated_errors)
-
-    @classmethod
-    def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]) -> dict[str, float]:
-        error_per_bs = {}
-        errors = []
-        for sample_errors in estimated_errors:
-            for bs_id, error in sample_errors.items():
-                if bs_id not in error_per_bs:
-                    error_per_bs[bs_id] = []
-                error_per_bs[bs_id].append(error)
-                errors.append(error)
-
-        error_info = {}
-        error_info['mean_error'] = np.mean(errors)
-        error_info['max_error'] = np.max(errors)
-        error_info['std_error'] = np.std(errors)
-
-        error_info['bs'] = {}
-        for bs_id, errors in error_per_bs.items():
-            error_info['bs'][bs_id] = {}
-            error_info['bs'][bs_id]['mean_error'] = np.mean(errors)
-            error_info['bs'][bs_id]['max_error'] = np.max(errors)
-            error_info['bs'][bs_id]['std_error'] = np.std(errors)
-
-        return error_info
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 7b99d6ef3..6be1a2fca 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -77,11 +77,6 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
         if not solution.progress_is_ok:
             return LhBsCfPoses(bs_poses={}, cf_poses=[]), cleaned_matched_samples
 
-        # TODO krri: We should check that we have enough links between base stations and fail with good
-        # user information if not.
-        # We could also filter out base stations that are not linked instead of failing the solution (in
-        # _estimate_bs_poses()).
-
         # Calculate the pose of all base stations, based on the pose of one base station
         try:
             bs_poses = cls._estimate_bs_poses(bs_poses_ref_cfs)
diff --git a/cflib/localization/lighthouse_utils.py b/cflib/localization/lighthouse_utils.py
new file mode 100644
index 000000000..c374878ea
--- /dev/null
+++ b/cflib/localization/lighthouse_utils.py
@@ -0,0 +1,190 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2025 Bitcraze AB
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, in version 3.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <http://www.gnu.org/licenses/>.
+from __future__ import annotations
+
+import math
+
+import numpy as np
+import numpy.typing as npt
+
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVector, LighthouseBsVectors
+from cflib.localization.lighthouse_types import Pose
+
+
+class LighthouseCrossingBeam:
+    """A class to calculate the crossing point of two "beams" from two base stations. The beams are defined by the line
+    where the two light planes intersect. In a perfect world the crossing point of the two beams is the position of
+    a sensor on the Crazyflie Lighthouse deck, but in reality the beams will most likely not cross and instead we
+    use the closest point between the two beams as the position estimate. The (minimum) distance between the beams
+    is also calculated and can be used as an error estimate for the position.
+    """
+
+    @classmethod
+    def position_distance(cls,
+                          bs1: Pose, angles_bs1: LighthouseBsVector,
+                          bs2: Pose, angles_bs2: LighthouseBsVector) -> tuple[npt.NDArray, float]:
+        """Calculate the estimated position of the crossing point of the beams
+        from two base stations as well as the distance.
+
+        Args:
+            bs1 (Pose): The pose of the first base station.
+            angles_bs1 (LighthouseBsVector): The sweep angles of the first base station.
+            bs2 (Pose): The pose of the second base station.
+            angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
+
+        Returns:
+            tuple[npt.NDArray, float]: The estimated position of the crossing point and the distance between the beams.
+        """
+        orig_1 = bs1.translation
+        vec_1 = bs1.rot_matrix @ angles_bs1.cart
+
+        orig_2 = bs2.translation
+        vec_2 = bs2.rot_matrix @ angles_bs2.cart
+
+        return cls._position_distance(orig_1, vec_1, orig_2, vec_2)
+
+    @classmethod
+    def position(cls,
+                 bs1: Pose, angles_bs1: LighthouseBsVector,
+                 bs2: Pose, angles_bs2: LighthouseBsVector) -> npt.NDArray:
+        """Calculate the estimated position of the crossing point of the beams
+        from two base stations.
+
+        Args:
+            bs1 (Pose): The pose of the first base station.
+            angles_bs1 (LighthouseBsVector): The sweep angles of the first base station.
+            bs2 (Pose): The pose of the second base station.
+            angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
+
+        Returns:
+            npt.NDArray: The estimated position of the crossing point of the two beams.
+        """
+        position, _ = cls.position_distance(bs1, angles_bs1, bs2, angles_bs2)
+        return position
+
+    @classmethod
+    def distance(cls,
+                 bs1: Pose, angles_bs1: LighthouseBsVector,
+                 bs2: Pose, angles_bs2: LighthouseBsVector) -> float:
+        """Calculate the minimum distance between the beams from two base stations.
+
+        Args:
+            bs1 (Pose): The pose of the first base station.
+            angles_bs1 (LighthouseBsVector): The sweep angles of the first base station.
+            bs2 (Pose): The pose of the second base station.
+            angles_bs2 (LighthouseBsVector): The sweep angles of the second base station.
+
+        Returns:
+            float: The shortest distance between the beams.
+        """
+        _, distance = cls.position_distance(bs1, angles_bs1, bs2, angles_bs2)
+        return distance
+
+    @classmethod
+    def distances(cls,
+                  bs1: Pose, angles_bs1: LighthouseBsVectors,
+                  bs2: Pose, angles_bs2: LighthouseBsVectors) -> list[float]:
+        """Calculate the minimum distance between the beams from two base stations for all sensors.
+
+        Args:
+            bs1 (Pose): The pose of the first base station.
+            angles_bs1 (LighthouseBsVectors): The sweep angles of the first base station.
+            bs2 (Pose): The pose of the second base station.
+            angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
+
+        Returns:
+            list[float]: A list of the distances.
+        """
+        return [cls.distance(bs1, angles1, bs2, angles2) for angles1, angles2 in zip(angles_bs1, angles_bs2)]
+
+    @classmethod
+    def max_distance(cls,
+                     bs1: Pose, angles_bs1: LighthouseBsVectors,
+                     bs2: Pose, angles_bs2: LighthouseBsVectors) -> float:
+        """Calculate the maximum distance between the beams from two base stations for all sensors.
+
+        Args:
+            bs1 (Pose): The pose of the first base station.
+            angles_bs1 (LighthouseBsVectors): The sweep angles of the first base station.
+            bs2 (Pose): The pose of the second base station.
+            angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
+
+        Returns:
+            float: The maximum distance between the beams.
+        """
+        return max(cls.distances(bs1, angles_bs1, bs2, angles_bs2))
+
+    @classmethod
+    def max_distance_all_permutations(cls, bs_angles: list[tuple[Pose, LighthouseBsVectors]]) -> float:
+        """Calculate the maximum distance between the beams from base stations for all sensors. All permutations of
+        base stations are considered. This result can be used as an estimation of the maximum error.
+
+        Args:
+            bs_angles (list[tuple[Pose, LighthouseBsVectors]]): A list of tuples containing the pose of the base
+            stations and their sweep angles.
+
+        Returns:
+            float: The maximum distance between the beams from all permutations of base stations.
+        """
+        if len(bs_angles) < 2:
+            raise ValueError("At least two base stations are required to calculate the maximum distance.")
+
+        max_distance = 0.0
+        bs_count = len(bs_angles)
+        for i1 in range(bs_count - 1):
+            for i2 in range(i1 + 1, bs_count):
+                bs1, angles_bs1 = bs_angles[i1]
+                bs2, angles_bs2 = bs_angles[i2]
+                # Calculate the distance for this pair of base stations
+                distance = cls.max_distance(bs1, angles_bs1, bs2, angles_bs2)
+                max_distance = max(max_distance, distance)
+
+        return max_distance
+
+    @classmethod
+    def _position_distance(cls,
+                           orig_1: npt.NDArray, vec_1: npt.NDArray,
+                           orig_2: npt.NDArray, vec_2: npt.NDArray) -> tuple[npt.NDArray, float]:
+        w0 = orig_1 - orig_2
+        a = np.dot(vec_1, vec_1)
+        b = np.dot(vec_1, vec_2)
+        c = np.dot(vec_2, vec_2)
+        d = np.dot(vec_1, w0)
+        e = np.dot(vec_2, w0)
+
+        denom = a * c - b * b
+
+        # Closest point to line 2 on line 1
+        t = (b * e - c * d) / denom
+        pt1 = orig_1 + t * vec_1
+
+        # Closest point to line 1 on line 2
+        t = (a * e - b * d) / denom
+        pt2 = orig_2 + t * vec_2
+
+        # Point between the two lines
+        pt = (pt1 + pt2) / 2
+
+        # Distance between the two closest points of the beams
+        distance = np.linalg.norm(pt1 - pt2)
+
+        return pt, float(distance)
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 31842ade0..f3696b57e 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -240,6 +240,7 @@ def _print(msg: str):
     _print(f'XY-plane: {solution.is_xy_plane_samples_valid}, {solution.xy_plane_samples_info}')
     _print(f'XYZ space: {solution.xyz_space_samples_info}')
     _print(f'General info: {solution.general_failure_info}')
+    _print(f'Error info: {solution.error_stats}')
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):
@@ -368,7 +369,7 @@ def timeout_cb():
         angle_reader = LighthouseMatchedSweepAngleReader(scf.cf, matched_angles_cb, timeout_cb=timeout_cb)
 
         def user_action_cb():
-            print("Sampling...")
+            print('Sampling...')
             angle_reader.start(timeout=1.0)
         detector = UserActionDetector(scf.cf, cb=user_action_cb)
 

From 968827992e77a98b8f52185713f3c66013e82b7c Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 4 Jul 2025 14:28:27 +0200
Subject: [PATCH 35/36] Added read/write of geo raw data as yaml

---
 cflib/localization/lighthouse_bs_vector.py    |  42 +++++++
 .../localization/lighthouse_cf_pose_sample.py |  34 +++++-
 .../lighthouse_geo_estimation_manager.py      | 105 +++++++++++++++---
 cflib/localization/lighthouse_types.py        |  27 +++++
 cflib/localization/lighthouse_utils.py        |   7 +-
 .../multi_bs_geometry_estimation.py           |  14 +--
 .../localization/test_lighthouse_bs_vector.py |  69 ++++++++++++
 .../test_lighthouse_cf_pose_sample.py         |  59 ++++++++++
 test/localization/test_lighthouse_types.py    |  24 ++++
 9 files changed, 351 insertions(+), 30 deletions(-)
 create mode 100644 test/localization/test_lighthouse_cf_pose_sample.py

diff --git a/cflib/localization/lighthouse_bs_vector.py b/cflib/localization/lighthouse_bs_vector.py
index 0f29eb432..41c9ed66a 100644
--- a/cflib/localization/lighthouse_bs_vector.py
+++ b/cflib/localization/lighthouse_bs_vector.py
@@ -25,6 +25,7 @@
 
 import numpy as np
 import numpy.typing as npt
+import yaml
 
 
 class LighthouseBsVector:
@@ -137,6 +138,32 @@ def projection(self) -> npt.NDArray[np.float32]:
     def _q(self):
         return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2)
 
+    def __eq__(self, other):
+        if not isinstance(other, LighthouseBsVector):
+            return NotImplemented
+
+        return (self._lh_v1_horiz_angle == other._lh_v1_horiz_angle and
+                self._lh_v1_vert_angle == other._lh_v1_vert_angle)
+
+    @staticmethod
+    def yaml_representer(dumper, data: 'LighthouseBsVector'):
+        return dumper.represent_mapping('!LighthouseBsVector', {
+            'lh_v1_angles': [data.lh_v1_horiz_angle, data.lh_v1_vert_angle],
+        })
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_mapping(node, deep=True)
+        lh_v1_angles = values.get('lh_v1_angles', [0.0, 0.0])
+        if len(lh_v1_angles) != 2:
+            raise ValueError('lh_v1_angles must be a list of two angles')
+        lh_v1_horiz_angle, lh_v1_vert_angle = lh_v1_angles
+        return LighthouseBsVector(lh_v1_horiz_angle, lh_v1_vert_angle)
+
+
+yaml.add_representer(LighthouseBsVector, LighthouseBsVector.yaml_representer)
+yaml.add_constructor('!LighthouseBsVector', LighthouseBsVector.yaml_constructor)
+
 
 class LighthouseBsVectors(list):
     """A list of 4 LighthouseBsVector, one for each sensor.
@@ -162,3 +189,18 @@ def angle_list(self) -> npt.NDArray:
             result[i * 2 + 1] = vector.lh_v1_vert_angle
 
         return result
+
+    @staticmethod
+    def yaml_representer(dumper, data: 'LighthouseBsVectors'):
+        # Instead of using a sequence of LighthouseBsVector, we represent it as a sequence of lists to make it more
+        # compact
+        return dumper.represent_sequence('!LighthouseBsVectors', [list(vector.lh_v1_angle_pair) for vector in data])
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_sequence(node, deep=True)
+        return LighthouseBsVectors([LighthouseBsVector(pair[0], pair[1]) for pair in values])
+
+
+yaml.add_representer(LighthouseBsVectors, LighthouseBsVectors.yaml_representer)
+yaml.add_constructor('!LighthouseBsVectors', LighthouseBsVectors.yaml_constructor)
diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index ef95ecfb1..919dd3b72 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -2,6 +2,7 @@
 
 import numpy as np
 import numpy.typing as npt
+import yaml
 
 from .ippe_cf import IppeCf
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
@@ -25,7 +26,8 @@ class LhCfPoseSample:
     The ippe solution is somewhat heavy and is only created on demand by calling augment_with_ippe()
     """
 
-    def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp: float = 0.0) -> None:
+    def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp: float = 0.0,
+                 is_mandatory: bool = False) -> None:
         self.timestamp: float = timestamp
 
         # Angles measured by the Crazyflie and compensated using calibration data
@@ -39,7 +41,7 @@ def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp:
 
         # Some samples are mandatory and must not be removed, even if they appear to be outliers. For instance the
         # the samples that define the origin or x-axis
-        self.is_mandatory = False
+        self.is_mandatory = is_mandatory
 
     def augment_with_ippe(self, sensor_positions: ArrayFloat) -> None:
         if not self.is_augmented:
@@ -77,3 +79,31 @@ def _convert_estimates_to_cf_reference_frame(self, estimates_ref_bs: list[IppeCf
         t_2 = np.dot(rot_2, -estimates_ref_bs[1].t)
 
         return BsPairPoses(Pose(rot_1, t_1), Pose(rot_2, t_2))
+
+    def __eq__(self, other):
+        if not isinstance(other, LhCfPoseSample):
+            return NotImplemented
+
+        return (self.timestamp == other.timestamp and
+                self.angles_calibrated == other.angles_calibrated and
+                self.is_mandatory == other.is_mandatory)
+
+    @staticmethod
+    def yaml_representer(dumper, data: 'LhCfPoseSample'):
+        return dumper.represent_mapping('!LhCfPoseSample', {
+            'timestamp': data.timestamp,
+            'angles_calibrated': data.angles_calibrated,
+            'is_mandatory': data.is_mandatory
+        })
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_mapping(node, deep=True)
+        timestamp = values.get('timestamp', 0.0)
+        angles_calibrated = values.get('angles_calibrated', {})
+        is_mandatory = values.get('is_mandatory', False)
+        return LhCfPoseSample(angles_calibrated, timestamp, is_mandatory)
+
+
+yaml.add_representer(LhCfPoseSample, LhCfPoseSample.yaml_representer)
+yaml.add_constructor('!LhCfPoseSample', LhCfPoseSample.yaml_constructor)
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 26a31c3e5..b85f39435 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -23,9 +23,11 @@
 
 import copy
 import threading
+from typing import TextIO
 
 import numpy as np
 import numpy.typing as npt
+import yaml
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
@@ -177,7 +179,7 @@ def _error_info_for(cls, solution: LighthouseGeometrySolution, samples: list[LhC
             return True, ''
 
     @classmethod
-    def _create_solution_stats(cls, matched_samples: list[LhCfPoseSample], solution: LighthouseGeometrySolution) -> None:
+    def _create_solution_stats(cls, matched_samples: list[LhCfPoseSample], solution: LighthouseGeometrySolution):
         """Calculate statistics about the solution and store them in the solution object"""
 
         # Estimated worst error for each sample based on crossing beams
@@ -256,8 +258,9 @@ def stop(self, do_join: bool = True):
 
 
 class LhGeoInputContainerData():
-    def __init__(self, sensor_positions: ArrayFloat) -> None:
-        self.EMPTY_POSE_SAMPLE = LhCfPoseSample(angles_calibrated={})
+    EMPTY_POSE_SAMPLE = LhCfPoseSample(angles_calibrated={})
+
+    def __init__(self, sensor_positions: ArrayFloat, version: int = 0) -> None:
         self.sensor_positions = sensor_positions
 
         self.origin: LhCfPoseSample = self.EMPTY_POSE_SAMPLE
@@ -265,7 +268,8 @@ def __init__(self, sensor_positions: ArrayFloat) -> None:
         self.xy_plane: list[LhCfPoseSample] = []
         self.xyz_space: list[LhCfPoseSample] = []
 
-        self.version = 0
+        # Used by LhGeoInputContainer to track changes in the data
+        self.version = version
 
     def get_matched_samples(self) -> list[LhCfPoseSample]:
         """Get all pose samples collected in a list
@@ -275,10 +279,50 @@ def get_matched_samples(self) -> list[LhCfPoseSample]:
         """
         return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
 
+    @staticmethod
+    def yaml_representer(dumper, data: LhGeoInputContainerData):
+        return dumper.represent_mapping('!LhGeoInputContainerData', {
+            'origin': data.origin,
+            'x_axis': data.x_axis,
+            'xy_plane': data.xy_plane,
+            'xyz_space': data.xyz_space,
+            'sensor_positions': data.sensor_positions.tolist(),
+        })
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        values = loader.construct_mapping(node, deep=True)
+        sensor_positions = np.array(values['sensor_positions'], dtype=np.float_)
+        result = LhGeoInputContainerData(sensor_positions)
+
+        result.origin = values['origin']
+        result.x_axis = values['x_axis']
+        result.xy_plane = values['xy_plane']
+        result.xyz_space = values['xyz_space']
+
+        # Augment the samples with the sensor positions
+        result.origin.augment_with_ippe(sensor_positions)
+
+        for sample in result.x_axis:
+            sample.augment_with_ippe(sensor_positions)
+
+        for sample in result.xy_plane:
+            sample.augment_with_ippe(sensor_positions)
+
+        for sample in result.xyz_space:
+            sample.augment_with_ippe(sensor_positions)
+
+        return result
+
+
+yaml.add_representer(LhGeoInputContainerData, LhGeoInputContainerData.yaml_representer)
+yaml.add_constructor('!LhGeoInputContainerData', LhGeoInputContainerData.yaml_constructor)
+
 
 class LhGeoInputContainer():
     """This class holds the input data required by the geometry estimation functionality.
     """
+    FILE_TYPE_VERSION = 1
 
     def __init__(self, sensor_positions: ArrayFloat) -> None:
         self._data = LhGeoInputContainerData(sensor_positions)
@@ -362,19 +406,7 @@ def xyz_space_sample_count(self) -> int:
 
     def clear_all_samples(self) -> None:
         """Clear all samples in the container"""
-        self._data.origin = self._data.EMPTY_POSE_SAMPLE
-        self._data.x_axis = []
-        self._data.xy_plane = []
-        self._data.xyz_space = []
-        self._update_version()
-
-    def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
-        sample.augment_with_ippe(self._data.sensor_positions)
-        sample.is_mandatory = is_mandatory
-
-    def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) -> None:
-        for sample in samples:
-            self._augment_sample(sample, is_mandatory)
+        self._set_new_data_container(LhGeoInputContainerData(self._data.sensor_positions))
 
     def get_data_version(self) -> int:
         """Get the current data version
@@ -392,6 +424,45 @@ def get_data_copy(self) -> LhGeoInputContainerData:
         """
         return copy.deepcopy(self._data)
 
+    def save_as_yaml_file(self, text_io: TextIO):
+        """Get the data in the container as a YAML string suitable for saving to a file
+        Returns:
+            str: The data in the container as a YAML string
+        """
+        data = {
+            'file_type_version': self.FILE_TYPE_VERSION,
+            'data': self._data
+        }
+        yaml.dump(data, text_io, default_flow_style=False)
+
+    def populate_from_file_yaml(self, text_io: TextIO) -> None:
+        """Load the data from file
+
+        Args:
+            wrapper
+        """
+        file_yaml = yaml.load(text_io, Loader=yaml.FullLoader)
+        if file_yaml['file_type_version'] != self.FILE_TYPE_VERSION:
+            raise ValueError(f'Unsupported file type version: {file_yaml["file_type_version"]}')
+        self._data = file_yaml['data']
+        self._update_version()
+
+    def _set_new_data_container(self, new_data: LhGeoInputContainerData) -> None:
+        """Set a new data container and update the version"""
+        # Maintain version
+        current_version = self._data.version
+        self._data = new_data
+        self._data.version = current_version
+        self._update_version()
+
+    def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
+        sample.augment_with_ippe(self._data.sensor_positions)
+        sample.is_mandatory = is_mandatory
+
+    def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) -> None:
+        for sample in samples:
+            self._augment_sample(sample, is_mandatory)
+
     def _update_version(self) -> None:
         """Update the data version and notify the waiting thread"""
         with self.is_modified_condition:
diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py
index 84ddb134d..23aa29683 100644
--- a/cflib/localization/lighthouse_types.py
+++ b/cflib/localization/lighthouse_types.py
@@ -25,6 +25,7 @@
 
 import numpy as np
 import numpy.typing as npt
+import yaml
 from scipy.spatial.transform import Rotation
 
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
@@ -135,6 +136,32 @@ def inv_rotate_translate_pose(self, pose: 'Pose') -> 'Pose':
 
         return Pose(R_matrix=R, t_vec=t)
 
+    def __eq__(self, other):
+        if not isinstance(other, Pose):
+            return NotImplemented
+
+        return np.array_equal(self._R_matrix, other._R_matrix) and np.array_equal(self._t_vec, other._t_vec)
+
+    @staticmethod
+    def yaml_representer(dumper, data: Pose):
+        """Represent a Pose object in YAML"""
+        return dumper.represent_mapping('!Pose', {
+            'R_matrix': data.rot_matrix.tolist(),
+            't_vec': data.translation.tolist()
+        })
+
+    @staticmethod
+    def yaml_constructor(loader, node):
+        """Construct a Pose object from YAML"""
+        values = loader.construct_mapping(node, deep=True)
+        R_matrix = np.array(values['R_matrix'])
+        t_vec = np.array(values['t_vec'])
+        return Pose(R_matrix=R_matrix, t_vec=t_vec)
+
+
+yaml.add_representer(Pose, Pose.yaml_representer)
+yaml.add_constructor('!Pose', Pose.yaml_constructor)
+
 
 class LhMeasurement(NamedTuple):
     """Represents a measurement from one base station."""
diff --git a/cflib/localization/lighthouse_utils.py b/cflib/localization/lighthouse_utils.py
index c374878ea..9e614e7ad 100644
--- a/cflib/localization/lighthouse_utils.py
+++ b/cflib/localization/lighthouse_utils.py
@@ -21,12 +21,11 @@
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
 from __future__ import annotations
 
-import math
-
 import numpy as np
 import numpy.typing as npt
 
-from cflib.localization.lighthouse_bs_vector import LighthouseBsVector, LighthouseBsVectors
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_types import Pose
 
 
@@ -146,7 +145,7 @@ def max_distance_all_permutations(cls, bs_angles: list[tuple[Pose, LighthouseBsV
             float: The maximum distance between the beams from all permutations of base stations.
         """
         if len(bs_angles) < 2:
-            raise ValueError("At least two base stations are required to calculate the maximum distance.")
+            raise ValueError('At least two base stations are required to calculate the maximum distance.')
 
         max_distance = 0.0
         bs_count = len(bs_angles)
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index f3696b57e..2f63700c4 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -44,7 +44,6 @@
 from __future__ import annotations
 
 import logging
-import pickle
 import time
 from threading import Event
 
@@ -214,14 +213,15 @@ def visualize(poses: LhBsCfPoses):
 
 def write_to_file(name: str | None, container: LhGeoInputContainer):
     if name:
-        with open(name, 'wb') as handle:
-            data = container.get_data_copy()
-            pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL)
+        with open(name, 'w', encoding='UTF8') as handle:
+            container.save_as_yaml_file(handle)
 
 
 def load_from_file(name: str) -> LhGeoInputContainerData:
-    with open(name, 'rb') as handle:
-        return pickle.load(handle)
+    container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
+    with open(name, 'r', encoding='UTF8') as handle:
+        container.populate_from_file_yaml(handle)
+        return container.get_data_copy()
 
 
 def print_solution(solution: LighthouseGeometrySolution):
@@ -391,7 +391,7 @@ def user_action_cb():
 
     # Set a file name to write the measurement data to file. Useful for debugging
     file_name = None
-    # file_name = 'lh_geo_estimate_data.pickle'
+    file_name = 'lh_geo_estimate_data.yaml'
 
     connect_and_estimate(uri, file_name=file_name)
 
diff --git a/test/localization/test_lighthouse_bs_vector.py b/test/localization/test_lighthouse_bs_vector.py
index 8d490cd91..9d0eb0e85 100644
--- a/test/localization/test_lighthouse_bs_vector.py
+++ b/test/localization/test_lighthouse_bs_vector.py
@@ -22,6 +22,7 @@
 from test.localization.lighthouse_test_base import LighthouseTestBase
 
 import numpy as np
+import yaml
 
 from cflib.localization import LighthouseBsVector
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
@@ -162,3 +163,71 @@ def test_conversion_to_angle_list(self):
 
         # Assert
         self.assertVectorsAlmostEqual((0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7), actual)
+
+    def test_LighthouseBsVector_equality(self):
+        # Fixture
+        vec1 = LighthouseBsVector(0.0, 1.0)
+        vec2 = LighthouseBsVector(0.1, 1.1)
+        vec3 = LighthouseBsVector(0.1, 1.1)
+
+        # Test
+        # Assert
+        self.assertNotEqual(vec1, vec2)
+        self.assertEqual(vec2, vec3)
+
+    def test_LighthouseBsVectors_equality(self):
+        # Fixture
+        vectors1 = LighthouseBsVectors((
+            LighthouseBsVector(0.1, 0.1),
+            LighthouseBsVector(0.2, 0.2),
+            LighthouseBsVector(0.3, 0.3),
+            LighthouseBsVector(0.4, 0.4),
+        ))
+
+        vectors2 = LighthouseBsVectors((
+            LighthouseBsVector(0.0, 0.1),
+            LighthouseBsVector(0.2, 0.3),
+            LighthouseBsVector(0.4, 0.5),
+            LighthouseBsVector(0.6, 0.7),
+        ))
+
+        vectors3 = LighthouseBsVectors((
+            LighthouseBsVector(0.0, 0.1),
+            LighthouseBsVector(0.2, 0.3),
+            LighthouseBsVector(0.4, 0.5),
+            LighthouseBsVector(0.6, 0.7),
+        ))
+
+        # Test
+        # Assert
+        self.assertNotEqual(vectors1, vectors2)
+        self.assertEqual(vectors2, vectors3)
+
+    def test_LighthouseBsVector_yaml(self):
+        # Fixture
+        expected = LighthouseBsVector(0.1, 1.1)
+
+        # Test
+        yaml_str = yaml.dump(expected)
+        actual = yaml.load(yaml_str, Loader=yaml.FullLoader)
+
+        # Assert
+        self.assertTrue(yaml_str.startswith('!LighthouseBsVector'))
+        self.assertEqual(expected, actual)
+
+    def test_LighthouseBsVectors_yaml(self):
+        # Fixture
+        expected = LighthouseBsVectors((
+            LighthouseBsVector(0.1, 0.1),
+            LighthouseBsVector(0.2, 0.2),
+            LighthouseBsVector(0.3, 0.3),
+            LighthouseBsVector(0.4, 0.4),
+        ))
+
+        # Test
+        yaml_str = yaml.dump(expected)
+        actual = yaml.load(yaml_str, Loader=yaml.FullLoader)
+
+        # Assert
+        self.assertTrue(yaml_str.startswith('!LighthouseBsVectors'))
+        self.assertEqual(expected, actual)
diff --git a/test/localization/test_lighthouse_cf_pose_sample.py b/test/localization/test_lighthouse_cf_pose_sample.py
new file mode 100644
index 000000000..0d9c8c5f2
--- /dev/null
+++ b/test/localization/test_lighthouse_cf_pose_sample.py
@@ -0,0 +1,59 @@
+# -*- coding: utf-8 -*-
+#
+# ,---------,       ____  _ __
+# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+# Copyright (C) 2025 Bitcraze AB
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, in version 3.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <http://www.gnu.org/licenses/>.
+import yaml
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from test.localization.lighthouse_test_base import LighthouseTestBase
+
+
+class TestLhCfPoseSample(LighthouseTestBase):
+    def setUp(self):
+        self.vec1 = LighthouseBsVector(0.0, 1.0)
+        self.vec2 = LighthouseBsVector(0.1, 1.1)
+        self.vec3 = LighthouseBsVector(0.2, 1.2)
+        self.vec4 = LighthouseBsVector(0.3, 1.3)
+
+        self.sample1 = LhCfPoseSample({})
+        self.sample2 = LhCfPoseSample({3: LighthouseBsVectors([self.vec1, self.vec2, self.vec3, self.vec4])})
+        self.sample3 = LhCfPoseSample({3: LighthouseBsVectors([self.vec4, self.vec3, self.vec2, self.vec1])})
+        self.sample4 = LhCfPoseSample({3: LighthouseBsVectors([self.vec4, self.vec3, self.vec2, self.vec1])})
+
+    def test_equality(self):
+        # Fixture
+        # Test
+        # Assert
+        self.assertEqual(self.sample3, self.sample4)
+        self.assertNotEqual(self.sample1, self.sample4)
+        self.assertNotEqual(self.sample2, self.sample4)
+
+    def test_yaml(self):
+        # Fixture
+        expected = self.sample3
+
+        # Test
+        yaml_str = yaml.dump(expected)
+        actual = yaml.load(yaml_str, Loader=yaml.FullLoader)
+
+        # Assert
+        self.assertTrue(yaml_str.startswith('!LhCfPoseSample'))
+        self.assertEqual(expected, actual)
diff --git a/test/localization/test_lighthouse_types.py b/test/localization/test_lighthouse_types.py
index dacc2e27b..dd63285b2 100644
--- a/test/localization/test_lighthouse_types.py
+++ b/test/localization/test_lighthouse_types.py
@@ -22,6 +22,7 @@
 from test.localization.lighthouse_test_base import LighthouseTestBase
 
 import numpy as np
+import yaml
 
 from cflib.localization.lighthouse_types import Pose
 
@@ -96,3 +97,26 @@ def test_rotate_translate_pose_and_back(self):
 
         # Assert
         self.assertPosesAlmostEqual(expected, actual)
+
+    def test_pose_equality(self):
+        # Fixture
+        pose1 = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3))
+        pose2 = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3))
+        pose3 = Pose.from_rot_vec(R_vec=(4.0, 5.0, 6.0), t_vec=(7.0, 8.0, 9.0))
+
+        # Test
+        # Assert
+        self.assertEqual(pose1, pose2)
+        self.assertNotEqual(pose1, pose3)
+
+    def test_pose_yaml(self):
+        # Fixture
+        expected = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0), t_vec=(0.1, 0.2, 0.3))
+
+        # Test
+        yaml_str = yaml.dump(expected)
+        actual = yaml.load(yaml_str, Loader=yaml.FullLoader)
+
+        # Assert
+        self.assertTrue(yaml_str.startswith('!Pose'))
+        self.assertEqual(expected, actual)

From 09fbc48ca9263d53d90bda3e0aa5b22ca5af8f6d Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 4 Jul 2025 18:16:52 +0200
Subject: [PATCH 36/36] Added session management

---
 .../lighthouse_geo_estimation_manager.py      | 153 ++++++++++++++----
 .../multi_bs_geometry_estimation.py           |  11 +-
 .../test_lighthouse_cf_pose_sample.py         |   6 +-
 3 files changed, 123 insertions(+), 47 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index b85f39435..76eaa675e 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -22,6 +22,9 @@
 from __future__ import annotations
 
 import copy
+import datetime
+import os
+import pathlib
 import threading
 from typing import TextIO
 
@@ -279,6 +282,17 @@ def get_matched_samples(self) -> list[LhCfPoseSample]:
         """
         return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
 
+    def is_empty(self) -> bool:
+        """Check if the container is empty, meaning no samples are set
+
+        Returns:
+            bool: True if the container is empty, False otherwise
+        """
+        return (len(self.x_axis) == 0 and
+                len(self.xy_plane) == 0 and
+                len(self.xyz_space) == 0 and
+                self.origin == self.EMPTY_POSE_SAMPLE)
+
     @staticmethod
     def yaml_representer(dumper, data: LhGeoInputContainerData):
         return dumper.represent_mapping('!LhGeoInputContainerData', {
@@ -328,15 +342,20 @@ def __init__(self, sensor_positions: ArrayFloat) -> None:
         self._data = LhGeoInputContainerData(sensor_positions)
         self.is_modified_condition = threading.Condition()
 
+        self._session_name = None
+        self._session_path = os.getcwd()
+        self._auto_save = False
+
     def set_origin_sample(self, origin: LhCfPoseSample) -> None:
         """Store/update the sample to be used for the origin
 
         Args:
             origin (LhCfPoseSample): the new origin
         """
-        self._data.origin = origin
-        self._augment_sample(self._data.origin, True)
-        self._update_version()
+        with self.is_modified_condition:
+            self._data.origin = origin
+            self._augment_sample(self._data.origin, True)
+            self._handle_data_modification()
 
     def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         """Store/update the sample to be used for the x_axis
@@ -344,9 +363,10 @@ def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         Args:
             x_axis (LhCfPoseSample): the new x-axis sample
         """
-        self._data.x_axis = [x_axis]
-        self._augment_samples(self._data.x_axis, True)
-        self._update_version()
+        with self.is_modified_condition:
+            self._data.x_axis = [x_axis]
+            self._augment_samples(self._data.x_axis, True)
+            self._handle_data_modification()
 
     def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         """Store/update the samples to be used for the xy-plane
@@ -354,9 +374,10 @@ def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         Args:
             xy_plane (list[LhCfPoseSample]): the new xy-plane samples
         """
-        self._data.xy_plane = xy_plane
-        self._augment_samples(self._data.xy_plane, True)
-        self._update_version()
+        with self.is_modified_condition:
+            self._data.xy_plane = xy_plane
+            self._augment_samples(self._data.xy_plane, True)
+            self._handle_data_modification()
 
     def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
         """append to the samples to be used for the xy-plane
@@ -364,9 +385,10 @@ def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
         Args:
             xy_plane (LhCfPoseSample): the new xy-plane sample
         """
-        self._augment_sample(xy_plane, True)
-        self._data.xy_plane.append(xy_plane)
-        self._update_version()
+        with self.is_modified_condition:
+            self._augment_sample(xy_plane, True)
+            self._data.xy_plane.append(xy_plane)
+            self._handle_data_modification()
 
     def xy_plane_sample_count(self) -> int:
         """Get the number of samples in the xy-plane
@@ -374,7 +396,8 @@ def xy_plane_sample_count(self) -> int:
         Returns:
             int: The number of samples in the xy-plane
         """
-        return len(self._data.xy_plane)
+        with self.is_modified_condition:
+            return len(self._data.xy_plane)
 
     def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """Store/update the samples for the volume
@@ -382,8 +405,12 @@ def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         Args:
             samples (list[LhMeasurement]): the new samples
         """
-        self._data.xyz_space = []
-        self.append_xyz_space_samples(samples)
+        new_samples = samples
+        self._augment_samples(new_samples, False)
+        with self.is_modified_condition:
+            self._data.xyz_space = []
+            self.append_xyz_space_samples(new_samples)
+            self._handle_data_modification()
 
     def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """Append to the samples for the volume
@@ -393,8 +420,9 @@ def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
         """
         new_samples = samples
         self._augment_samples(new_samples, False)
-        self._data.xyz_space += new_samples
-        self._update_version()
+        with self.is_modified_condition:
+            self._data.xyz_space += new_samples
+            self._handle_data_modification()
 
     def xyz_space_sample_count(self) -> int:
         """Get the number of samples in the xyz space
@@ -402,7 +430,8 @@ def xyz_space_sample_count(self) -> int:
         Returns:
             int: The number of samples in the xyz space
         """
-        return len(self._data.xyz_space)
+        with self.is_modified_condition:
+            return len(self._data.xyz_space)
 
     def clear_all_samples(self) -> None:
         """Clear all samples in the container"""
@@ -414,7 +443,8 @@ def get_data_version(self) -> int:
         Returns:
             int: The current data version
         """
-        return self._data.version
+        with self.is_modified_condition:
+            return self._data.version
 
     def get_data_copy(self) -> LhGeoInputContainerData:
         """Get a copy of the data in the container
@@ -422,38 +452,75 @@ def get_data_copy(self) -> LhGeoInputContainerData:
         Returns:
             LhGeoInputContainerData: A copy of the data in the container
         """
-        return copy.deepcopy(self._data)
+        with self.is_modified_condition:
+            return copy.deepcopy(self._data)
+
+    def is_empty(self) -> bool:
+        """Check if the container is empty
 
-    def save_as_yaml_file(self, text_io: TextIO):
-        """Get the data in the container as a YAML string suitable for saving to a file
         Returns:
-            str: The data in the container as a YAML string
+            bool: True if the container is empty, False otherwise
+        """
+        with self.is_modified_condition:
+            return self._data.is_empty()
+
+    def save_as_yaml_file(self, text_io: TextIO):
+        """Save the data container as a YAML file
+
+        Args:
+            text_io (TextIO): The text IO stream to write the YAML data to
         """
-        data = {
-            'file_type_version': self.FILE_TYPE_VERSION,
-            'data': self._data
+        with self.is_modified_condition:
+            self.save_data_container_as_yaml(self._data, text_io)
+
+    @classmethod
+    def save_data_container_as_yaml(cls, container_data: LhGeoInputContainerData, text_io: TextIO):
+        """Save the data container as a YAML string suitable for saving to a file
+
+        Args:
+            container_data (LhGeoInputContainerData): The data container to save
+            text_io (TextIO): The text IO stream to write the YAML data to
+        """
+        file_data = {
+            'file_type_version': cls.FILE_TYPE_VERSION,
+            'data': container_data
         }
-        yaml.dump(data, text_io, default_flow_style=False)
+        yaml.dump(file_data, text_io, default_flow_style=False)
 
     def populate_from_file_yaml(self, text_io: TextIO) -> None:
         """Load the data from file
 
         Args:
-            wrapper
+            text_io (TextIO): The text IO stream to read the YAML data from
+        Raises:
+            ValueError: If the file type version is not supported
         """
         file_yaml = yaml.load(text_io, Loader=yaml.FullLoader)
         if file_yaml['file_type_version'] != self.FILE_TYPE_VERSION:
             raise ValueError(f'Unsupported file type version: {file_yaml["file_type_version"]}')
-        self._data = file_yaml['data']
-        self._update_version()
+        self._set_new_data_container(file_yaml['data'])
+
+    def enable_auto_save(self, session_path: str = os.getcwd()) -> None:
+        """Enable auto-saving of the session data to a file in the specified path.
+        Session files will be named with the current date and time.
+
+        Args:
+            session_path (str): The path to save the session data to. Defaults to the current working directory.
+        """
+        self._session_path = session_path
+        self._auto_save = True
 
     def _set_new_data_container(self, new_data: LhGeoInputContainerData) -> None:
         """Set a new data container and update the version"""
+
         # Maintain version
-        current_version = self._data.version
-        self._data = new_data
-        self._data.version = current_version
-        self._update_version()
+        with self.is_modified_condition:
+            current_version = self._data.version
+            self._data = new_data
+            self._data.version = current_version
+
+            self._new_session()
+            self._handle_data_modification()
 
     def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
         sample.augment_with_ippe(self._data.sensor_positions)
@@ -463,8 +530,24 @@ def _augment_samples(self, samples: list[LhCfPoseSample], is_mandatory: bool) ->
         for sample in samples:
             self._augment_sample(sample, is_mandatory)
 
-    def _update_version(self) -> None:
+    def _handle_data_modification(self) -> None:
         """Update the data version and notify the waiting thread"""
         with self.is_modified_condition:
             self._data.version += 1
             self.is_modified_condition.notify()
+
+        self._save_session()
+
+    def _save_session(self) -> None:
+        if self._auto_save and not self.is_empty():
+            if self._session_name is None:
+                self._session_name = datetime.datetime.now().isoformat(timespec='seconds')
+
+            file_name = os.path.join(self._session_path, f'lh_geo_{self._session_name}.yaml')
+            pathlib.Path(self._session_path).mkdir(parents=True, exist_ok=True)
+            with open(file_name, 'w', encoding='UTF8') as handle:
+                self.save_as_yaml_file(handle)
+
+    def _new_session(self) -> None:
+        """Start a new session"""
+        self._session_name = None
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 2f63700c4..bea4f49a1 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -211,12 +211,6 @@ def visualize(poses: LhBsCfPoses):
         plt.show()
 
 
-def write_to_file(name: str | None, container: LhGeoInputContainer):
-    if name:
-        with open(name, 'w', encoding='UTF8') as handle:
-            container.save_as_yaml_file(handle)
-
-
 def load_from_file(name: str) -> LhGeoInputContainerData:
     container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
     with open(name, 'r', encoding='UTF8') as handle:
@@ -321,6 +315,7 @@ def connect_and_estimate(uri: str, file_name: str | None = None):
     print(f'Step 1. Connecting to the Crazyflie on uri {uri}...')
     with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
         container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
+        container.enable_auto_save('lh_geo_sessions')
         print('Starting geometry estimation thread...')
 
         def _local_solution_handler(solution: LighthouseGeometrySolution):
@@ -339,17 +334,14 @@ def _local_solution_handler(solution: LighthouseGeometrySolution):
         print('Step 2. Put the Crazyflie where you want the origin of your coordinate system.')
 
         container.set_origin_sample(get_recording(scf))
-        write_to_file(file_name, container)
 
         print(f'Step 3. Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the origin. ' +
               'This position defines the direction of the X-axis, but it is also used for scaling the system.')
         container.set_x_axis_sample(get_recording(scf))
-        write_to_file(file_name, container)
 
         print('Step 4. Put the Crazyflie somewhere in the XY-plane, but not on the X-axis.')
         print('Multiple samples can be recorded if you want to.')
         container.set_xy_plane_samples(get_multiple_recordings(scf))
-        write_to_file(file_name, container)
 
         print()
         print('Step 5. We will now record data from the space you plan to fly in and optimize the base station ' +
@@ -361,7 +353,6 @@ def matched_angles_cb(sample: LhCfPoseSample):
             scf.cf.platform.send_user_notification(True)
             container.append_xyz_space_samples([sample])
             scf.cf.platform.send_user_notification()
-            write_to_file(file_name, container)
 
         def timeout_cb():
             print('Timeout, no angles received. Please try again.')
diff --git a/test/localization/test_lighthouse_cf_pose_sample.py b/test/localization/test_lighthouse_cf_pose_sample.py
index 0d9c8c5f2..a6d4e61f1 100644
--- a/test/localization/test_lighthouse_cf_pose_sample.py
+++ b/test/localization/test_lighthouse_cf_pose_sample.py
@@ -19,11 +19,13 @@
 #
 # You should have received a copy of the GNU General Public License
 # along with this program. If not, see <http://www.gnu.org/licenses/>.
+from test.localization.lighthouse_test_base import LighthouseTestBase
+
 import yaml
-from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
+
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
+from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
-from test.localization.lighthouse_test_base import LighthouseTestBase
 
 
 class TestLhCfPoseSample(LighthouseTestBase):