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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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/55] 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):

From f3d91c95a532ae59ad350109a5ce6260b79617e8 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 8 Jul 2025 12:31:19 +0200
Subject: [PATCH 37/55] Clean solution before adding new

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

diff --git a/cflib/localization/lighthouse_geometry_solver.py b/cflib/localization/lighthouse_geometry_solver.py
index ffa0e3e6e..fe722bb2a 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -383,6 +383,9 @@ def _condense_results(cls, lsq_result, defs: SolverData, matched_samples: list[L
                           solution: LighthouseGeometrySolution) -> None:
         bss, cf_poses = cls._params_to_struct(lsq_result.x, defs)
 
+        # Clean previous solution
+        solution.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
+
         # Extract CF pose estimates
         # First pose (origin) is not in the parameter list
         solution.poses.cf_poses.append(Pose())

From b57ddc60479060a84a9c0a43a8c424549525a3be Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 8 Jul 2025 12:31:44 +0200
Subject: [PATCH 38/55] Improved information

---
 cflib/localization/__init__.py                     | 4 +++-
 cflib/localization/lighthouse_initial_estimator.py | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py
index 0d873d0d7..eff3da0cf 100644
--- a/cflib/localization/__init__.py
+++ b/cflib/localization/__init__.py
@@ -23,6 +23,7 @@
 from .lighthouse_bs_vector import LighthouseBsVector
 from .lighthouse_config_manager import LighthouseConfigFileManager
 from .lighthouse_config_manager import LighthouseConfigWriter
+from .lighthouse_geometry_solution import LighthouseGeometrySolution
 from .lighthouse_sweep_angle_reader import LighthouseMatchedSweepAngleReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader
 from .lighthouse_sweep_angle_reader import LighthouseSweepAngleReader
@@ -38,4 +39,5 @@
     'LighthouseConfigFileManager',
     'LighthouseConfigWriter',
     'ParamFileManager',
-    'LighthouseCrossingBeam']
+    'LighthouseCrossingBeam',
+    'LighthouseGeometrySolution']
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 6be1a2fca..62185a6ed 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -191,6 +191,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], bs_positions: d
         result: list[dict[int, Pose]] = []
 
         cleaned_matched_samples: list[LhCfPoseSample] = []
+        outlier_count = 0
 
         for sample in matched_samples:
             solutions = sample.ippe_solutions
@@ -213,7 +214,8 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], bs_positions: d
                     if sample.is_mandatory:
                         solution.append_mandatory_issue_sample(sample, 'Outlier detected')
                     else:
-                        solution.xyz_space_samples_info = 'Sample(s) with outliers skipped'
+                        outlier_count += 1
+                        solution.xyz_space_samples_info = f'{outlier_count} sample(s) with outliers skipped'
                     break
 
             if is_sample_valid or sample.is_mandatory:

From 3f428abdc62cdc03edc6b9e89541dec79ff29ddb Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 14 Jul 2025 06:09:33 +0200
Subject: [PATCH 39/55] Added error information per sample in solution

---
 .../lighthouse_geo_estimation_manager.py             | 12 +++++++-----
 cflib/localization/lighthouse_geometry_solution.py   |  3 +++
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 76eaa675e..ea8ede6af 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -186,19 +186,21 @@ def _create_solution_stats(cls, matched_samples: list[LhCfPoseSample], solution:
         """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] = []
+        cf_error: 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)
+            cf_error.append(sample_error)
+
+        solution.cf_error = cf_error
 
         solution.error_stats = LighthouseGeometrySolution.ErrorStats(
-            mean=np.mean(estimated_errors),
-            max=np.max(estimated_errors),
-            std=np.std(estimated_errors)
+            mean=np.mean(cf_error),
+            max=np.max(cf_error),
+            std=np.std(cf_error)
         )
 
     class SolverThread(threading.Thread):
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index bbc531365..9ca8eb5b7 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -42,6 +42,9 @@ def __init__(self):
         # Information about errors in the solution
         self.error_stats = self.ErrorStats(0.0, 0.0, 0.0)
 
+        # A list of errors corresponding to the cf_poses in self.poses.
+        self.cf_error = []
+
         # Indicates if the solution converged (True).
         # If it did not converge, the solution is possibly not good enough to use
         self.has_converged = False

From d4be7fa65d67f099643c46788abe0930da993ade Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Mon, 14 Jul 2025 14:34:34 +0200
Subject: [PATCH 40/55] Reworked sample management and error reporting

---
 .../localization/lighthouse_cf_pose_sample.py |  85 ++++++++---
 .../lighthouse_geo_estimation_manager.py      | 144 ++++++++++--------
 .../lighthouse_geometry_solution.py           |  33 ++--
 .../lighthouse_geometry_solver.py             |  41 ++---
 .../lighthouse_initial_estimator.py           |  39 +++--
 .../localization/lighthouse_sample_matcher.py | 108 -------------
 .../localization/lighthouse_system_scaler.py  |   7 +-
 cflib/localization/lighthouse_types.py        |   6 -
 .../multi_bs_geometry_estimation.py           |  33 ----
 9 files changed, 206 insertions(+), 290 deletions(-)
 delete mode 100644 cflib/localization/lighthouse_sample_matcher.py

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 919dd3b72..c231c12df 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -1,3 +1,4 @@
+import enum
 from typing import NamedTuple
 
 import numpy as np
@@ -19,17 +20,13 @@ class BsPairPoses(NamedTuple):
 
 class LhCfPoseSample:
     """ Represents a sample of a Crazyflie pose in space, it contains:
-    - a timestamp (if applicable)
     - lighthouse angles from one or more base stations
     - 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, angles_calibrated: dict[int, LighthouseBsVectors], timestamp: float = 0.0,
-                 is_mandatory: bool = False) -> None:
-        self.timestamp: float = timestamp
-
+    def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors]) -> None:
         # 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
@@ -39,10 +36,6 @@ def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors], timestamp:
         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 = is_mandatory
-
     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)
@@ -84,26 +77,84 @@ 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)
+        return self.angles_calibrated == other.angles_calibrated
 
     @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)
+        return LhCfPoseSample(angles_calibrated)
 
 
 yaml.add_representer(LhCfPoseSample, LhCfPoseSample.yaml_representer)
 yaml.add_constructor('!LhCfPoseSample', LhCfPoseSample.yaml_constructor)
+
+
+@enum.unique
+class LhCfPoseSampleType(enum.Enum):
+    """An enum representing the type of a pose sample"""
+    ORIGIN = 'origin'
+    X_AXIS = 'x-axis'
+    XY_PLANE = 'xy-plane'
+    XYZ_SPACE = 'xyz-space'
+
+    def __str__(self):
+        return self.value
+
+
+@enum.unique
+class LhCfPoseSampleStatus(enum.Enum):
+    """An enum representing the status of a pose sample"""
+    OK = 'OK'
+    TOO_FEW_BS = 'Too few bs'
+    OUTLIER = 'Outlier'
+    NO_DATA = 'No data'
+
+    def __str__(self):
+        return self.value
+
+
+class LhCfPoseSampleWrapper():
+    """A wrapper of LhCfPoseSample that includes more information, useful in the estimation process and in a UI."""
+
+    NO_POSE = Pose()
+
+    def __init__(self, pose_sample: LhCfPoseSample,
+                 sample_type: LhCfPoseSampleType = LhCfPoseSampleType.XYZ_SPACE) -> None:
+        self.pose_sample: LhCfPoseSample = pose_sample
+
+        self.sample_type = sample_type
+
+        # 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 = self.sample_type in (LhCfPoseSampleType.ORIGIN,
+                                                 LhCfPoseSampleType.X_AXIS,
+                                                 LhCfPoseSampleType.XY_PLANE)
+
+        self.status = LhCfPoseSampleStatus.OK
+
+        self.pose: Pose = self.NO_POSE  # The pose of the sample, if available
+        self.error_distance: float = 0.0  # The error distance of the pose, if available
+
+    @property
+    def angles_calibrated(self) -> dict[int, LighthouseBsVectors]:
+        return self.pose_sample.angles_calibrated
+
+    @property
+    def ippe_solutions(self) -> dict[int, BsPairPoses]:
+        return self.pose_sample.ippe_solutions
+
+    @property
+    def is_valid(self) -> bool:
+        return self.status == LhCfPoseSampleStatus.OK
+
+    @property
+    def base_station_ids(self) -> list[int]:
+        """Get the base station ids of the sample"""
+        return list(self.angles_calibrated.keys())
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index ea8ede6af..da673efb1 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -33,12 +33,14 @@
 import yaml
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleStatus
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleType
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleWrapper
 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_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
 
 
@@ -49,21 +51,25 @@ class LhGeoEstimationManager():
     REFERENCE_DIST = 1.0  # Reference distance used for scaling the solution
 
     @classmethod
-    def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhBsCfPoses,
-                                 reference_distance: float) -> LhBsCfPoses:
+    def align_and_scale_solution(cls, container: LhGeoInputContainerData, solution: LighthouseGeometrySolution,
+                                 samples: list[LhCfPoseSampleWrapper], reference_distance: float):
+
+        # Note: samples is a subset of solution.samples
+        bs_poses = solution.bs_poses
+
         start_idx_x_axis = 1
         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
-        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_xyz_space]
-        xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses))
+        origin_pos = samples[0].pose.translation
+        x_axis_samples = samples[start_idx_x_axis:start_idx_x_axis + len(container.x_axis)]
+        x_axis_pos = list(map(lambda x: x.pose.translation, x_axis_samples))
+        xy_plane_samples = samples[start_idx_xy_plane:start_idx_xyz_space]
+        xy_plane_pos = list(map(lambda x: x.pose.translation, xy_plane_samples))
 
         # 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))
+        bs_aligned_poses, trnsfrm = LighthouseSystemAligner.align(origin_pos, x_axis_pos, xy_plane_pos, bs_poses)
+        cf_aligned_poses = list(map(lambda sample: trnsfrm.rotate_translate_pose(sample.pose), samples))
 
         # Scale the solution
         bs_scaled_poses, cf_scaled_poses, scale = LighthouseSystemScaler.scale_fixed_point(bs_aligned_poses,
@@ -71,28 +77,28 @@ def align_and_scale_solution(cls, container: LhGeoInputContainerData, poses: LhB
                                                                                            [reference_distance, 0, 0],
                                                                                            cf_aligned_poses[1])
 
-        return LhBsCfPoses(bs_poses=bs_scaled_poses, cf_poses=cf_scaled_poses)
+        # Update the solution with the aligned and scaled poses
+        solution.bs_poses = bs_scaled_poses
+        for sample, pose in zip(samples, cf_scaled_poses):
+            sample.pose = pose
 
     @classmethod
     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()
+        solution = LighthouseGeometrySolution(samples=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
+            cleaned_matched_samples = LighthouseInitialEstimator.estimate(validated_matched_samples, solution)
             if solution.progress_is_ok:
                 solution.progress_info = 'Refining geometry solution'
-                LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, container.sensor_positions,
-                                               solution)
+                LighthouseGeometrySolver.solve(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
+                cls.align_and_scale_solution(container, solution, validated_matched_samples,
+                                             cls.REFERENCE_DIST)
 
                 cls._create_solution_stats(validated_matched_samples, solution)
 
@@ -103,21 +109,30 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
         return solution
 
     @classmethod
-    def _data_validation(cls, matched_samples: list[LhCfPoseSample], container: LhGeoInputContainerData,
-                         solution: LighthouseGeometrySolution) -> list[LhCfPoseSample]:
+    def _data_validation(cls, matched_samples: list[LhCfPoseSampleWrapper], container: LhGeoInputContainerData,
+                         solution: LighthouseGeometrySolution) -> list[LhCfPoseSampleWrapper]:
         """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)
+        if len(matched_samples) == 0:
+            solution.is_origin_sample_valid = False
+            solution.origin_sample_info = NO_DATA
+            solution.progress_is_ok = False
+            return result
+        else:
+            origin = matched_samples[0]
+            if len(origin.angles_calibrated) == 0:
+                origin.status = LhCfPoseSampleStatus.NO_DATA
+                solution.progress_is_ok = False
+            elif len(origin.angles_calibrated) < 2:
+                origin.status = LhCfPoseSampleStatus.TOO_FEW_BS
+                solution.progress_is_ok = False
+
+            result.append(origin)
 
         # Check the x-axis samples
         if len(container.x_axis) == 0:
@@ -134,21 +149,16 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSample], container: LhGe
             solution.xyz_space_samples_info = NO_DATA
 
         # 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
-
+        for sample in matched_samples[1:]:
             if len(sample.angles_calibrated) >= 2:
                 result.append(sample)
             else:
+                sample.status = LhCfPoseSampleStatus.TOO_FEW_BS
+
                 # 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
+                    result.append(sample)
+                    solution.progress_is_ok = False
 
         return result
 
@@ -158,23 +168,22 @@ def _humanize_error_info(cls, solution: LighthouseGeometrySolution, container: L
 
         # 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])
+            solution.is_origin_sample_valid, solution.origin_sample_info = cls._error_info_for(
+                solution, LhCfPoseSampleType.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)
+            solution.is_x_axis_samples_valid, solution.x_axis_samples_info = cls._error_info_for(
+                solution, LhCfPoseSampleType.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)
+            solution.is_xy_plane_samples_valid, solution.xy_plane_samples_info = cls._error_info_for(
+                solution, LhCfPoseSampleType.XY_PLANE)
 
     @classmethod
-    def _error_info_for(cls, solution: LighthouseGeometrySolution, samples: list[LhCfPoseSample]) -> tuple[bool, str]:
+    def _error_info_for(cls, solution: LighthouseGeometrySolution, sample_type: LhCfPoseSampleType) -> 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)
+        for sample in solution.samples:
+            if sample.sample_type == sample_type and sample.status != LhCfPoseSampleStatus.OK:
+                info_strings.append(f'{sample.status}')
 
         if len(info_strings) > 0:
             return False, ', '.join(info_strings)
@@ -182,7 +191,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):
+    def _create_solution_stats(cls, matched_samples: list[LhCfPoseSampleWrapper], solution: LighthouseGeometrySolution):
         """Calculate statistics about the solution and store them in the solution object"""
 
         # Estimated worst error for each sample based on crossing beams
@@ -191,12 +200,11 @@ def _create_solution_stats(cls, matched_samples: list[LhCfPoseSample], solution:
         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]
+            bs_angle_list = [(solution.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)
+            sample.error_distance = sample_error
             cf_error.append(sample_error)
 
-        solution.cf_error = cf_error
-
         solution.error_stats = LighthouseGeometrySolution.ErrorStats(
             mean=np.mean(cf_error),
             max=np.max(cf_error),
@@ -276,13 +284,18 @@ def __init__(self, sensor_positions: ArrayFloat, version: int = 0) -> None:
         # Used by LhGeoInputContainer to track changes in the data
         self.version = version
 
-    def get_matched_samples(self) -> list[LhCfPoseSample]:
+    def get_matched_samples(self) -> list[LhCfPoseSampleWrapper]:
         """Get all pose samples collected in a list
 
         Returns:
-            list[LhCfPoseSample]: _description_
+            list[LhCfPoseSampleWrapper]: A list of pose samples wrapped in LhCfPoseSampleWrapper
         """
-        return [self.origin] + self.x_axis + self.xy_plane + self.xyz_space
+        result = [LhCfPoseSampleWrapper(self.origin, sample_type=LhCfPoseSampleType.ORIGIN)]
+        result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.X_AXIS) for sample in self.x_axis]
+        result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.XY_PLANE) for sample in self.xy_plane]
+        result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.XYZ_SPACE) for sample in self.xyz_space]
+
+        return result
 
     def is_empty(self) -> bool:
         """Check if the container is empty, meaning no samples are set
@@ -356,7 +369,7 @@ def set_origin_sample(self, origin: LhCfPoseSample) -> None:
         """
         with self.is_modified_condition:
             self._data.origin = origin
-            self._augment_sample(self._data.origin, True)
+            self._augment_sample(self._data.origin)
             self._handle_data_modification()
 
     def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
@@ -367,7 +380,7 @@ def set_x_axis_sample(self, x_axis: LhCfPoseSample) -> None:
         """
         with self.is_modified_condition:
             self._data.x_axis = [x_axis]
-            self._augment_samples(self._data.x_axis, True)
+            self._augment_samples(self._data.x_axis)
             self._handle_data_modification()
 
     def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
@@ -378,7 +391,7 @@ def set_xy_plane_samples(self, xy_plane: list[LhCfPoseSample]) -> None:
         """
         with self.is_modified_condition:
             self._data.xy_plane = xy_plane
-            self._augment_samples(self._data.xy_plane, True)
+            self._augment_samples(self._data.xy_plane)
             self._handle_data_modification()
 
     def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
@@ -388,7 +401,7 @@ def append_xy_plane_sample(self, xy_plane: LhCfPoseSample) -> None:
             xy_plane (LhCfPoseSample): the new xy-plane sample
         """
         with self.is_modified_condition:
-            self._augment_sample(xy_plane, True)
+            self._augment_sample(xy_plane)
             self._data.xy_plane.append(xy_plane)
             self._handle_data_modification()
 
@@ -408,7 +421,7 @@ def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
             samples (list[LhMeasurement]): the new samples
         """
         new_samples = samples
-        self._augment_samples(new_samples, False)
+        self._augment_samples(new_samples)
         with self.is_modified_condition:
             self._data.xyz_space = []
             self.append_xyz_space_samples(new_samples)
@@ -421,7 +434,7 @@ def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
             samples (LhMeasurement): the new samples
         """
         new_samples = samples
-        self._augment_samples(new_samples, False)
+        self._augment_samples(new_samples)
         with self.is_modified_condition:
             self._data.xyz_space += new_samples
             self._handle_data_modification()
@@ -524,13 +537,12 @@ def _set_new_data_container(self, new_data: LhGeoInputContainerData) -> None:
             self._new_session()
             self._handle_data_modification()
 
-    def _augment_sample(self, sample: LhCfPoseSample, is_mandatory: bool) -> None:
+    def _augment_sample(self, sample: LhCfPoseSample) -> 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:
+    def _augment_samples(self, samples: list[LhCfPoseSample]) -> None:
         for sample in samples:
-            self._augment_sample(sample, is_mandatory)
+            self._augment_sample(sample)
 
     def _handle_data_modification(self) -> None:
         """Update the data version and notify the waiting thread"""
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index 9ca8eb5b7..24af24a5e 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -24,8 +24,8 @@
 # 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
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleWrapper
+from cflib.localization.lighthouse_types import Pose
 
 
 class LighthouseGeometrySolution:
@@ -35,16 +35,19 @@ class LighthouseGeometrySolution:
 
     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=[])
+    def __init__(self, samples: list[LhCfPoseSampleWrapper]):
+        # The samples used to estimate the geometry of the system. The samples are wrapped in
+        # LhCfPoseSampleWrapper to provide additional information about the sample type and status. The status can be
+        # altered during the solution process to indicate if the sample is valid or not.
+        # The estimated pose of the CF is also stored in the wrapper.
+        self.samples = samples
+
+        # The estimated poses of the base stations. The keys are the base station ids and the values are the poses.
+        self.bs_poses: dict[int, Pose] = {}
 
         # Information about errors in the solution
         self.error_stats = self.ErrorStats(0.0, 0.0, 0.0)
 
-        # A list of errors corresponding to the cf_poses in self.poses.
-        self.cf_error = []
-
         # Indicates if the solution converged (True).
         # If it did not converge, the solution is possibly not good enough to use
         self.has_converged = False
@@ -66,10 +69,6 @@ def __init__(self):
         # 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]] = []
-
         # General failure information if the problem is not related to a specific sample
         self.general_failure_info = ''
 
@@ -77,13 +76,3 @@ def __init__(self):
         # 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.
-
-        :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 fe722bb2a..c3c352c30 100644
--- a/cflib/localization/lighthouse_geometry_solver.py
+++ b/cflib/localization/lighthouse_geometry_solver.py
@@ -25,10 +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 LhCfPoseSampleWrapper
 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 SolverData:
@@ -117,7 +116,7 @@ class LighthouseGeometrySolver:
     """
 
     @classmethod
-    def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample],
+    def solve(cls, matched_samples: list[LhCfPoseSampleWrapper],
               sensor_positions: npt.ArrayLike, solution: LighthouseGeometrySolution) -> None:
         """
         Solve for the pose of base stations and CF samples.
@@ -127,23 +126,26 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
         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.
+        :param matched_samples: List of matched samples. Note: matched_samples is a subset of solution.samples.
+
         :param sensor_positions: Sensor positions (3D), in the CF reference frame
         :param solution: an instance of LighthouseGeometrySolution that is filled with the result
         """
+        initial_guess_bs_poses = solution.bs_poses
+        initial_guess_cf_poses = [sample.pose for sample in matched_samples]
+
         defs = SolverData()
 
-        defs.n_bss = len(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)
+        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, defs)
-        params_bs, params_cfs = cls._populate_initial_guess(initial_guess, defs)
+        params_bs, params_cfs = cls._populate_initial_guess(initial_guess_bs_poses, initial_guess_cf_poses, defs)
 
         # Extra arguments passed on to calc_residual()
         args = (defs, idx_agl_pr_to_bs, idx_agl_pr_to_cf, idx_agl_pr_to_sens_pos, target_angles, sensor_positions)
@@ -164,7 +166,7 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
         cls._condense_results(result, defs, matched_samples, solution)
 
     @classmethod
-    def _populate_target_angles(cls, matched_samples: list[LhCfPoseSample]) -> npt.NDArray:
+    def _populate_target_angles(cls, matched_samples: list[LhCfPoseSampleWrapper]) -> npt.NDArray:
         """
         A np.array of all measured angles, the target angles
         """
@@ -176,7 +178,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: SolverData
+    def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSampleWrapper], 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),
@@ -236,18 +238,18 @@ def _populate_indexes_and_jacobian(cls, matched_samples: list[LhCfPoseSample], d
                 jac_sparsity)
 
     @classmethod
-    def _populate_initial_guess(cls, initial_guess: LhBsCfPoses,
+    def _populate_initial_guess(cls, initial_guess_bs_poses: dict[int, Pose], initial_guess_cf_poses: list[Pose],
                                 defs: SolverData) -> tuple[npt.NDArray, npt.NDArray]:
         """
         Generate parameters for base stations and CFs, this is the initial guess we start to iterate from.
         """
         params_bs = np.zeros((defs.n_bss, defs.n_params_per_bs))
-        for bs_id, pose in initial_guess.bs_poses.items():
+        for bs_id, pose in initial_guess_bs_poses.items():
             params_bs[defs.bs_id_to_index[bs_id], :] = cls._pose_to_params(pose)
 
         # Skip the first CF pose, it is the definition of the origin and is not a parameter
         params_cfs = np.zeros((defs.n_cfs_in_params, defs.n_params_per_cf))
-        for index, inital_est_pose in enumerate(initial_guess.cf_poses[1:]):
+        for index, inital_est_pose in enumerate(initial_guess_cf_poses[1:]):
             params_cfs[index, :] = cls._pose_to_params(inital_est_pose)
 
         return params_bs, params_cfs
@@ -379,22 +381,23 @@ 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, defs: SolverData, matched_samples: list[LhCfPoseSample],
+    def _condense_results(cls, lsq_result, defs: SolverData, matched_samples: list[LhCfPoseSampleWrapper],
                           solution: LighthouseGeometrySolution) -> None:
+        # Note: matched_samples is a subset of solution.samples.
+
         bss, cf_poses = cls._params_to_struct(lsq_result.x, defs)
 
-        # Clean previous solution
-        solution.poses = LhBsCfPoses(bs_poses={}, cf_poses=[])
+        solution.bs_poses = {}
 
         # Extract CF pose estimates
         # First pose (origin) is not in the parameter list
-        solution.poses.cf_poses.append(Pose())
+        matched_samples[0].pose = Pose()
         for i in range(len(matched_samples) - 1):
-            solution.poses.cf_poses.append(cls._params_to_pose(cf_poses[i], defs))
+            matched_samples[i + 1].pose = cls._params_to_pose(cf_poses[i], defs)
 
         # Extract base station pose estimates
         for index, pose in enumerate(bss):
             bs_id = defs.bs_index_to_id[index]
-            solution.poses.bs_poses[bs_id] = cls._params_to_pose(pose, defs)
+            solution.bs_poses[bs_id] = cls._params_to_pose(pose, defs)
 
         solution.has_converged = lsq_result.success
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 62185a6ed..4710972ff 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -27,9 +27,9 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_cf_pose_sample import BsPairPoses
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleStatus
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleWrapper
 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
 
@@ -53,8 +53,8 @@ class LighthouseInitialEstimator:
     OUTLIER_DETECTION_ERROR = 0.5
 
     @classmethod
-    def estimate(cls, matched_samples: list[LhCfPoseSample],
-                 solution: LighthouseGeometrySolution) -> tuple[LhBsCfPoses, list[LhCfPoseSample]]:
+    def estimate(cls, matched_samples: list[LhCfPoseSampleWrapper],
+                 solution: LighthouseGeometrySolution) -> list[LhCfPoseSampleWrapper]:
         """
         Make a rough estimate of the poses of all base stations and CF poses found in the samples.
 
@@ -62,10 +62,10 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
         global reference frame.
 
         :param matched_samples: A list of samples with lighthouse angles. It is assumed that all samples have data for
-                                two or more base stations.
+                                two or more base stations. Note: matched_samples is a subset of solution.samples.
+
         :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.
+        :return: a subset of the matched_samples where outliers are removed.
         """
 
         bs_positions = cls._find_bs_to_bs_poses(matched_samples)
@@ -75,7 +75,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
         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
+            return cleaned_matched_samples
 
         # Calculate the pose of all base stations, based on the pose of one base station
         try:
@@ -84,15 +84,20 @@ def estimate(cls, matched_samples: list[LhCfPoseSample],
             # 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
+            return 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
+        # Store the results in the solution
+        for pose, sample in zip(cf_poses, cleaned_matched_samples):
+            sample.pose = pose
+        solution.bs_poses = bs_poses
+
+        return cleaned_matched_samples
 
     @classmethod
-    def _build_link_stats(cls, matched_samples: list[LhCfPoseSample], solution: LighthouseGeometrySolution) -> None:
+    def _build_link_stats(cls, matched_samples: list[LhCfPoseSampleWrapper], solution: LighthouseGeometrySolution):
         """
         Build statistics about the number of links between base stations, based on the matched samples.
         :param matched_samples: List of matched samples
@@ -115,7 +120,7 @@ def increase_link_count(bs1: int, bs2: int):
                         increase_link_count(bs1, bs2)
 
     @classmethod
-    def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSample]) -> dict[BsPairIds, ArrayFloat]:
+    def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSampleWrapper]) -> dict[BsPairIds, ArrayFloat]:
         """
         Find the pose of all base stations, in the reference frame of other base stations.
 
@@ -174,8 +179,9 @@ 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],
-                         solution: LighthouseGeometrySolution) -> tuple[list[dict[int, Pose]], list[LhCfPoseSample]]:
+    def _angles_to_poses(cls, matched_samples: list[LhCfPoseSampleWrapper], bs_positions: dict[BsPairIds, ArrayFloat],
+                         solution: LighthouseGeometrySolution) -> tuple[list[dict[int, Pose]],
+                                                                        list[LhCfPoseSampleWrapper]]:
         """
         Estimate the base station poses in the Crazyflie reference frames, for each sample.
 
@@ -190,7 +196,7 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], bs_positions: d
         """
         result: list[dict[int, Pose]] = []
 
-        cleaned_matched_samples: list[LhCfPoseSample] = []
+        cleaned_matched_samples: list[LhCfPoseSampleWrapper] = []
         outlier_count = 0
 
         for sample in matched_samples:
@@ -211,8 +217,9 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSample], bs_positions: d
                     poses[pair_ids.bs2] = pair_poses.bs2
                 else:
                     is_sample_valid = False
+                    sample.status = LhCfPoseSampleStatus.OUTLIER
                     if sample.is_mandatory:
-                        solution.append_mandatory_issue_sample(sample, 'Outlier detected')
+                        solution.progress_is_ok = False
                     else:
                         outlier_count += 1
                         solution.xyz_space_samples_info = f'{outlier_count} sample(s) with outliers skipped'
diff --git a/cflib/localization/lighthouse_sample_matcher.py b/cflib/localization/lighthouse_sample_matcher.py
deleted file mode 100644
index ccc9c94d4..000000000
--- a/cflib/localization/lighthouse_sample_matcher.py
+++ /dev/null
@@ -1,108 +0,0 @@
-# -*- coding: utf-8 -*-
-#
-# ,---------,       ____  _ __
-# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
-# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
-# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
-#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
-#
-# Copyright (C) 2022 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
-
-from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
-from cflib.localization.lighthouse_types import LhMeasurement
-
-
-class LighthouseSampleMatcher:
-    """Utility class to match samples of measurements from multiple lighthouse base stations.
-
-    Assuming that the Crazyflie was moving when the measurements were recorded,
-    samples that were measured approximately at the same position are aggregated into
-    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 in a list
-        """
-
-        result = []
-        matcher = cls(max_time_diff, min_nr_of_bs_in_match)
-
-        for sample in samples:
-            pose_sample = matcher.match_one(sample)
-            if pose_sample is not None:
-                result.append(pose_sample)
-
-        pose_sample = matcher.purge()
-        if pose_sample is not None:
-            result.append(pose_sample)
-
-        return result
diff --git a/cflib/localization/lighthouse_system_scaler.py b/cflib/localization/lighthouse_system_scaler.py
index 8e9469328..37cf41a54 100644
--- a/cflib/localization/lighthouse_system_scaler.py
+++ b/cflib/localization/lighthouse_system_scaler.py
@@ -27,7 +27,7 @@
 import numpy.typing as npt
 
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
-from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleWrapper
 from cflib.localization.lighthouse_cf_pose_sample import Pose
 
 
@@ -53,7 +53,8 @@ def scale_fixed_point(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], expe
         return cls._scale_system(bs_poses, cf_poses, scale_factor)
 
     @classmethod
-    def scale_diagonals(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], matched_samples: list[LhCfPoseSample],
+    def scale_diagonals(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose],
+                        matched_samples: list[LhCfPoseSampleWrapper],
                         expected_diagonal: float) -> tuple[dict[int, Pose], list[Pose], float]:
         """
         Scale a system based on where base station "rays" intersects the lighthouse deck in relation to sensor
@@ -89,7 +90,7 @@ def _scale_system(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose],
 
     @classmethod
     def _calculate_mean_diagonal(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose],
-                                 matched_samples: list[LhCfPoseSample]) -> float:
+                                 matched_samples: list[LhCfPoseSampleWrapper]) -> float:
         """
         Calculate the average diagonal sensor distance based on where the rays intersect the lighthouse deck
         """
diff --git a/cflib/localization/lighthouse_types.py b/cflib/localization/lighthouse_types.py
index 23aa29683..8bef7a0ad 100644
--- a/cflib/localization/lighthouse_types.py
+++ b/cflib/localization/lighthouse_types.py
@@ -170,12 +170,6 @@ class LhMeasurement(NamedTuple):
     angles: LighthouseBsVectors
 
 
-class LhBsCfPoses(NamedTuple):
-    """Represents all poses of base stations and CF samples"""
-    bs_poses: dict[int, Pose]
-    cf_poses: list[Pose]
-
-
 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 bea4f49a1..9730d9fac 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -64,7 +64,6 @@
 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
@@ -179,38 +178,6 @@ def set_axes_equal(ax):
     ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
 
 
-def visualize(poses: LhBsCfPoses):
-    """Visualize positions of base stations and Crazyflie positions"""
-    # Set to True to visualize positions
-    # Requires PyPlot
-    visualize_positions = True
-    if visualize_positions:
-        import matplotlib.pyplot as plt
-
-        positions = np.array(list(map(lambda x: x.translation, poses.cf_poses)))
-
-        fig = plt.figure()
-        ax = fig.add_subplot(projection='3d')
-
-        x_cf = positions[:, 0]
-        y_cf = positions[:, 1]
-        z_cf = positions[:, 2]
-
-        ax.scatter(x_cf, y_cf, z_cf)
-
-        positions = np.array(list(map(lambda x: x.translation, poses.bs_poses.values())))
-
-        x_bs = positions[:, 0]
-        y_bs = positions[:, 1]
-        z_bs = positions[:, 2]
-
-        ax.scatter(x_bs, y_bs, z_bs, c='red')
-
-        set_axes_equal(ax)
-        print('Close graph window to continue')
-        plt.show()
-
-
 def load_from_file(name: str) -> LhGeoInputContainerData:
     container = LhGeoInputContainer(LhDeck4SensorPositions.positions)
     with open(name, 'r', encoding='UTF8') as handle:

From 77a90cfb0499922f0560277b4e0dd23410ab7f35 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 15 Jul 2025 13:33:06 +0200
Subject: [PATCH 41/55] Added func to remove samples

---
 .../localization/lighthouse_cf_pose_sample.py | 16 ++++++++-
 .../lighthouse_geo_estimation_manager.py      | 33 +++++++++++++++++++
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index c231c12df..d57dddde1 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -139,9 +139,23 @@ def __init__(self, pose_sample: LhCfPoseSample,
 
         self.status = LhCfPoseSampleStatus.OK
 
-        self.pose: Pose = self.NO_POSE  # The pose of the sample, if available
+        self._pose: Pose = self.NO_POSE  # The pose of the sample, if available
+        self._has_pose: bool = False  # Indicates if the pose is set
         self.error_distance: float = 0.0  # The error distance of the pose, if available
 
+    @property
+    def has_pose(self) -> bool:
+        return self._has_pose
+
+    @property
+    def pose(self) -> Pose:
+        return self._pose
+
+    @pose.setter
+    def pose(self, pose: Pose) -> None:
+        self._pose = pose
+        self._has_pose = True
+
     @property
     def angles_calibrated(self) -> dict[int, LighthouseBsVectors]:
         return self.pose_sample.angles_calibrated
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index da673efb1..6d5478bb9 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -448,6 +448,39 @@ def xyz_space_sample_count(self) -> int:
         with self.is_modified_condition:
             return len(self._data.xyz_space)
 
+    def remove_sample(self, index: int) -> None:
+        """Remove a sample from the container by index, including origin, x-axis, xy-plane, or xyz-space samples.
+
+        Args:
+            index (int): The index of the sample to remove
+        """
+        with self.is_modified_condition:
+            if index < 0:
+                raise IndexError('Index out of range')
+
+            origin_idx = 0
+            x_axis_start_idx = 1
+            xy_plane_start_idx = x_axis_start_idx + len(self._data.x_axis)
+            xyz_space_start_idx = xy_plane_start_idx + len(self._data.xy_plane)
+
+            if index == origin_idx:
+                # Remove the origin sample
+                self._data.origin = LhGeoInputContainerData.EMPTY_POSE_SAMPLE
+            elif index < xy_plane_start_idx:
+                # Remove an x-axis sample
+                del self._data.x_axis[index - x_axis_start_idx]
+            elif index < xyz_space_start_idx:
+                # Remove an xy-plane sample
+                del self._data.xy_plane[index - xy_plane_start_idx]
+            else:
+                xyz_index = index - xyz_space_start_idx
+                if xyz_index > len(self._data.xyz_space):
+                    raise IndexError('Index out of range')
+                # Remove an xyz-space sample
+                del self._data.xyz_space[xyz_index]
+
+            self._handle_data_modification()
+
     def clear_all_samples(self) -> None:
         """Clear all samples in the container"""
         self._set_new_data_container(LhGeoInputContainerData(self._data.sensor_positions))

From b892748ce3d51309f4ff22b5bfbdb8ba230bf67e Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 15 Jul 2025 14:14:46 +0200
Subject: [PATCH 42/55] Added error threshold

---
 cflib/localization/lighthouse_cf_pose_sample.py | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index d57dddde1..e0fabc167 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -124,6 +124,7 @@ class LhCfPoseSampleWrapper():
     """A wrapper of LhCfPoseSample that includes more information, useful in the estimation process and in a UI."""
 
     NO_POSE = Pose()
+    LARGE_ERROR_THRESHOLD = 0.01  # Threshold for large error distance, in meters
 
     def __init__(self, pose_sample: LhCfPoseSample,
                  sample_type: LhCfPoseSampleType = LhCfPoseSampleType.XYZ_SPACE) -> None:
@@ -172,3 +173,8 @@ def is_valid(self) -> bool:
     def base_station_ids(self) -> list[int]:
         """Get the base station ids of the sample"""
         return list(self.angles_calibrated.keys())
+
+    @property
+    def is_error_large(self) -> bool:
+        """Check if the error distance is large enough to be considered an outlier"""
+        return self.error_distance > self.LARGE_ERROR_THRESHOLD

From d4bfb2fa3c4af14b67c0dc6fd48bba5571299d50 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 15 Jul 2025 14:47:20 +0200
Subject: [PATCH 43/55] Added cb for start of solution

---
 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 6d5478bb9..6c2a5df98 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -104,8 +104,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. progress_is_ok is not a good indicator
-
         return solution
 
     @classmethod
@@ -216,7 +214,7 @@ class SolverThread(threading.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:
+        def __init__(self, container: LhGeoInputContainer, is_done_cb, is_starting_estimation_cb=None) -> 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
@@ -232,6 +230,7 @@ def __init__(self, container: LhGeoInputContainer, is_done_cb) -> None:
             self.latest_solved_data_version = -1
 
             self.is_done_cb = is_done_cb
+            self.is_starting_estimation_cb = is_starting_estimation_cb
 
             self.is_running = False
             self.is_done = False
@@ -249,6 +248,9 @@ def run(self):
                     if self.container.get_data_version() > self.latest_solved_data_version:
                         self.is_done = False
 
+                        if self.is_starting_estimation_cb:
+                            self.is_starting_estimation_cb()
+
                         # Copy the container as the original container may be modified while the solver is running
                         container_copy = self.container.get_data_copy()
                         solution = LhGeoEstimationManager.estimate_geometry(container_copy)

From 2e1b5941123c95c3770f2a47025a40f279852791 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Tue, 15 Jul 2025 15:06:48 +0200
Subject: [PATCH 44/55] Added write status method to config writer

---
 cflib/localization/lighthouse_config_manager.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/cflib/localization/lighthouse_config_manager.py b/cflib/localization/lighthouse_config_manager.py
index d5204ea4b..63f82a59d 100644
--- a/cflib/localization/lighthouse_config_manager.py
+++ b/cflib/localization/lighthouse_config_manager.py
@@ -167,6 +167,10 @@ def write_and_store_config_from_file(self, data_stored_cb, file_name):
         geos, calibs, system_type = LighthouseConfigFileManager.read(file_name)
         self.write_and_store_config(data_stored_cb, geos=geos, calibs=calibs, system_type=system_type)
 
+    @property
+    def is_write_ongoing(self) -> bool:
+        return self._data_stored_cb is not None
+
     def _next(self):
         if self._geos_to_write is not None:
             self._helper.write_geos(self._geos_to_write, self._upload_done)

From dccbe3e70a2008ef594f64b8ba515cc7c9242e8b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 17 Jul 2025 16:05:23 +0200
Subject: [PATCH 45/55] Use UID to identify samples

---
 .../localization/lighthouse_cf_pose_sample.py | 28 +++++++++++++++++++
 .../lighthouse_geo_estimation_manager.py      | 14 +++++++++-
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index e0fabc167..d1d7c190a 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -1,4 +1,5 @@
 import enum
+import threading
 from typing import NamedTuple
 
 import numpy as np
@@ -18,6 +19,17 @@ class BsPairPoses(NamedTuple):
     bs2: Pose
 
 
+class AtomicCounter:
+    def __init__(self):
+        self.value = 0
+        self._lock = threading.Lock()
+
+    def increment(self, num=1):
+        with self._lock:
+            self.value += num
+            return self.value
+
+
 class LhCfPoseSample:
     """ Represents a sample of a Crazyflie pose in space, it contains:
     - lighthouse angles from one or more base stations
@@ -26,6 +38,8 @@ class LhCfPoseSample:
     The ippe solution is somewhat heavy and is only created on demand by calling augment_with_ippe()
     """
 
+    global_uid = AtomicCounter()
+
     def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors]) -> None:
         # Angles measured by the Crazyflie and compensated using calibration data
         # Stored in a dictionary using base station id as the key
@@ -36,6 +50,15 @@ def __init__(self, angles_calibrated: dict[int, LighthouseBsVectors]) -> None:
         self.ippe_solutions: dict[int, BsPairPoses] = {}
         self.is_augmented = False
 
+        # A unique Id for each sample, at least guaranteed to be unique per session.
+        # Used to identify samples in the container.
+        self._uid = LhCfPoseSample.global_uid.increment()
+
+    @property
+    def uid(self) -> int:
+        """Get the unique identifier of the sample"""
+        return self._uid
+
     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)
@@ -157,6 +180,11 @@ def pose(self, pose: Pose) -> None:
         self._pose = pose
         self._has_pose = True
 
+    @property
+    def uid (self) -> int:
+        """Get the unique identifier of the sample"""
+        return self.pose_sample.uid
+
     @property
     def angles_calibrated(self) -> dict[int, LighthouseBsVectors]:
         return self.pose_sample.angles_calibrated
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 6c2a5df98..32558a08c 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -450,7 +450,19 @@ def xyz_space_sample_count(self) -> int:
         with self.is_modified_condition:
             return len(self._data.xyz_space)
 
-    def remove_sample(self, index: int) -> None:
+    def remove_sample(self, uid: int) -> None:
+        """Remove a sample from the container by UID, including origin, x-axis, xy-plane, or xyz-space samples.
+
+        Args:
+            uid (int): The UID of the sample to remove
+        """
+        with self.is_modified_condition:
+            for index, sample in enumerate(self._data.get_matched_samples()):
+                if sample.uid == uid:
+                    self.remove_sample_by_index(index)
+                    return
+
+    def remove_sample_by_index(self, index: int) -> None:
         """Remove a sample from the container by index, including origin, x-axis, xy-plane, or xyz-space samples.
 
         Args:

From d665065781328bf62178188d9891e9bdc6e672b5 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 17 Jul 2025 16:11:45 +0200
Subject: [PATCH 46/55] Renamed outlier to ambiguous

---
 cflib/localization/lighthouse_cf_pose_sample.py  |  4 ++--
 .../localization/lighthouse_initial_estimator.py | 16 ++++++++--------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index d1d7c190a..0aff43a5d 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -136,7 +136,7 @@ class LhCfPoseSampleStatus(enum.Enum):
     """An enum representing the status of a pose sample"""
     OK = 'OK'
     TOO_FEW_BS = 'Too few bs'
-    OUTLIER = 'Outlier'
+    AMBIGUOUS = 'Ambiguous'
     NO_DATA = 'No data'
 
     def __str__(self):
@@ -155,7 +155,7 @@ def __init__(self, pose_sample: LhCfPoseSample,
 
         self.sample_type = sample_type
 
-        # Some samples are mandatory and must not be removed, even if they appear to be outliers. For instance the
+        # Some samples are mandatory and must not be removed, even if they appear to be ambiguous. For instance the
         # the samples that define the origin or x-axis
         self.is_mandatory = self.sample_type in (LhCfPoseSampleType.ORIGIN,
                                                  LhCfPoseSampleType.X_AXIS,
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 4710972ff..f010dbad2 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -50,7 +50,7 @@ class LighthouseInitialEstimator:
     calculations.
     """
 
-    OUTLIER_DETECTION_ERROR = 0.5
+    AMBIGUOUS_DETECTION_ERROR = 0.5
 
     @classmethod
     def estimate(cls, matched_samples: list[LhCfPoseSampleWrapper],
@@ -65,7 +65,7 @@ def estimate(cls, matched_samples: list[LhCfPoseSampleWrapper],
                                 two or more base stations. Note: matched_samples is a subset of solution.samples.
 
         :param solution: A LighthouseGeometrySolution object to store progress information and issues in
-        :return: a subset of the matched_samples where outliers are removed.
+        :return: a subset of the matched_samples where ambiguous samples are removed.
         """
 
         bs_positions = cls._find_bs_to_bs_poses(matched_samples)
@@ -192,12 +192,12 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSampleWrapper], bs_posit
         :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 ambiguous samples are removed.
         """
         result: list[dict[int, Pose]] = []
 
         cleaned_matched_samples: list[LhCfPoseSampleWrapper] = []
-        outlier_count = 0
+        ambiguous_count = 0
 
         for sample in matched_samples:
             solutions = sample.ippe_solutions
@@ -217,12 +217,12 @@ def _angles_to_poses(cls, matched_samples: list[LhCfPoseSampleWrapper], bs_posit
                     poses[pair_ids.bs2] = pair_poses.bs2
                 else:
                     is_sample_valid = False
-                    sample.status = LhCfPoseSampleStatus.OUTLIER
+                    sample.status = LhCfPoseSampleStatus.AMBIGUOUS
                     if sample.is_mandatory:
                         solution.progress_is_ok = False
                     else:
-                        outlier_count += 1
-                        solution.xyz_space_samples_info = f'{outlier_count} sample(s) with outliers skipped'
+                        ambiguous_count += 1
+                        solution.xyz_space_samples_info = f'{ambiguous_count} sample(s) with ambiguities skipped'
                     break
 
             if is_sample_valid or sample.is_mandatory:
@@ -248,7 +248,7 @@ def _choose_solutions(cls, solutions_1: BsPairPoses, solutions_2: BsPairPoses,
                     min_dist = dist
                     best = BsPairPoses(solution_1, solution_2)
 
-        if min_dist > cls.OUTLIER_DETECTION_ERROR:
+        if min_dist > cls.AMBIGUOUS_DETECTION_ERROR:
             success = False
 
         return success, best

From 241fe976ca1cfca10923cc8f5c5ff813ddbfc794 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 17 Jul 2025 16:44:40 +0200
Subject: [PATCH 47/55] Cleaned up index handling

---
 .../lighthouse_geo_estimation_manager.py      | 67 +++++++++++++++----
 1 file changed, 55 insertions(+), 12 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 32558a08c..efd88dbbf 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -53,18 +53,15 @@ class LhGeoEstimationManager():
     @classmethod
     def align_and_scale_solution(cls, container: LhGeoInputContainerData, solution: LighthouseGeometrySolution,
                                  samples: list[LhCfPoseSampleWrapper], reference_distance: float):
-
-        # Note: samples is a subset of solution.samples
         bs_poses = solution.bs_poses
 
-        start_idx_x_axis = 1
-        start_idx_xy_plane = start_idx_x_axis + len(container.x_axis)
-        start_idx_xyz_space = start_idx_xy_plane + len(container.xy_plane)
+        # Note: samples is a subset of solution.samples but samples are never removed from origin, x-axis or xy-plane
+        # so we can use the number of samples in the container to determine the indices in the sample list.
 
-        origin_pos = samples[0].pose.translation
-        x_axis_samples = samples[start_idx_x_axis:start_idx_x_axis + len(container.x_axis)]
+        origin_pos = samples[container.origin_index].pose.translation
+        x_axis_samples = samples[container.x_axis_slice]
         x_axis_pos = list(map(lambda x: x.pose.translation, x_axis_samples))
-        xy_plane_samples = samples[start_idx_xy_plane:start_idx_xyz_space]
+        xy_plane_samples = samples[container.xy_plane_slice]
         xy_plane_pos = list(map(lambda x: x.pose.translation, xy_plane_samples))
 
         # Align the solution
@@ -133,20 +130,21 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSampleWrapper], containe
             result.append(origin)
 
         # Check the x-axis samples
-        if len(container.x_axis) == 0:
+        if container.x_axis_sample_count == 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:
+        if container.xy_plane_sample_count == 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:
+        if container.xyz_space_sample_count == 0:
             solution.xyz_space_samples_info = NO_DATA
 
-        # Samples must contain at least two base stations
+        # Samples must contain at least two base stations.
+        # Skip the origin sample as it is already checked above.
         for sample in matched_samples[1:]:
             if len(sample.angles_calibrated) >= 2:
                 result.append(sample)
@@ -286,6 +284,51 @@ def __init__(self, sensor_positions: ArrayFloat, version: int = 0) -> None:
         # Used by LhGeoInputContainer to track changes in the data
         self.version = version
 
+    @property
+    def origin_index(self) -> int:
+        """Get the index of the origin sample in the list of samples"""
+        return 0
+
+    @property
+    def x_axis_start_index(self) -> int:
+        """Get the index of the first x-axis sample in the list of samples"""
+        return self.origin_index + 1
+
+    @property
+    def x_axis_slice(self) -> slice:
+        """Get the slice for the x-axis samples in the list of samples"""
+        return slice(self.x_axis_start_index, self.x_axis_start_index + len(self.x_axis))
+
+    @property
+    def x_axis_sample_count(self) -> int:
+        """Get the count of x-axis samples in the list of samples"""
+        return len(self.x_axis)
+
+    @property
+    def xy_plane_start_index(self) -> int:
+        """Get the index of the first xy-plane sample in the list of samples"""
+        return self.x_axis_start_index + len(self.x_axis)
+
+    @property
+    def xy_plane_slice(self) -> slice:
+        """Get the slice for the xy-plane samples in the list of samples"""
+        return slice(self.xy_plane_start_index, self.xy_plane_start_index + len(self.xy_plane))
+
+    @property
+    def xy_plane_sample_count(self) -> int:
+        """Get the count of xy-plane samples in the list of samples"""
+        return len(self.xy_plane)
+
+    @property
+    def xyz_space_start_index(self) -> int:
+        """Get the index of the first xyz-space sample in the list of samples"""
+        return self.xy_plane_start_index + len(self.xy_plane)
+
+    @property
+    def xyz_space_sample_count(self) -> int:
+        """Get the count of xyz-space samples in the list of samples"""
+        return len(self.xyz_space)
+
     def get_matched_samples(self) -> list[LhCfPoseSampleWrapper]:
         """Get all pose samples collected in a list
 

From db45841efa9a7c57ec6851ec208c3d1bbb4238b1 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 18 Jul 2025 11:39:00 +0200
Subject: [PATCH 48/55] Added verification samples

---
 .../localization/lighthouse_cf_pose_sample.py |   3 +-
 .../lighthouse_geo_estimation_manager.py      | 143 +++++++++++++-----
 .../lighthouse_geometry_solution.py           |   5 +-
 cflib/localization/lighthouse_utils.py        |  87 ++++++++---
 .../multi_bs_geometry_estimation.py           |   4 +-
 5 files changed, 181 insertions(+), 61 deletions(-)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 0aff43a5d..109b9dd14 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -126,6 +126,7 @@ class LhCfPoseSampleType(enum.Enum):
     X_AXIS = 'x-axis'
     XY_PLANE = 'xy-plane'
     XYZ_SPACE = 'xyz-space'
+    VERIFICATION = 'verification'
 
     def __str__(self):
         return self.value
@@ -181,7 +182,7 @@ def pose(self, pose: Pose) -> None:
         self._has_pose = True
 
     @property
-    def uid (self) -> int:
+    def uid(self) -> int:
         """Get the unique identifier of the sample"""
         return self.pose_sample.uid
 
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index efd88dbbf..7daac5e0c 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -41,6 +41,7 @@
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
 from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner
 from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler
+from cflib.localization.lighthouse_types import Pose
 from cflib.localization.lighthouse_utils import LighthouseCrossingBeam
 
 
@@ -50,6 +51,11 @@
 class LhGeoEstimationManager():
     REFERENCE_DIST = 1.0  # Reference distance used for scaling the solution
 
+    ESTIMATION_TYPES = (LhCfPoseSampleType.ORIGIN,
+                        LhCfPoseSampleType.X_AXIS,
+                        LhCfPoseSampleType.XY_PLANE,
+                        LhCfPoseSampleType.XYZ_SPACE)
+
     @classmethod
     def align_and_scale_solution(cls, container: LhGeoInputContainerData, solution: LighthouseGeometrySolution,
                                  samples: list[LhCfPoseSampleWrapper], reference_distance: float):
@@ -98,6 +104,7 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
                                              cls.REFERENCE_DIST)
 
                 cls._create_solution_stats(validated_matched_samples, solution)
+                cls._create_verification_stats(solution)
 
         cls._humanize_error_info(solution, container)
 
@@ -106,7 +113,8 @@ def estimate_geometry(cls, container: LhGeoInputContainerData) -> LighthouseGeom
     @classmethod
     def _data_validation(cls, matched_samples: list[LhCfPoseSampleWrapper], container: LhGeoInputContainerData,
                          solution: LighthouseGeometrySolution) -> list[LhCfPoseSampleWrapper]:
-        """Validate the data collected by the Crazyflie and update the solution object with the results"""
+        """Validate the data collected by the Crazyflie and update the solution object with the results.
+        Filter out samples that will not be used for the geometry estimation."""
 
         result = []
 
@@ -135,11 +143,13 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSampleWrapper], containe
             solution.x_axis_samples_info = NO_DATA
             solution.progress_is_ok = False
 
+        # Check the xy-plane samples
         if container.xy_plane_sample_count == 0:
             solution.is_xy_plane_samples_valid = False
             solution.xy_plane_samples_info = NO_DATA
             solution.progress_is_ok = False
 
+        # Check the xyz-space samples
         if container.xyz_space_sample_count == 0:
             solution.xyz_space_samples_info = NO_DATA
 
@@ -147,7 +157,8 @@ def _data_validation(cls, matched_samples: list[LhCfPoseSampleWrapper], containe
         # Skip the origin sample as it is already checked above.
         for sample in matched_samples[1:]:
             if len(sample.angles_calibrated) >= 2:
-                result.append(sample)
+                if sample.sample_type in cls.ESTIMATION_TYPES:
+                    result.append(sample)
             else:
                 sample.status = LhCfPoseSampleStatus.TOO_FEW_BS
 
@@ -207,6 +218,29 @@ def _create_solution_stats(cls, matched_samples: list[LhCfPoseSampleWrapper], so
             std=np.std(cf_error)
         )
 
+    @classmethod
+    def _create_verification_stats(cls, solution: LighthouseGeometrySolution):
+        """Compute poses and errors for the verification samples in the solution using the crossing beam method."""
+        # Estimated worst error for each sample based on crossing beams
+        cf_error: list[float] = []
+
+        for sample in solution.samples:
+            if sample.sample_type == LhCfPoseSampleType.VERIFICATION:
+                bs_ids = list(sample.angles_calibrated.keys())
+                bs_angle_list = [(solution.bs_poses[bs_id], sample.angles_calibrated[bs_id]) for bs_id in bs_ids]
+                position, error = LighthouseCrossingBeam.position_max_distance_all_permutations(bs_angle_list)
+
+                sample.pose = Pose.from_rot_vec(t_vec=position)
+                sample.error_distance = error
+                cf_error.append(error)
+
+        if len(cf_error) > 0:
+            solution.verification_stats = LighthouseGeometrySolution.ErrorStats(
+                mean=np.mean(cf_error),
+                max=np.max(cf_error),
+                std=np.std(cf_error)
+            )
+
     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.
@@ -281,6 +315,9 @@ def __init__(self, sensor_positions: ArrayFloat, version: int = 0) -> None:
         self.xy_plane: list[LhCfPoseSample] = []
         self.xyz_space: list[LhCfPoseSample] = []
 
+        # Samples that are used to verify the geometry but are not used for the geometry estimation
+        self.verification: list[LhCfPoseSample] = []
+
         # Used by LhGeoInputContainer to track changes in the data
         self.version = version
 
@@ -326,7 +363,7 @@ def xyz_space_start_index(self) -> int:
 
     @property
     def xyz_space_sample_count(self) -> int:
-        """Get the count of xyz-space samples in the list of samples"""
+        """Get the count of xyz-space samples in the container"""
         return len(self.xyz_space)
 
     def get_matched_samples(self) -> list[LhCfPoseSampleWrapper]:
@@ -339,6 +376,8 @@ def get_matched_samples(self) -> list[LhCfPoseSampleWrapper]:
         result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.X_AXIS) for sample in self.x_axis]
         result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.XY_PLANE) for sample in self.xy_plane]
         result += [LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.XYZ_SPACE) for sample in self.xyz_space]
+        result += [
+            LhCfPoseSampleWrapper(sample, sample_type=LhCfPoseSampleType.VERIFICATION) for sample in self.verification]
 
         return result
 
@@ -360,6 +399,7 @@ def yaml_representer(dumper, data: LhGeoInputContainerData):
             'x_axis': data.x_axis,
             'xy_plane': data.xy_plane,
             'xyz_space': data.xyz_space,
+            'verification': data.verification,
             'sensor_positions': data.sensor_positions.tolist(),
         })
 
@@ -373,6 +413,12 @@ def yaml_constructor(loader, node):
         result.x_axis = values['x_axis']
         result.xy_plane = values['xy_plane']
         result.xyz_space = values['xyz_space']
+        if 'verification' in values:
+            # If verification is not present, it will be an empty list
+            # This is to ensure backward compatibility with older versions of the data container
+            result.verification = values['verification']
+        else:
+            result.verification = []
 
         # Augment the samples with the sensor positions
         result.origin.augment_with_ippe(sensor_positions)
@@ -386,6 +432,9 @@ def yaml_constructor(loader, node):
         for sample in result.xyz_space:
             sample.augment_with_ippe(sensor_positions)
 
+        for sample in result.verification:
+            sample.augment_with_ippe(sensor_positions)
+
         return result
 
 
@@ -494,49 +543,42 @@ def xyz_space_sample_count(self) -> int:
             return len(self._data.xyz_space)
 
     def remove_sample(self, uid: int) -> None:
-        """Remove a sample from the container by UID, including origin, x-axis, xy-plane, or xyz-space samples.
+        """Remove a sample from the container by UID.
 
         Args:
             uid (int): The UID of the sample to remove
         """
         with self.is_modified_condition:
-            for index, sample in enumerate(self._data.get_matched_samples()):
-                if sample.uid == uid:
-                    self.remove_sample_by_index(index)
-                    return
+            sample = self._remove_sample_by_uid(uid)
+            if sample is not None:
+                self._handle_data_modification()
 
-    def remove_sample_by_index(self, index: int) -> None:
-        """Remove a sample from the container by index, including origin, x-axis, xy-plane, or xyz-space samples.
+    def convert_to_verification_sample(self, uid: int) -> None:
+        """Convert a sample to a verification sample by UID.
+        The sample will be moved to the verification list and removed from the other lists.
 
         Args:
-            index (int): The index of the sample to remove
+            uid (int): The UID of the sample to convert
         """
+        print(f'Converting sample with UID {uid} to verification sample')
         with self.is_modified_condition:
-            if index < 0:
-                raise IndexError('Index out of range')
-
-            origin_idx = 0
-            x_axis_start_idx = 1
-            xy_plane_start_idx = x_axis_start_idx + len(self._data.x_axis)
-            xyz_space_start_idx = xy_plane_start_idx + len(self._data.xy_plane)
+            sample = self._remove_sample_by_uid(uid)
+            if sample is not None:
+                self._data.verification.append(sample)
+                self._handle_data_modification()
 
-            if index == origin_idx:
-                # Remove the origin sample
-                self._data.origin = LhGeoInputContainerData.EMPTY_POSE_SAMPLE
-            elif index < xy_plane_start_idx:
-                # Remove an x-axis sample
-                del self._data.x_axis[index - x_axis_start_idx]
-            elif index < xyz_space_start_idx:
-                # Remove an xy-plane sample
-                del self._data.xy_plane[index - xy_plane_start_idx]
-            else:
-                xyz_index = index - xyz_space_start_idx
-                if xyz_index > len(self._data.xyz_space):
-                    raise IndexError('Index out of range')
-                # Remove an xyz-space sample
-                del self._data.xyz_space[xyz_index]
+    def convert_to_xyz_space_sample(self, uid: int) -> None:
+        """Convert a sample to a xyz-space sample by UID.
+        The sample will be moved to the xyz-space list and removed from the other lists.
 
-            self._handle_data_modification()
+        Args:
+            uid (int): The UID of the sample to convert
+        """
+        with self.is_modified_condition:
+            sample = self._remove_sample_by_uid(uid)
+            if sample is not None:
+                self._data.xyz_space.append(sample)
+                self._handle_data_modification()
 
     def clear_all_samples(self) -> None:
         """Clear all samples in the container"""
@@ -615,6 +657,39 @@ def enable_auto_save(self, session_path: str = os.getcwd()) -> None:
         self._session_path = session_path
         self._auto_save = True
 
+    def _remove_sample_by_uid(self, uid: int) -> LhCfPoseSample | None:
+        removed = None
+        if self._data.origin != LhGeoInputContainerData.EMPTY_POSE_SAMPLE:
+            if self._data.origin.uid == uid:
+                removed = self._data.origin
+                self._data.origin = LhGeoInputContainerData.EMPTY_POSE_SAMPLE
+
+        if removed is None:
+            for index, sample in enumerate(self._data.x_axis):
+                if sample.uid == uid:
+                    removed = self._data.x_axis.pop(index)
+                    break
+
+        if removed is None:
+            for index, sample in enumerate(self._data.xy_plane):
+                if sample.uid == uid:
+                    removed = self._data.xy_plane.pop(index)
+                    break
+
+        if removed is None:
+            for index, sample in enumerate(self._data.xyz_space):
+                if sample.uid == uid:
+                    removed = self._data.xyz_space.pop(index)
+                    break
+
+        if removed is None:
+            for index, sample in enumerate(self._data.verification):
+                if sample.uid == uid:
+                    removed = self._data.verification.pop(index)
+                    break
+
+        return removed
+
     def _set_new_data_container(self, new_data: LhGeoInputContainerData) -> None:
         """Set a new data container and update the version"""
 
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index 24af24a5e..9b3748d7d 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -45,9 +45,12 @@ def __init__(self, samples: list[LhCfPoseSampleWrapper]):
         # The estimated poses of the base stations. The keys are the base station ids and the values are the poses.
         self.bs_poses: dict[int, Pose] = {}
 
-        # Information about errors in the solution
+        # Information about errors for the samples that are used in the solution
         self.error_stats = self.ErrorStats(0.0, 0.0, 0.0)
 
+        # Information about the verification samples
+        self.verification_stats: None | LighthouseGeometrySolution.ErrorStats = None
+
         # Indicates if the solution converged (True).
         # If it did not converge, the solution is possibly not good enough to use
         self.has_converged = False
diff --git a/cflib/localization/lighthouse_utils.py b/cflib/localization/lighthouse_utils.py
index 9e614e7ad..e69a6b218 100644
--- a/cflib/localization/lighthouse_utils.py
+++ b/cflib/localization/lighthouse_utils.py
@@ -38,9 +38,9 @@ class LighthouseCrossingBeam:
     """
 
     @classmethod
-    def position_distance(cls,
-                          bs1: Pose, angles_bs1: LighthouseBsVector,
-                          bs2: Pose, angles_bs2: LighthouseBsVector) -> tuple[npt.NDArray, float]:
+    def position_distance_sensor(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.
 
@@ -62,9 +62,9 @@ def position_distance(cls,
         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:
+    def position_sensor(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.
 
@@ -77,13 +77,13 @@ def position(cls,
         Returns:
             npt.NDArray: The estimated position of the crossing point of the two beams.
         """
-        position, _ = cls.position_distance(bs1, angles_bs1, bs2, angles_bs2)
+        position, _ = cls.position_distance_sensor(bs1, angles_bs1, bs2, angles_bs2)
         return position
 
     @classmethod
-    def distance(cls,
-                 bs1: Pose, angles_bs1: LighthouseBsVector,
-                 bs2: Pose, angles_bs2: LighthouseBsVector) -> float:
+    def distance_sensor(cls,
+                        bs1: Pose, angles_bs1: LighthouseBsVector,
+                        bs2: Pose, angles_bs2: LighthouseBsVector) -> float:
         """Calculate the minimum distance between the beams from two base stations.
 
         Args:
@@ -95,14 +95,14 @@ def distance(cls,
         Returns:
             float: The shortest distance between the beams.
         """
-        _, distance = cls.position_distance(bs1, angles_bs1, bs2, angles_bs2)
+        _, distance = cls.position_distance_sensor(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.
+    def positions_distances(cls,
+                            bs1: Pose, angles_bs1: LighthouseBsVectors,
+                            bs2: Pose, angles_bs2: LighthouseBsVectors) -> list[tuple[npt.NDArray, float]]:
+        """Calculate the positions and minimum distance between the beams from two base stations for all sensors.
 
         Args:
             bs1 (Pose): The pose of the first base station.
@@ -111,15 +111,17 @@ def distances(cls,
             angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
 
         Returns:
-            list[float]: A list of the distances.
+            list[tuple[npt.NDArray, float]]: A list of the positions and distances for each sensor.
         """
-        return [cls.distance(bs1, angles1, bs2, angles2) for angles1, angles2 in zip(angles_bs1, angles_bs2)]
+        return [cls.position_distance_sensor(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.
+    def position_max_distance(cls,
+                              bs1: Pose, angles_bs1: LighthouseBsVectors,
+                              bs2: Pose, angles_bs2: LighthouseBsVectors) -> tuple[npt.NDArray, float]:
+        """Calculate the position and maximum distance between the beams from two base stations.
+        The position is the average position for all sensors which is the center of the lighthouse deck.
 
         Args:
             bs1 (Pose): The pose of the first base station.
@@ -128,9 +130,12 @@ def max_distance(cls,
             angles_bs2 (LighthouseBsVectors): The sweep angles of the second base station.
 
         Returns:
-            float: The maximum distance between the beams.
+            float: The position and maximum distance between the beams.
         """
-        return max(cls.distances(bs1, angles_bs1, bs2, angles_bs2))
+        position_distances = cls.positions_distances(bs1, angles_bs1, bs2, angles_bs2)
+        positions, distances = zip(*position_distances)
+
+        return np.mean(positions, axis=0), max(distances)
 
     @classmethod
     def max_distance_all_permutations(cls, bs_angles: list[tuple[Pose, LighthouseBsVectors]]) -> float:
@@ -154,11 +159,45 @@ def max_distance_all_permutations(cls, bs_angles: list[tuple[Pose, LighthouseBsV
                 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)
+                _, distance = cls.position_max_distance(bs1, angles_bs1, bs2, angles_bs2)
                 max_distance = max(max_distance, distance)
 
         return max_distance
 
+    @classmethod
+    def position_max_distance_all_permutations(cls, bs_angles: list[tuple[Pose, LighthouseBsVectors]]
+                                               ) -> tuple[npt.NDArray, float]:
+        """Calculate the average position and the maximum distance between the beams from base stations
+        for all sensors. All permutations of base stations are considered.
+
+        The position will be an estimate of the position of the center of the lighthouse deck and the maximum distance
+        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:
+            tuple[npt.NDArray, float]: The position and 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.')
+
+        distances = []
+        positions = []
+        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 position and distance for this pair of base stations
+                position, distance = cls.position_max_distance(bs1, angles_bs1, bs2, angles_bs2)
+                positions.append(position)
+                distances.append(distance)
+
+        return np.mean(positions, axis=0), max(distances)
+
     @classmethod
     def _position_distance(cls,
                            orig_1: npt.NDArray, vec_1: npt.NDArray,
diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py
index 9730d9fac..f472c215e 100644
--- a/examples/lighthouse/multi_bs_geometry_estimation.py
+++ b/examples/lighthouse/multi_bs_geometry_estimation.py
@@ -190,7 +190,7 @@ def _print(msg: str):
         print(f'      * {msg}')
     _print('Solution ready --------------------------------------')
     _print('  Base stations at:')
-    bs_poses = solution.poses.bs_poses
+    bs_poses = solution.bs_poses
     print_base_stations_poses(bs_poses, printer=_print)
 
     _print(f'Converged: {solution.has_converged}')
@@ -202,6 +202,8 @@ def _print(msg: str):
     _print(f'XYZ space: {solution.xyz_space_samples_info}')
     _print(f'General info: {solution.general_failure_info}')
     _print(f'Error info: {solution.error_stats}')
+    if solution.verification_stats:
+        _print(f'Verification info: {solution.verification_stats}')
 
 
 def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]):

From 9990e9ee4390236d19d03d0a0e9e4ce78b16de1e Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 18 Jul 2025 15:32:27 +0200
Subject: [PATCH 49/55] added verification methods

---
 .../lighthouse_geo_estimation_manager.py      | 27 ++++++++++++++++---
 1 file changed, 24 insertions(+), 3 deletions(-)

diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 7daac5e0c..2f43beebd 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -509,7 +509,7 @@ def xy_plane_sample_count(self) -> int:
             return len(self._data.xy_plane)
 
     def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
-        """Store/update the samples for the volume
+        """Store/update the samples for the xyz space
 
         Args:
             samples (list[LhMeasurement]): the new samples
@@ -522,10 +522,10 @@ def set_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
             self._handle_data_modification()
 
     def append_xyz_space_samples(self, samples: list[LhCfPoseSample]) -> None:
-        """Append to the samples for the volume
+        """Append to the samples for the xyz space
 
         Args:
-            samples (LhMeasurement): the new samples
+            samples (list[LhMeasurement]): the new samples
         """
         new_samples = samples
         self._augment_samples(new_samples)
@@ -542,6 +542,27 @@ def xyz_space_sample_count(self) -> int:
         with self.is_modified_condition:
             return len(self._data.xyz_space)
 
+    def append_verification_samples(self, samples: list[LhCfPoseSample]) -> None:
+        """Append to the samples for verification
+
+        Args:
+            samples (list[LhCfPoseSample]): the new samples
+        """
+        new_samples = samples
+        self._augment_samples(new_samples)
+        with self.is_modified_condition:
+            self._data.verification += new_samples
+            self._handle_data_modification()
+
+    def verification_sample_count(self) -> int:
+        """Get the number of samples used for verification
+
+        Returns:
+            int: The number of samples used for verification
+        """
+        with self.is_modified_condition:
+            return len(self._data.verification)
+
     def remove_sample(self, uid: int) -> None:
         """Remove a sample from the container by UID.
 

From 288abb73f10f86eccf87ff667533d727472e7f7c Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 18 Jul 2025 16:12:15 +0200
Subject: [PATCH 50/55] Added header

---
 .../localization/lighthouse_cf_pose_sample.py | 21 +++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 109b9dd14..0f61a42f2 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -1,3 +1,24 @@
+# -*- 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 enum
 import threading
 from typing import NamedTuple

From 33db4bd50a8fd49b5816e216a24a584448643561 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 18 Jul 2025 18:15:01 +0200
Subject: [PATCH 51/55] Added threshold for bs links

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

diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index f010dbad2..45a46b206 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -119,6 +119,10 @@ def increase_link_count(bs1: int, bs2: int):
                     if bs1 != bs2:
                         increase_link_count(bs1, bs2)
 
+        solution.link_count_ok_threshold = 1
+        if len(solution.link_count) > 2:
+            solution.link_count_ok_threshold = 2
+
     @classmethod
     def _find_bs_to_bs_poses(cls, matched_samples: list[LhCfPoseSampleWrapper]) -> dict[BsPairIds, ArrayFloat]:
         """

From b207d861d97377cbb01d9cf000d9acb46aac18b9 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sat, 19 Jul 2025 09:39:02 +0200
Subject: [PATCH 52/55] Handle some corner cases

---
 cflib/localization/__init__.py                |  4 ++-
 .../localization/lighthouse_cf_pose_sample.py |  1 +
 .../lighthouse_geo_estimation_manager.py      | 35 +++++++++++--------
 .../lighthouse_geometry_solution.py           |  3 ++
 4 files changed, 27 insertions(+), 16 deletions(-)

diff --git a/cflib/localization/__init__.py b/cflib/localization/__init__.py
index eff3da0cf..2f3f0f6c8 100644
--- a/cflib/localization/__init__.py
+++ b/cflib/localization/__init__.py
@@ -21,6 +21,7 @@
 # along with this program. If not, see <https://www.gnu.org/licenses/>.
 from .lighthouse_bs_geo import LighthouseBsGeoEstimator
 from .lighthouse_bs_vector import LighthouseBsVector
+from .lighthouse_cf_pose_sample import LhCfPoseSampleType
 from .lighthouse_config_manager import LighthouseConfigFileManager
 from .lighthouse_config_manager import LighthouseConfigWriter
 from .lighthouse_geometry_solution import LighthouseGeometrySolution
@@ -40,4 +41,5 @@
     'LighthouseConfigWriter',
     'ParamFileManager',
     'LighthouseCrossingBeam',
-    'LighthouseGeometrySolution']
+    'LighthouseGeometrySolution',
+    'LhCfPoseSampleType']
diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 0f61a42f2..2b4fec2bf 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -160,6 +160,7 @@ class LhCfPoseSampleStatus(enum.Enum):
     TOO_FEW_BS = 'Too few bs'
     AMBIGUOUS = 'Ambiguous'
     NO_DATA = 'No data'
+    BS_UNKNOWN = 'Bs unknown'
 
     def __str__(self):
         return self.value
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index 2f43beebd..f11195d41 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -227,12 +227,22 @@ def _create_verification_stats(cls, solution: LighthouseGeometrySolution):
         for sample in solution.samples:
             if sample.sample_type == LhCfPoseSampleType.VERIFICATION:
                 bs_ids = list(sample.angles_calibrated.keys())
-                bs_angle_list = [(solution.bs_poses[bs_id], sample.angles_calibrated[bs_id]) for bs_id in bs_ids]
-                position, error = LighthouseCrossingBeam.position_max_distance_all_permutations(bs_angle_list)
 
-                sample.pose = Pose.from_rot_vec(t_vec=position)
-                sample.error_distance = error
-                cf_error.append(error)
+                # Make sure all base stations in the sample are present in the solution
+                is_ok = True
+                for bs in bs_ids:
+                    if bs not in solution.bs_poses:
+                        sample.status = LhCfPoseSampleStatus.BS_UNKNOWN
+                        is_ok = False
+                        continue
+
+                if is_ok:
+                    bs_angle_list = [(solution.bs_poses[bs_id], sample.angles_calibrated[bs_id]) for bs_id in bs_ids]
+                    position, error = LighthouseCrossingBeam.position_max_distance_all_permutations(bs_angle_list)
+
+                    sample.pose = Pose.from_rot_vec(t_vec=position)
+                    sample.error_distance = error
+                    cf_error.append(error)
 
         if len(cf_error) > 0:
             solution.verification_stats = LighthouseGeometrySolution.ErrorStats(
@@ -409,16 +419,11 @@ def yaml_constructor(loader, node):
         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']
-        if 'verification' in values:
-            # If verification is not present, it will be an empty list
-            # This is to ensure backward compatibility with older versions of the data container
-            result.verification = values['verification']
-        else:
-            result.verification = []
+        result.origin = values['origin'] if 'origin' in values else LhGeoInputContainerData.EMPTY_POSE_SAMPLE
+        result.x_axis = values['x_axis'] if 'x_axis' in values else []
+        result.xy_plane = values['xy_plane'] if 'xy_plane' in values else []
+        result.xyz_space = values['xyz_space'] if 'xyz_space' in values else []
+        result.verification = values['verification'] if 'verification' in values else []
 
         # Augment the samples with the sensor positions
         result.origin.augment_with_ippe(sensor_positions)
diff --git a/cflib/localization/lighthouse_geometry_solution.py b/cflib/localization/lighthouse_geometry_solution.py
index 9b3748d7d..355c7eade 100644
--- a/cflib/localization/lighthouse_geometry_solution.py
+++ b/cflib/localization/lighthouse_geometry_solution.py
@@ -79,3 +79,6 @@ def __init__(self, samples: list[LhCfPoseSampleWrapper]):
         # 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]] = {}
+
+        # The threshold for the number of links between base stations that is considered ok.
+        self.link_count_ok_threshold = 1

From 27b1a433f38dc6c8424e706370de5b48ca1e6040 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sat, 19 Jul 2025 13:19:23 +0200
Subject: [PATCH 53/55] Styling

---
 examples/lighthouse/upload_geos.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/examples/lighthouse/upload_geos.py b/examples/lighthouse/upload_geos.py
index df38c0587..d2dfbf269 100644
--- a/examples/lighthouse/upload_geos.py
+++ b/examples/lighthouse/upload_geos.py
@@ -1,8 +1,8 @@
 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
+from cflib.localization import LighthouseConfigFileManager
+from cflib.localization import LighthouseConfigWriter
 
 
 # Upload a geometry to one or more Crazyflies.
@@ -11,7 +11,7 @@
 geos, calibs, type = mgr.read('/path/to/your/geo.yaml')
 
 uri_list = [
-    "radio://0/70/2M/E7E7E7E770"
+    'radio://0/70/2M/E7E7E7E770'
 ]
 
 # Initialize the low-level drivers

From 302498f401e0254449c226d25373a4ae941bfcf0 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sat, 19 Jul 2025 13:23:09 +0200
Subject: [PATCH 54/55] Updated tests

---
 .../test_lighthouse_geometry_solver.py        | 74 ++++++++-------
 .../test_lighthouse_initial_estimator.py      | 93 ++++++++++---------
 .../test_lighthouse_sample_matcher.py         | 82 ----------------
 3 files changed, 89 insertions(+), 160 deletions(-)
 delete mode 100644 test/localization/test_lighthouse_sample_matcher.py

diff --git a/test/localization/test_lighthouse_geometry_solver.py b/test/localization/test_lighthouse_geometry_solver.py
index cf4fef036..51da035d2 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_cf_pose_sample import LhCfPoseSampleWrapper
 from cflib.localization.lighthouse_geometry_solution import LighthouseGeometrySolution
 from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver
 from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator
@@ -32,30 +33,28 @@
 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
         # CF_ORIGIN is used in the first sample and will define the global reference frame
         bs_id0 = 3
         bs_id1 = 1
-        matched_samples = [
-            LhCfPoseSample(angles_calibrated={
-                bs_id0: self.fixtures.angles_cf_origin_bs0,
-                bs_id1: self.fixtures.angles_cf_origin_bs1,
-            }),
-        ]
-        for sample in matched_samples:
-            sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+        matched_samples = self.augment_and_wrap(
+            [
+                LhCfPoseSample(angles_calibrated={
+                    bs_id0: self.fixtures.angles_cf_origin_bs0,
+                    bs_id1: self.fixtures.angles_cf_origin_bs1,
+                }),
+            ])
+        solution = LighthouseGeometrySolution(matched_samples)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, self.solution)
+        cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, solution)
 
         # Test
-        LighthouseGeometrySolver.solve(
-            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions, self.solution)
+        LighthouseGeometrySolver.solve(cleaned_matched_samples, LhDeck4SensorPositions.positions, solution)
 
         # Assert
-        bs_poses = self.solution.poses.bs_poses
+        bs_poses = solution.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)
 
@@ -66,32 +65,41 @@ def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self):
         bs_id1 = 2
         bs_id2 = 9
         bs_id3 = 3
-        matched_samples = [
-            LhCfPoseSample(angles_calibrated={
-                bs_id0: self.fixtures.angles_cf_origin_bs0,
-                bs_id1: self.fixtures.angles_cf_origin_bs1,
-            }),
-            LhCfPoseSample(angles_calibrated={
-                bs_id1: self.fixtures.angles_cf1_bs1,
-                bs_id2: self.fixtures.angles_cf1_bs2,
-            }),
-            LhCfPoseSample(angles_calibrated={
-                bs_id2: self.fixtures.angles_cf2_bs2,
-                bs_id3: self.fixtures.angles_cf2_bs3,
-            }),
-        ]
-        for sample in matched_samples:
-            sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+        matched_samples = self.augment_and_wrap(
+            [
+                LhCfPoseSample(angles_calibrated={
+                    bs_id0: self.fixtures.angles_cf_origin_bs0,
+                    bs_id1: self.fixtures.angles_cf_origin_bs1,
+                }),
+                LhCfPoseSample(angles_calibrated={
+                    bs_id1: self.fixtures.angles_cf1_bs1,
+                    bs_id2: self.fixtures.angles_cf1_bs2,
+                }),
+                LhCfPoseSample(angles_calibrated={
+                    bs_id2: self.fixtures.angles_cf2_bs2,
+                    bs_id3: self.fixtures.angles_cf2_bs3,
+                }),
+            ]
+        )
+        solution = LighthouseGeometrySolution(matched_samples)
 
-        initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, self.solution)
+        cleaned_matched_samples = LighthouseInitialEstimator.estimate(matched_samples, solution)
 
         # Test
-        LighthouseGeometrySolver.solve(
-            initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions, self.solution)
+        LighthouseGeometrySolver.solve(cleaned_matched_samples, LhDeck4SensorPositions.positions, solution)
 
         # Assert
-        bs_poses = self.solution.poses.bs_poses
+        bs_poses = solution.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)
         self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, bs_poses[bs_id3], places=3)
+
+    # Helpers
+
+    def augment_and_wrap(self, samples: list[LhCfPoseSample]) -> list[LhCfPoseSampleWrapper]:
+        wrapped_samples = []
+        for sample in samples:
+            sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+            wrapped_samples.append(LhCfPoseSampleWrapper(sample))
+        return wrapped_samples
diff --git a/test/localization/test_lighthouse_initial_estimator.py b/test/localization/test_lighthouse_initial_estimator.py
index cff39835f..479abfc54 100644
--- a/test/localization/test_lighthouse_initial_estimator.py
+++ b/test/localization/test_lighthouse_initial_estimator.py
@@ -25,6 +25,7 @@
 import numpy as np
 
 from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSample
+from cflib.localization.lighthouse_cf_pose_sample import LhCfPoseSampleWrapper
 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
@@ -34,42 +35,41 @@
 class TestLighthouseInitialEstimator(LighthouseTestBase):
     def setUp(self):
         self.fixtures = LighthouseFixtures()
-        self.solution = LighthouseGeometrySolution()
 
     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
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={bs_id: self.fixtures.angles_cf_origin_bs0}),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        assert self.solution.progress_is_ok is False
+        assert solution.progress_is_ok is False
 
     def test_that_two_bs_poses_in_same_sample_are_found(self):
         # Fixture
         # CF_ORIGIN is used in the first sample and will define the global reference frame
         bs_id0 = 3
         bs_id1 = 1
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf_origin_bs0,
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual.bs_poses[bs_id1], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, solution.bs_poses[bs_id0], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, solution.bs_poses[bs_id1], places=3)
 
     def test_that_linked_bs_poses_in_multiple_samples_are_found(self):
         # Fixture
@@ -78,7 +78,7 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self):
         bs_id1 = 1
         bs_id2 = 5
         bs_id3 = 0
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf_origin_bs0,
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
@@ -91,17 +91,17 @@ def test_that_linked_bs_poses_in_multiple_samples_are_found(self):
                 bs_id2: self.fixtures.angles_cf2_bs2,
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, actual.bs_poses[bs_id0], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, actual.bs_poses[bs_id1], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, actual.bs_poses[bs_id2], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, actual.bs_poses[bs_id3], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS0_POSE, solution.bs_poses[bs_id0], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS1_POSE, solution.bs_poses[bs_id1], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS2_POSE, solution.bs_poses[bs_id2], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.BS3_POSE, solution.bs_poses[bs_id3], places=3)
 
     def test_that_cf_poses_are_estimated(self):
         # Fixture
@@ -110,7 +110,7 @@ def test_that_cf_poses_are_estimated(self):
         bs_id1 = 1
         bs_id2 = 5
         bs_id3 = 0
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf_origin_bs0,
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
@@ -123,16 +123,16 @@ def test_that_cf_poses_are_estimated(self):
                 bs_id2: self.fixtures.angles_cf2_bs2,
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, actual.cf_poses[0], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.CF1_POSE, actual.cf_poses[1], places=3)
-        self.assertPosesAlmostEqual(self.fixtures.CF2_POSE, actual.cf_poses[2], places=3)
+        self.assertPosesAlmostEqual(self.fixtures.CF_ORIGIN_POSE, samples[0].pose, places=3)
+        self.assertPosesAlmostEqual(self.fixtures.CF1_POSE, samples[1].pose, places=3)
+        self.assertPosesAlmostEqual(self.fixtures.CF2_POSE, samples[2].pose, places=3)
 
     def test_that_the_global_ref_frame_is_used(self):
         # Fixture
@@ -140,7 +140,7 @@ def test_that_the_global_ref_frame_is_used(self):
         bs_id0 = 3
         bs_id1 = 1
         bs_id2 = 2
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf2_bs0,
                 bs_id1: self.fixtures.angles_cf2_bs1,
@@ -149,19 +149,19 @@ def test_that_the_global_ref_frame_is_used(self):
                 bs_id1: self.fixtures.angles_cf1_bs1,
                 bs_id2: self.fixtures.angles_cf1_bs2,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        actual, cleaned_samples = LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
         self.assertPosesAlmostEqual(
-            Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(1.0, 3.0, 3.0)), actual.bs_poses[bs_id0], places=3)
+            Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2), t_vec=(1.0, 3.0, 3.0)), solution.bs_poses[bs_id0], places=3)
         self.assertPosesAlmostEqual(
-            Pose.from_rot_vec(R_vec=(0.0, 0.0, 0.0), t_vec=(-2.0, 1.0, 3.0)), actual.bs_poses[bs_id1], places=3)
+            Pose.from_rot_vec(R_vec=(0.0, 0.0, 0.0), t_vec=(-2.0, 1.0, 3.0)), solution.bs_poses[bs_id1], places=3)
         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)
+            Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi), t_vec=(2.0, 1.0, 3.0)), solution.bs_poses[bs_id2], places=3)
 
     def test_that_solution_failes_for_isolated_bs(self):
         # Fixture
@@ -169,7 +169,7 @@ def test_that_solution_failes_for_isolated_bs(self):
         bs_id1 = 1
         bs_id2 = 2
         bs_id3 = 4
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf_origin_bs0,
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
@@ -178,14 +178,14 @@ def test_that_solution_failes_for_isolated_bs(self):
                 bs_id2: self.fixtures.angles_cf1_bs2,
                 bs_id3: self.fixtures.angles_cf2_bs2,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        assert self.solution.progress_is_ok is False
+        assert solution.progress_is_ok is False
 
     def test_that_link_count_is_right(self):
         # Fixture
@@ -193,7 +193,7 @@ def test_that_link_count_is_right(self):
         bs_id1 = 1
         bs_id2 = 2
         bs_id3 = 4
-        samples = [
+        samples = self.augment_and_wrap([
             LhCfPoseSample(angles_calibrated={
                 bs_id0: self.fixtures.angles_cf_origin_bs0,
                 bs_id1: self.fixtures.angles_cf_origin_bs1,
@@ -208,14 +208,14 @@ def test_that_link_count_is_right(self):
                 bs_id2: self.fixtures.angles_cf2_bs2,
                 bs_id3: self.fixtures.angles_cf2_bs3,
             }),
-        ]
-        self.augment(samples)
+        ])
+        solution = LighthouseGeometrySolution(samples)
 
         # Test
-        LighthouseInitialEstimator.estimate(samples, self.solution)
+        LighthouseInitialEstimator.estimate(samples, solution)
 
         # Assert
-        assert self.solution.link_count == {
+        assert 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},
@@ -224,6 +224,9 @@ def test_that_link_count_is_right(self):
 
 # helpers
 
-    def augment(self, samples):
+    def augment_and_wrap(self, samples: list[LhCfPoseSample]) -> list[LhCfPoseSampleWrapper]:
+        wrapped_samples = []
         for sample in samples:
             sample.augment_with_ippe(LhDeck4SensorPositions.positions)
+            wrapped_samples.append(LhCfPoseSampleWrapper(sample))
+        return wrapped_samples
diff --git a/test/localization/test_lighthouse_sample_matcher.py b/test/localization/test_lighthouse_sample_matcher.py
deleted file mode 100644
index 225be6f85..000000000
--- a/test/localization/test_lighthouse_sample_matcher.py
+++ /dev/null
@@ -1,82 +0,0 @@
-# -*- coding: utf-8 -*-
-#
-# ,---------,       ____  _ __
-# |  ,-^-,  |      / __ )(_) /_______________ _____  ___
-# | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
-# | / ,--'  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
-#    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
-#
-# Copyright (C) 2021 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 unittest
-
-from cflib.localization.lighthouse_bs_vector import LighthouseBsVector
-from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher
-from cflib.localization.lighthouse_types import LhMeasurement
-
-
-class TestLighthouseSampleMatcher(unittest.TestCase):
-    def setUp(self):
-        self.vec0 = LighthouseBsVector(0.0, 0.0)
-        self.vec1 = LighthouseBsVector(0.1, 0.1)
-        self.vec2 = LighthouseBsVector(0.2, 0.2)
-        self.vec3 = LighthouseBsVector(0.3, 0.3)
-
-        self.samples = [
-            LhMeasurement(timestamp=1.000, base_station_id=0, angles=self.vec0),
-            LhMeasurement(timestamp=1.015, base_station_id=1, angles=self.vec1),
-            LhMeasurement(timestamp=1.020, base_station_id=0, angles=self.vec2),
-            LhMeasurement(timestamp=1.035, base_station_id=1, angles=self.vec3),
-        ]
-
-    def test_that_samples_are_aggregated(self):
-        # Fixture
-
-        # Test
-        actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010)
-
-        # Assert
-        self.assertEqual(1.000, actual[0].timestamp)
-        self.assertEqual(1, len(actual[0].angles_calibrated))
-        self.assertEqual(self.vec0, actual[0].angles_calibrated[0])
-
-        self.assertEqual(1.015, actual[1].timestamp)
-        self.assertEqual(2, len(actual[1].angles_calibrated))
-        self.assertEqual(self.vec1, actual[1].angles_calibrated[1])
-        self.assertEqual(self.vec2, actual[1].angles_calibrated[0])
-
-        self.assertEqual(1.035, actual[2].timestamp)
-        self.assertEqual(1, len(actual[2].angles_calibrated))
-        self.assertEqual(self.vec3, actual[2].angles_calibrated[1])
-
-    def test_that_single_bs_samples_are_fitered_out(self):
-        # Fixture
-
-        # Test
-        actual = LighthouseSampleMatcher.match(self.samples, max_time_diff=0.010, min_nr_of_bs_in_match=2)
-
-        # Assert
-        self.assertEqual(1.015, actual[0].timestamp)
-        self.assertEqual(2, len(actual[0].angles_calibrated))
-        self.assertEqual(self.vec1, actual[0].angles_calibrated[1])
-        self.assertEqual(self.vec2, actual[0].angles_calibrated[0])
-
-    def test_that_empty_sample_list_works(self):
-        # Fixture
-
-        # Test
-        actual = LighthouseSampleMatcher.match([])
-
-        # Assert
-        self.assertEqual(0, len(actual))

From 8ac1a9b188c9a657bddebc87a1c6c85568625602 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Sat, 19 Jul 2025 13:57:29 +0200
Subject: [PATCH 55/55] removed np.float_

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

diff --git a/cflib/localization/lighthouse_cf_pose_sample.py b/cflib/localization/lighthouse_cf_pose_sample.py
index 2b4fec2bf..efe156313 100644
--- a/cflib/localization/lighthouse_cf_pose_sample.py
+++ b/cflib/localization/lighthouse_cf_pose_sample.py
@@ -31,7 +31,7 @@
 from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors
 from cflib.localization.lighthouse_types import Pose
 
-ArrayFloat = npt.NDArray[np.float_]
+ArrayFloat = npt.NDArray[np.float64]
 
 
 class BsPairPoses(NamedTuple):
diff --git a/cflib/localization/lighthouse_geo_estimation_manager.py b/cflib/localization/lighthouse_geo_estimation_manager.py
index f11195d41..709c92ab3 100644
--- a/cflib/localization/lighthouse_geo_estimation_manager.py
+++ b/cflib/localization/lighthouse_geo_estimation_manager.py
@@ -45,7 +45,7 @@
 from cflib.localization.lighthouse_utils import LighthouseCrossingBeam
 
 
-ArrayFloat = npt.NDArray[np.float_]
+ArrayFloat = npt.NDArray[np.float64]
 
 
 class LhGeoEstimationManager():
@@ -416,7 +416,7 @@ def yaml_representer(dumper, data: LhGeoInputContainerData):
     @staticmethod
     def yaml_constructor(loader, node):
         values = loader.construct_mapping(node, deep=True)
-        sensor_positions = np.array(values['sensor_positions'], dtype=np.float_)
+        sensor_positions = np.array(values['sensor_positions'], dtype=np.float64)
         result = LhGeoInputContainerData(sensor_positions)
 
         result.origin = values['origin'] if 'origin' in values else LhGeoInputContainerData.EMPTY_POSE_SAMPLE
diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py
index 45a46b206..4cc88a3f9 100644
--- a/cflib/localization/lighthouse_initial_estimator.py
+++ b/cflib/localization/lighthouse_initial_estimator.py
@@ -34,7 +34,7 @@
 from cflib.localization.lighthouse_types import Pose
 
 
-ArrayFloat = npt.NDArray[np.float_]
+ArrayFloat = npt.NDArray[np.float64]
 
 
 class BsPairIds(NamedTuple):