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)
x_axis = [(-1.0, 0.0 + 0.01, 0.0), (-2.0, 0.0 - 0.02, 0.0)]
xy_plane = [(2.0, 2.0, 0.0 + 0.02), (-3.0, -2.0, 0.0 + 0.01),
(5.0, 0.0, 0.0 - 0.01)]
# Note: Z of base stations must be positive (above the floor)
bs_poses = {
1: Pose.from_rot_vec(t_vec=(1.0, 0.0, 1.0)),
2: Pose.from_rot_vec(t_vec=(0.0, 1.0, 1.0)),
3: Pose.from_rot_vec(t_vec=(0.0, 0.0, 1.0)),
}
# Test
actual, transform = LighthouseSystemAligner.align(
origin, x_axis, xy_plane, bs_poses)
# Assert
self.assertVectorsAlmostEqual((-1.0, 0.0, 1.0),
actual[1].translation,
places=1)
self.assertVectorsAlmostEqual((0.0, -1.0, 1.0),
actual[2].translation,
places=1)
self.assertVectorsAlmostEqual((0.0, 0.0, 1.0),
actual[3].translation,
places=1)
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
"""
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_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:
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)
return transformation
def _convert_estimates_to_cf_reference_frame(
cls, estimates_ref_bs: list[IppeCf.Solution]) -> tuple[Pose, Pose]:
"""
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 Pose(rot_1, t_1), Pose(rot_2, t_2)
def test_rotate_translate_pose_and_back(self):
# Fixture
transform = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0),
t_vec=(0.1, 0.2, 0.3))
expected = Pose(t_vec=(2.0, 3.0, 4.0))
# Test
actual = transform.inv_rotate_translate_pose(
transform.rotate_translate_pose(expected))
# Assert
self.assertPosesAlmostEqual(expected, actual)
def test_rotate_translate_pose(self):
# Fixture
transform = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2),
t_vec=(1.0, 0.0, 0.0))
pose = Pose(t_vec=(2.0, 0.0, 0.0))
expected = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2),
t_vec=(1.0, 2.0, 0.0))
# Test
actual = transform.rotate_translate_pose(pose)
# Assert
self.assertPosesAlmostEqual(expected, actual)
def _condense_results(cls, lsq_result,
solution: LighthouseGeometrySolution,
matched_samples: list[LhCfPoseSample]) -> None:
bss, cf_poses = cls._params_to_struct(lsq_result.x, solution)
# Extract CF pose estimates
# First pose (origin) is not in the parameter list
solution.cf_poses.append(Pose())
for i in range(len(matched_samples) - 1):
solution.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.success = lsq_result.success
# Extract the error for each CF pose
residuals = lsq_result.fun
i = 0
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)
solution.error_info = cls._aggregate_error_info(
solution.estimated_errors)
def test_that_solution_is_de_flipped(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_id = 7
bs_poses = {bs_id: Pose.from_rot_vec(t_vec=(0.0, 0.0, 1.0))}
expected = 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, actual[bs_id])
def test_that_base_stations_are_rotated(self):
# Fixture
origin = (1.0, 0.0, 0.0)
x_axis = [(1.0, 1.0, 0.0)]
xy_plane = [(2.0, 1.0, 0.0)]
bs_id = 7
bs_poses = {bs_id: Pose.from_rot_vec(t_vec=(1.0, 0.0, 1.0))}
# Test
actual, transform = LighthouseSystemAligner.align(
origin, x_axis, xy_plane, bs_poses)
# Assert
self.assertPosesAlmostEqual(
Pose.from_rot_vec(R_vec=(0.0, 0.0, -np.pi / 2),
t_vec=(0.0, 0.0, 1.0)), actual[bs_id])
def _params_to_pose(cls, params: npt.ArrayLike,
defs: LighthouseGeometrySolution) -> Pose:
"""
Convert from the array format used in the solver to Pose
"""
r_vec = params[:defs.len_rot_vec]
t = params[defs.len_rot_vec:defs.len_pose]
return Pose.from_rot_vec(R_vec=r_vec, t_vec=t)
def test_default_rot_vec_constructor(self):
# Fixture
# Test
actual = Pose.from_rot_vec()
# Assert
self.assertEqual(0.0, np.linalg.norm(actual.translation))
self.assertEqual(0.0,
np.linalg.norm(actual.rot_matrix - np.identity(3)))
def _map_pose_to_ref_frame(cls, pose1_ref1: Pose, pose1_ref2: Pose,
pose2_ref2: Pose) -> Pose:
"""
Express pose2 in reference system 1, given pose1 in both reference system 1 and 2
"""
R_o2_in_1, t_o2_in_1 = cls._map_cf_pos_to_cf_pos(
pose1_ref1, pose1_ref2).matrix_vec
t = np.dot(R_o2_in_1, pose2_ref2.translation) + t_o2_in_1
R = np.dot(R_o2_in_1, pose2_ref2.rot_matrix)
return Pose(R, t)
def _map_cf_pos_to_cf_pos(cls, pose1_ref1: Pose, pose1_ref2: Pose) -> Pose:
"""
Find the rotation/translation from ref1 to ref2 given a pose,
that is the returned Pose will tell us where the origin in ref2 is,
expressed in ref1
"""
R_inv_ref2 = np.matrix.transpose(pose1_ref2.rot_matrix)
R = np.dot(pose1_ref1.rot_matrix, R_inv_ref2)
t = pose1_ref1.translation - np.dot(R, pose1_ref2.translation)
return Pose(R, t)
def test_rotate_translate(self):
# Fixture
transform = Pose.from_rot_vec(R_vec=(0.0, 0.0, np.pi / 2),
t_vec=(1.0, 0.0, 0.0))
point = (2.0, 0.0, 0.0)
# Test
actual = transform.rotate_translate(point)
# Assert
self.assertAlmostEqual(1.0, actual[0])
self.assertAlmostEqual(2.0, actual[1])
self.assertAlmostEqual(0.0, actual[2])
def test_rotate_translate_and_back(self):
# Fixture
transform = Pose.from_rot_vec(R_vec=(1.0, 2.0, 3.0),
t_vec=(0.1, 0.2, 0.3))
expected = (2.0, 3.0, 4.0)
# Test
actual = transform.inv_rotate_translate(
transform.rotate_translate(expected))
# Assert
self.assertAlmostEqual(expected[0], actual[0])
self.assertAlmostEqual(expected[1], actual[1])
self.assertAlmostEqual(expected[2], actual[2])
def test_that_the_global_ref_frame_is_used(self):
# Fixture
# CF2 is used in the first sample and will define the global reference frame
bs_id0 = 3
bs_id1 = 1
bs_id2 = 2
samples = [
LhCfPoseSample(
angles_calibrated={
bs_id0: self.fixtures.angles_cf2_bs0,
bs_id1: self.fixtures.angles_cf2_bs1,
}),
LhCfPoseSample(
angles_calibrated={
bs_id1: self.fixtures.angles_cf1_bs1,
bs_id2: self.fixtures.angles_cf1_bs2,
}),
]
# Test
actual = LighthouseInitialEstimator.estimate(
samples, LhDeck4SensorPositions.positions)
# 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)
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)
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 _avarage_poses(cls, poses: list[Pose]) -> Pose:
"""
Averaging of quaternions to get the "average" orientation of multiple samples.
From https://stackoverflow.com/a/61013769
"""
def q_average(Q, W=None):
if W is not None:
Q *= W[:, None]
eigvals, eigvecs = np.linalg.eig(Q.T @ Q)
return eigvecs[:, eigvals.argmax()]
positions = map(lambda x: x.translation, poses)
average_pos = np.average(np.array(list(positions)), axis=0)
quats = map(lambda x: x.rot_quat, poses)
average_quaternion = q_average(np.array(list(quats)))
return Pose.from_quat(R_quat=average_quaternion, t_vec=average_pos)
def _Pose_from_params(cls, params: npt.ArrayLike) -> Pose:
return Pose.from_rot_vec(R_vec=params[:3], t_vec=params[3:])