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
for sample in samples:
ts = sample.timestamp
if current is None:
current = LhCfPoseSample(timestamp=ts)
if ts > (current.timestamp + max_time_diff):
cls._append_result(current, result, min_nr_of_bs_in_match)
current = LhCfPoseSample(timestamp=ts)
current.angles_calibrated[sample.base_station_id] = sample.angles
cls._append_result(current, result, min_nr_of_bs_in_match)
return result
def test_that_linked_bs_poses_in_multiple_samples_are_estimated(self):
# Fixture
# CF_ORIGIN is used in the first sample and will define the global reference frame
bs_id0 = 7
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,
}),
]
initial_guess = LighthouseInitialEstimator.estimate(matched_samples, LhDeck4SensorPositions.positions)
# Test
actual = LighthouseGeometrySolver.solve(
initial_guess, matched_samples, LhDeck4SensorPositions.positions)
# Assert
bs_poses = actual.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)
def record_angles_average(scf: SyncCrazyflie) -> LhCfPoseSample:
"""Record angles and average over the samples to reduce noise"""
recorded_angles = None
is_ready = Event()
def ready_cb(averages):
nonlocal recorded_angles
recorded_angles = averages
is_ready.set()
reader = LighthouseSweepAngleAverageReader(scf.cf, ready_cb)
reader.start_angle_collection()
is_ready.wait()
angles_calibrated = {}
for bs_id, data in recorded_angles.items():
angles_calibrated[bs_id] = data[1]
result = LhCfPoseSample(angles_calibrated=angles_calibrated)
visible = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys()))
print(f' Position recorded, base station ids visible: {visible}')
if len(recorded_angles.keys()) < 2:
print(
'Received too few base stations, we need at least two. Please try again!'
)
result = None
return result
def test_that_cf_poses_are_estimated(self):
# Fixture
# CF_ORIGIN is used in the first sample and will define the global reference frame
bs_id0 = 7
bs_id1 = 1
bs_id2 = 5
bs_id3 = 0
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,
}),
]
# Test
actual = LighthouseInitialEstimator.estimate(
samples, LhDeck4SensorPositions.positions)
# 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)
def test_that_one_bs_pose_raises_exception(self):
# Fixture
# CF_ORIGIN is used in the first sample and will define the global reference frame
bs_id = 3
samples = [
LhCfPoseSample(
angles_calibrated={bs_id: self.fixtures.angles_cf_origin_bs0}),
]
# Test
# Assert
with self.assertRaises(Exception):
LighthouseInitialEstimator.estimate(
samples, LhDeck4SensorPositions.positions)
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 test_that_raises_for_isolated_bs(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_bs2,
bs_id3: self.fixtures.angles_cf2_bs2,
}),
]
# Test
# Assert
with self.assertRaises(Exception):
LighthouseInitialEstimator.estimate(
samples, LhDeck4SensorPositions.positions)
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 = [
LhCfPoseSample(
angles_calibrated={
bs_id0: self.fixtures.angles_cf_origin_bs0,
bs_id1: self.fixtures.angles_cf_origin_bs1,
}),
]
# Test
actual = LighthouseInitialEstimator.estimate(
samples, LhDeck4SensorPositions.positions)
# 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)