Merge pull request #324 from bitcraze/krichardsson/multi-bs-estimation

Improved multi bs estimation

cflib/__init__.py

@@ -55,5 +55,4 @@
 ```
 """
 __pdoc__ = {}
-__pdoc__['cflib.localization'] = False
 __pdoc__['cflib.crtp.cflinkcppdriver'] = False

cflib/localization/lighthouse_bs_vector.py

@@ -138,11 +138,10 @@ def _q(self):
         return math.tan(self._lh_v1_vert_angle) / math.sqrt(1 + math.tan(self._lh_v1_horiz_angle) ** 2)
 
 
-# A LighthouseBsVectors is a list of 4 LighthouseBsVector, one for
-# each sensor
-# LighthouseBsVectors = list[LighthouseBsVector]
-
 class LighthouseBsVectors(list):
+    """A list of 4 LighthouseBsVector, one for each sensor.
+       LighthouseBsVectors is essentially the same as list[LighthouseBsVector]"""
+
     def projection_pair_list(self) -> npt.NDArray:
         """
         Genereate a list of projection pairs for all vectors

cflib/localization/lighthouse_geometry_solver.py

@@ -131,7 +131,6 @@ class LighthouseGeometrySolver:
     cf2/bs3/sens1/ang0                                  X                  X
     cf2/bs3/sens1/ang1                                  X                  X
     ...
-
     """
 
     @classmethod
@@ -141,8 +140,8 @@ 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 after a fixed number of iteration if acceptable solution
-        is found.
+        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.
 
         :param initial_guess: Initial guess for the base stations and CF sample poses
         :param matched_samples: List of matched samples.
@@ -155,7 +154,7 @@ def solve(cls, initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample]
         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._crate_bs_map(initial_guess.bs_poses)
+        solution.bs_id_to_index, solution.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(
@@ -330,7 +329,7 @@ def _calc_angle_pairs(cls, bs_p_a, cf_p_a, sens_pos_p_a, defs: LighthouseGeometr
 
         :param bs_p_a: Poses base stations
         :param cf_p_a: Poses CFs
-        :paran sens_pos_p_a: Sensor positions
+        :param sens_pos_p_a: Sensor positions
         :return: angle pairs
 
         All lists are equally long, one entry per output angle pair
@@ -378,7 +377,7 @@ def _params_to_pose(cls, params: npt.ArrayLike, defs: LighthouseGeometrySolution
         return Pose.from_rot_vec(R_vec=r_vec, t_vec=t)
 
     @classmethod
-    def _crate_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[int, int], dict[int, int]]:
+    def _create_bs_map(cls, initial_guess_bs_poses: dict[int, Pose]) -> tuple[dict[int, int], dict[int, int]]:
         """
         We might have gaps in the list of base station ids that is used in the system, use an index instead
         when refering to a base station. This method creates dictionaries to go from index to base station id,
@@ -427,7 +426,7 @@ def _condense_results(cls, lsq_result, solution: LighthouseGeometrySolution,
         solution.error_info = cls._aggregate_error_info(solution.estimated_errors)
 
     @classmethod
-    def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]):
+    def _aggregate_error_info(cls, estimated_errors: list[dict[int, float]]) -> dict[str, float]:
         error_per_bs = {}
         errors = []
         for sample_errors in estimated_errors:

cflib/localization/lighthouse_sample_matcher.py

@@ -36,6 +36,9 @@ class LighthouseSampleMatcher:
     @classmethod
     def match(cls, samples: list[LhMeasurement], max_time_diff: float = 0.010,
               min_nr_of_bs_in_match: int = 0) -> list[LhCfPoseSample]:
+        """
+        Aggregate samples close in time into lists
+        """
 
         result = []
         current: LhCfPoseSample = None

cflib/localization/lighthouse_system_aligner.py

@@ -29,9 +29,10 @@
 
 
 class LighthouseSystemAligner:
+    """This class is used to align a lighthouse system to a few sampled positions"""
     @classmethod
     def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: list[npt.ArrayLike],
-              bs_poses: dict[int, Pose]) -> dict[int, Pose]:
+              bs_poses: dict[int, Pose]) -> tuple[dict[int, Pose], Pose]:
         """
         Align a coordinate system with the physical world. Finds the transform from the
         current reference frame to one that is aligned with measured positions, and transforms base station
@@ -42,7 +43,7 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis
                        reference frame
         :param x_axis: One or more positions in the desired XY-plane (Z=0) in the current reference frame
         :param bs_poses: a dictionary with the base station poses in the current reference frame
-        :return: a dictionary with the base station poses in the desired reference frame
+        :return: a dictionary with the base station poses in the desired reference frame and the transformation
         """
         raw_transformation = cls._find_transformation(origin, x_axis, xy_plane)
         transformation = cls._de_flip_transformation(raw_transformation, x_axis, bs_poses)
@@ -51,7 +52,7 @@ def align(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike], xy_plane: lis
         for bs_id, pose in bs_poses.items():
             result[bs_id] = transformation.rotate_translate_pose(pose)
 
-        return result
+        return result, transformation
 
     @classmethod
     def _find_transformation(cls, origin: npt.ArrayLike, x_axis: list[npt.ArrayLike],

cflib/localization/lighthouse_system_scaler.py

@@ -0,0 +1,131 @@
+# -*- 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
+
+import copy
+
+import numpy as np
+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
+
+
+class LighthouseSystemScaler:
+    """This class is used to re-scale a system based on various measurements."""
+    @classmethod
+    def scale_fixed_point(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose], expected: npt.ArrayLike,
+                          actual: Pose) -> tuple[dict[int, Pose], list[Pose], float]:
+        """
+        Scale a system based on a position in the physical world in relation to where it is in the estimated system
+        geometry. Assume the system is aligned and simply use the distance to the points for scaling.
+
+        :param bs_poses: a dictionary with the base station poses in the current reference frame
+        :param cf_poses: List of CF poses
+        :param expected: The real world position to use as reference
+        :param actual: The estimated position in the current system geometry
+        :return: a tuple containing a dictionary with the base station poses in the scaled system,
+                 a list of Crazyflie poses in the scaled system and the scaling factor
+        """
+        expected_distance = np.linalg.norm(expected)
+        actual_distance = np.linalg.norm(actual.translation)
+        scale_factor = expected_distance / actual_distance
+        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],
+                        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
+        positions. Calculates the intersection points for all samples and scales the system to match the expected
+        distance between sensors on the deck.
+
+        :param bs_poses: a dictionary with the base station poses in the current reference frame
+        :param cf_poses: List of CF poses
+        :param matched_samples: List of samples. Length must be the same as cf_poses.
+        :return: a tuple containing a dictionary with the base station poses in the scaled system,
+                 a list of Crazyflie poses in the scaled system and the scaling factor
+        """
+
+        estimated_diagonal = cls._calculate_mean_diagonal(bs_poses, cf_poses, matched_samples)
+        scale_factor = expected_diagonal / estimated_diagonal
+        return cls._scale_system(bs_poses, cf_poses, scale_factor)
+
+    @classmethod
+    def _scale_system(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose],
+                      scale_factor: float) -> tuple[dict[int, Pose], list[Pose], float]:
+        """
+        Scale poses of base stations and crazyflie samples.
+        """
+        bs_scaled = {bs_id: copy.copy(pose) for bs_id, pose in bs_poses.items()}
+        for pose in bs_scaled.values():
+            pose.scale(scale_factor)
+
+        cf_scaled = [copy.copy(pose) for pose in cf_poses]
+        for pose in cf_scaled:
+            pose.scale(scale_factor)
+
+        return bs_scaled, cf_scaled, scale_factor
+
+    @classmethod
+    def _calculate_mean_diagonal(cls, bs_poses: dict[int, Pose], cf_poses: list[Pose],
+                                 matched_samples: list[LhCfPoseSample]) -> float:
+        """
+        Calculate the average diagonal sensor distance based on where the rays intersect the lighthouse deck
+        """
+        diagonals: list[float] = []
+
+        for cf_pose, sample in zip(cf_poses, matched_samples):
+            for bs_id, vectors in sample.angles_calibrated.items():
+                diagonals.append(cls.calc_intersection_distance(vectors[0], vectors[3], bs_poses[bs_id], cf_pose))
+                diagonals.append(cls.calc_intersection_distance(vectors[1], vectors[2], bs_poses[bs_id], cf_pose))
+
+        estimated_diagonal = np.mean(diagonals)
+
+        return estimated_diagonal
+
+    @classmethod
+    def calc_intersection_distance(cls, vector1: LighthouseBsVector, vector2: LighthouseBsVector,
+                                   bs_pose: Pose, cf_pose: Pose) -> float:
+        """Calculate distance between intersection points of rays on the plane defined by the lighthouse deck"""
+
+        intersection1 = cls.calc_intersection_point(vector1, bs_pose, cf_pose)
+        intersection2 = cls.calc_intersection_point(vector2, bs_pose, cf_pose)
+        distance = np.linalg.norm(intersection1 - intersection2)
+        return distance
+
+    @classmethod
+    def calc_intersection_point(cls, vector: LighthouseBsVector, bs_pose: Pose, cf_pose: Pose) -> npt.NDArray:
+        """Calculate the intersetion point of a lines and a plane.
+        The line is the intersection of the two light planes from a base station, while the
+        plane is defined by the lighthouse deck of the Crazyflie."""
+
+        plane_base = cf_pose.translation
+        plane_normal = np.dot(cf_pose.rot_matrix, (0.0, 0.0, 1.0))
+
+        line_base = bs_pose.translation
+        line_vector = np.dot(bs_pose.rot_matrix, vector.cart)
+
+        dist_on_line = np.dot((plane_base - line_base), plane_normal) / np.dot(line_vector, plane_normal)
+
+        return line_base + line_vector * dist_on_line

cflib/localization/lighthouse_types.py

@@ -36,6 +36,7 @@ class Pose:
 
     _NO_ROTATION_MTX = np.identity(3)
     _NO_ROTATION_VCT = np.array((0.0, 0.0, 0.0))
+    _NO_ROTATION_QUAT = np.array((0.0, 0.0, 0.0, 0.0))
     _ORIGIN = np.array((0.0, 0.0, 0.0))
 
     def __init__(self, R_matrix: npt.ArrayLike = _NO_ROTATION_MTX, t_vec: npt.ArrayLike = _ORIGIN) -> None:
@@ -52,6 +53,19 @@ def from_rot_vec(cls, R_vec: npt.ArrayLike = _NO_ROTATION_VCT, t_vec: npt.ArrayL
         """
         return Pose(Rotation.from_rotvec(R_vec).as_matrix(), t_vec)
 
+    @classmethod
+    def from_quat(cls, R_quat: npt.ArrayLike = _NO_ROTATION_QUAT, t_vec: npt.ArrayLike = _ORIGIN) -> 'Pose':
+        """
+        Create a Pose from a quaternion and translation vector
+        """
+        return Pose(Rotation.from_quat(R_quat).as_matrix(), t_vec)
+
+    def scale(self, scale) -> None:
+        """
+        quiet
+        """
+        self._t_vec = self._t_vec * scale
+
     @property
     def rot_matrix(self) -> npt.NDArray:
         """
@@ -66,6 +80,13 @@ def rot_vec(self) -> npt.NDArray:
         """
         return Rotation.from_matrix(self._R_matrix).as_rotvec()
 
+    @property
+    def rot_quat(self) -> npt.NDArray:
+        """
+        Get the quaternion of the pose
+        """
+        return Rotation.from_matrix(self._R_matrix).as_quat()
+
     @property
     def translation(self) -> npt.NDArray:
         """
@@ -159,3 +180,5 @@ class LhDeck4SensorPositions:
         (-_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0),
         (_sensor_distance_length / 2, _sensor_distance_width / 2, 0.0),
         (_sensor_distance_length / 2, -_sensor_distance_width / 2, 0.0)])
+
+    diagonal_distance = np.sqrt(_sensor_distance_length ** 2 + _sensor_distance_length ** 2)