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)