def make_instance(self, *args, **kwargs): states = [ ts.TrackingState.NOT_INITIALIZED, ts.TrackingState.OK, ts.TrackingState.LOST ] kwargs = du.defaults( kwargs, { 'system_id': np.random.randint(10, 20), 'trajectory': { np.random.uniform(0, 600): tf.Transform( location=np.random.uniform(-1000, 1000, 3), rotation=np.random.uniform(0, 1, 4)) for _ in range(100) }, 'ground_truth_trajectory': { np.random.uniform(0, 600): tf.Transform( location=np.random.uniform(-1000, 1000, 3), rotation=np.random.uniform(0, 1, 4)) for _ in range(100) }, 'tracking_stats': { np.random.uniform(0, 600): states[np.random.randint( 0, len(states))] for _ in range(100) }, 'sequence_type': core.sequence_type.ImageSequenceType.SEQUENTIAL, 'system_settings': { 'a': np.random.randint(20, 30) } }) return vs.SLAMTrialResult(*args, **kwargs)
def test_matches_too_close_are_trivial_matches(self): trajectory = { 1.33333: tf.Transform(location=(100, 100, 0), rotation=(0, 0, 0, 1)), 3.66667: tf.Transform(location=(100, 100, 0), rotation=(0, 0, 0, 1)), 14.33333: tf.Transform(location=(-100, 100, 0), rotation=(0, 0, 0, 1)), 15.66667: tf.Transform(location=(-100, 100, 0), rotation=(0, 0, 0, 1)) } closures = {3.66667: 1.33333} trial_result = MockTrialResult(gt_trajectory=trajectory, loop_closures=closures) # Perform the benchmark with a larger threshold benchmark = lc.BenchmarkLoopClosure(distance_threshold=20, trivial_closure_index_distance=10) result = benchmark.benchmark_results(trial_result) self.assertEqual(match_res.MatchType.FALSE_POSITIVE, result.matches[3.66667]) self.assertEqual(match_res.MatchType.TRUE_NEGATIVE, result.matches[15.66667]) # Try again with a smaller threshold, should become a true positive, # since the indexes are further appart than the threshold benchmark = lc.BenchmarkLoopClosure(distance_threshold=20, trivial_closure_index_distance=1) result = benchmark.benchmark_results(trial_result) self.assertEqual(match_res.MatchType.TRUE_POSITIVE, result.matches[3.66667]) self.assertEqual(match_res.MatchType.FALSE_NEGATIVE, result.matches[15.66667])
def test_distance_threshold_determines_acceptable_matches(self): trajectory = { 1.33333: tf.Transform(location=(100, 100, 0), rotation=(0, 0, 0, 1)), 10.66667: tf.Transform(location=(110, 100, 0), rotation=(0, 0, 0, 1)), 15.33333: tf.Transform(location=(-100, 100, 0), rotation=(0, 0, 0, 1)), 20.66667: tf.Transform(location=(-110, 100, 0), rotation=(0, 0, 0, 1)) } closures = {10.66667: 1.33333} trial_result = MockTrialResult(gt_trajectory=trajectory, loop_closures=closures) # Perform the benchmark with a larger threshold benchmark = lc.BenchmarkLoopClosure(distance_threshold=20) result = benchmark.benchmark_results(trial_result) self.assertEqual(match_res.MatchType.TRUE_POSITIVE, result.matches[10.66667]) self.assertEqual(match_res.MatchType.FALSE_NEGATIVE, result.matches[20.66667]) # Try again with a smaller threshold, should become a false positive benchmark = lc.BenchmarkLoopClosure(distance_threshold=1) result = benchmark.benchmark_results(trial_result) self.assertEqual(match_res.MatchType.FALSE_POSITIVE, result.matches[10.66667]) self.assertEqual(match_res.MatchType.TRUE_NEGATIVE, result.matches[20.66667])
def test_set(self): a = imeta.LabelledObject(class_names=('class_1', ), bounding_box=(152, 239, 14, 78), label_color=(127, 33, 67), relative_pose=tf.Transform( location=(123, -45, 23), rotation=(0.5, 0.23, 0.1)), object_id='LabelledObject-18569') b = imeta.LabelledObject(class_names=('class_2', ), bounding_box=(39, 169, 96, 16), label_color=(2, 227, 34), relative_pose=tf.Transform(location=(-246, 468, 4), rotation=(0.2, 0.3, 0.4)), object_id='LabelledObject-68478') c = imeta.LabelledObject(class_names=('class_3', ), bounding_box=(148, 468, 82, 241), label_color=(12, 82, 238), relative_pose=tf.Transform( location=(85, -648, -376), rotation=(0.8, -0.64, -0.73)), object_id='LabelledObject-87684') subject_set = {a, a, a, b} self.assertEqual(2, len(subject_set)) self.assertIn(a, subject_set) self.assertIn(b, subject_set) self.assertNotIn(c, subject_set)
def begin(self): """ Start producing images. This will trigger any necessary startup code, and will allow get_next_image to be called. Return False if there is a problem starting the source. :return: True iff we have successfully started iteration """ if self._simulator is None: return False self._simulator.begin() self._current_index = 0 self._velocity = np.random.uniform((-1, -1, 0), (1, 1, 0), 3) self._velocity = self._max_speed * self._velocity / np.linalg.norm(self._velocity) # Set the initial facing direction to point in a random direction and random reachable location for _ in range(4): self._simulator.move_camera_to( tf.Transform(location=self._simulator.current_pose.location + (np.random.randint(-1000, 1000), np.random.randint(-1000, 1000), 0))) self._simulator.move_camera_to( tf.Transform(location=self._simulator.current_pose.location + (np.random.randint(-1000, 1000), np.random.randint(-1000, 1000), 0), rotation=tf3d.quaternions.axangle2quat((0, 0, 1), np.random.uniform(-np.pi, np.pi)), w_first=True))
def test_hash(self): kwargs = { 'class_names': ('class_1', ), 'bounding_box': (152, 239, 14, 78), 'label_color': (127, 33, 67), 'relative_pose': tf.Transform(location=(123, -45, 23), rotation=(0.5, 0.23, 0.1)), 'object_id': 'LabelledObject-18569' } a = imeta.LabelledObject(**kwargs) b = imeta.LabelledObject(**kwargs) self.assertEqual(hash(a), hash(b)) b = imeta.LabelledObject( **du.defaults({'class_names': 'class_41'}, kwargs)) self.assertNotEqual(hash(a), hash(b)) b = imeta.LabelledObject( **du.defaults({'bounding_box': (47, 123, 45, 121)}, kwargs)) self.assertNotEqual(hash(a), hash(b)) b = imeta.LabelledObject( **du.defaults({'label_color': (247, 123, 14)}, kwargs)) self.assertNotEqual(hash(a), hash(b)) b = imeta.LabelledObject(**du.defaults( {'relative_pose': tf.Transform((62, -81, 43), (0.1, 0.1, 0.1))}, kwargs)) self.assertNotEqual(hash(a), hash(b)) b = imeta.LabelledObject( **du.defaults({'object_id': 'Cat-12'}, kwargs)) self.assertNotEqual(hash(a), hash(b))
def test_find_relative_pose_changes_location_coordinates(self): pose = trans.Transform(location=(11, 12, 13)) tf = trans.Transform(location=(10, 9, 8), rotation=_make_quat((0, 0, 1), np.pi / 2), w_first=True) pose_rel = tf.find_relative(pose) self.assert_close(pose_rel.location, (3, -1, 5))
def test_constructor_clone(self): tf1 = trans.Transform(location=(1, 2, 3), rotation=(4, 5, 6, 7)) tf2 = trans.Transform(tf1) self.assert_array(tf1.location, tf2.location) self.assert_array(tf1.rotation_quat(w_first=True), tf2.rotation_quat(w_first=True)) self.assert_array(tf1.transform_matrix, tf2.transform_matrix)
def test_reads_relative_to_first_pose(self): first_pose = tf.Transform(location=(15.2, -1167.9, -1.2), rotation=(0.535, 0.2525, 0.11, 0.2876)) relative_trajectory = {} trajectory_text = "" for time in np.arange(0, 10, 0.45): pose = tf.Transform(location=(0.122 * time, -0.53112 * time, 1.893 * time), rotation=(0.772 * time, -0.8627 * time, -0.68782 * time)) relative_trajectory[time] = pose absolute_pose = first_pose.find_independent(pose) quat = absolute_pose.rotation_quat(w_first=True) trajectory_text += "{time},{x},{y},{z},{qw},{qx},{qy},{qz}\n".format( time=repr(time), x=repr(-1 * absolute_pose.location[1]), y=repr(-1 * absolute_pose.location[2]), z=repr(absolute_pose.location[0]), qw=repr(quat[0]), qx=repr(-1 * quat[2]), qy=repr(-1 * quat[3]), qz=repr(quat[1]) ) self.assertEqual(time, float(repr(time))) mock_open = mock.mock_open(read_data=trajectory_text) extend_mock_open(mock_open) with mock.patch('dataset.euroc.euroc_loader.open', mock_open, create=True): trajectory = euroc_loader.read_trajectory('test_filepath') self.assertEqual(len(trajectory), len(relative_trajectory)) for time, pose in relative_trajectory.items(): self.assertIn(time, trajectory) self.assertNPClose(pose.location, trajectory[time].location) self.assertNPClose(pose.rotation_quat(True), trajectory[time].rotation_quat(True))
def create_noise(trajectory, random_state, time_offset=0, time_noise=0.01, loc_noise=100, rot_noise=np.pi / 64): if not isinstance(loc_noise, np.ndarray): loc_noise = np.array([loc_noise, loc_noise, loc_noise]) noise = {} for time, pose in trajectory.items(): noise[time] = tf.Transform( location=random_state.uniform(-loc_noise, loc_noise), rotation=tf3d.quaternions.axangle2quat( random_state.uniform(-1, 1, 3), random_state.uniform(-rot_noise, rot_noise)), w_first=True) relative_frame = tf.Transform(location=random_state.uniform( -1000, 1000, 3), rotation=random_state.uniform(0, 1, 4)) changed_trajectory = {} for time, pose in trajectory.items(): relative_pose = relative_frame.find_relative(pose) noisy_time = time + time_offset + random_state.uniform( -time_noise, time_noise) noisy_pose = relative_pose.find_independent(noise[time]) changed_trajectory[noisy_time] = noisy_pose return changed_trajectory, noise
def test_down_is_down_vector(self): pose = trans.Transform(rotation=_make_quat((1, 0, 0), np.pi / 4), w_first=True) self.assert_close((0, np.sqrt(2) / 2, -np.sqrt(2) / 2), pose.down) pose = trans.Transform(rotation=_make_quat((0, 1, 0), np.pi / 4), w_first=True) self.assert_close((-np.sqrt(2) / 2, 0, -np.sqrt(2) / 2), pose.down)
def test_right_is_left_vector(self): pose = trans.Transform(rotation=_make_quat((1, 0, 0), np.pi / 4), w_first=True) self.assert_close((0, -np.sqrt(2) / 2, -np.sqrt(2) / 2), pose.right) pose = trans.Transform(rotation=_make_quat((0, 0, 1), np.pi / 4), w_first=True) self.assert_close((np.sqrt(2) / 2, -np.sqrt(2) / 2, 0), pose.right)
def test_forward_is_forward_vector(self): pose = trans.Transform(rotation=_make_quat((0, 1, 0), np.pi / 4), w_first=True) self.assert_close((np.sqrt(2) / 2, 0, -np.sqrt(2) / 2), pose.forward) pose = trans.Transform(rotation=_make_quat((0, 0, 1), np.pi / 4), w_first=True) self.assert_close((np.sqrt(2) / 2, np.sqrt(2) / 2, 0), pose.forward)
def test_down_is_independent_of_location(self): quat = _make_quat((13, -4, 5), 13 * np.pi / 27) pose = trans.Transform(rotation=quat, w_first=True) down = pose.down for _ in range(10): pose = trans.Transform(location=np.random.uniform(-1000, 1000, 3), rotation=quat, w_first=True) self.assert_array(down, pose.down)
def test_find_independent_pose_changes_orientation(self): pose = trans.Transform(location=(11, 12, 13), rotation=_make_quat((1, 0, 0), np.pi / 4), w_first=True) tf = trans.Transform(location=(10, 9, 8), rotation=_make_quat((0, 0, 1), np.pi / 2), w_first=True) pose_rel = tf.find_independent(pose) self.assert_close(pose_rel.euler, (0, np.pi / 4, np.pi / 2))
def test_euler_each_axis(self): # Yaw qrot = _make_quat((0, 0, 1), np.pi / 6) tf = trans.Transform(rotation=qrot, w_first=True) self.assert_array(tf.euler, np.array([0, 0, np.pi / 6])) # Pitch qrot = _make_quat((0, 1, 0), np.pi / 6) tf = trans.Transform(rotation=qrot, w_first=True) self.assert_array(tf.euler, np.array([0, np.pi / 6, 0])) # Roll qrot = _make_quat((1, 0, 0), np.pi / 6) tf = trans.Transform(rotation=qrot, w_first=True) self.assert_array(tf.euler, np.array([np.pi / 6, 0, 0]))
def get_circle_pitch_trajectory(): """ Get a trajectory that moves in a 1m diameter circle, facing outwards :return: """ framerate = 30 # frames per second angular_vel = np.pi / 36 # radians per second relative_pose = tf.Transform(location=(0.5, 0, 0), rotation=(0, 0, 0)) return { time: (tf.Transform(rotation=(0, time * angular_vel, 0))).find_independent(relative_pose) for time in np.arange(0, 2 * np.pi / angular_vel, 1 / framerate) }
def test_relative_pose_contains_relative_point(self): loc = (-13, 27, -127) qrot = _make_quat(np.array((-1, 0.1, -0.37)), 7 * np.pi / 26) tf = trans.Transform(location=loc, rotation=qrot, w_first=True) point = np.array([41, -153, 16]) pose = trans.Transform(location=point, rotation=_make_quat((0, 1, 0), np.pi / 4), w_first=True) pose_rel = tf.find_relative(pose) point_rel = pose_rel.location point_prime = tf.find_independent(point_rel) self.assert_close(point, point_prime)
def test_find_relative_undoes_pose(self): loc = (-13, 27, -127) qrot = _make_quat(np.array((-1, 0.1, -0.37)), 7 * np.pi / 26) tf = trans.Transform(location=loc, rotation=qrot, w_first=True) pose = trans.Transform(location=(10, 100, -5), rotation=_make_quat((0, 1, 0), np.pi / 4), w_first=True) pose_rel = tf.find_relative(pose) pose_prime = tf.find_independent(pose_rel) self.assert_close(pose_prime.location, pose.location) self.assert_close(pose_prime.rotation_quat(w_first=True), pose.rotation_quat(w_first=True))
def test_hash(self): tf1 = trans.Transform(location=(1, 2, 3), rotation=(-0.5, 0.5, 0.5, -0.5)) tf2 = trans.Transform(location=(1, 2, 3), rotation=(-0.5, 0.5, 0.5, -0.5)) tf3 = trans.Transform(location=(1, 2, 4), rotation=(-0.5, 0.5, 0.5, -0.5)) tf4 = trans.Transform(location=(1, 2, 3), rotation=(0.1, 0.2, 0.3, 0.4)) self.assertEqual(hash(tf1), hash(tf1)) self.assertEqual(hash(tf1), hash(tf2)) self.assertNotEqual(hash(tf1), hash(tf3)) self.assertNotEqual(hash(tf1), hash(tf4)) self.assertEqual({tf1, tf1, tf1}, {tf1}) # Set literals
def test_to_unreal_preserves_find_relative_simple(self): pose1 = mytf.Transform((73, -47, -23), (0.92387953, 0, 0.38268343, 0), w_first=True) pose2 = mytf.Transform((-19, 83, -61), (0.5, -0.5, 0.5, -0.5), w_first=True) relative_pose = pose1.find_relative(pose2) ue_pose1 = uetrans.transform_to_unreal(pose1) ue_pose2 = uetrans.transform_to_unreal(pose2) ue_relative_pose = ue_pose1.find_relative(ue_pose2) relative_pose_prime = uetrans.transform_from_unreal(ue_relative_pose) self.assert_close(relative_pose.location, relative_pose_prime.location) self.assert_close(relative_pose.rotation_quat(), relative_pose_prime.rotation_quat())
def test_pitch_to_unreal(self): # This pose is rotated 45 degrees clockwise around right (neg Y) pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((0, -1, 0), np.pi / 4), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((0, 45, 0), ue_pose.euler) # This pose is rotated 30 degrees anticlockwise around right (neg Y) pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((0, -1, 0), -np.pi / 6), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((0, -30, 0), ue_pose.euler)
def create_random_trajectory(random_state, duration=600, length=100): return { random_state.uniform(0, duration): tf.Transform(location=random_state.uniform(-1000, 1000, 3), rotation=random_state.uniform(0, 1, 4)) for _ in range(length) }
def create_mock_image(width=128, height=128): """ Make an image by drawing a bunch of semi-transparent squares This should actually have discernible features, in contrast to pure noise :param width: :param height: :return: """ im = np.zeros((width, height, 3), dtype='uint8') for _ in range(10): shape = (np.random.randint(width), np.random.randint(height)) position = (np.random.randint(1 - shape[0], width), np.random.randint(1 - shape[1], height)) colour = np.random.randint(0, 256, 3, dtype='uint8') im[max(0, position[1]):min(height, position[1] + shape[1]), max(0, position[0]):min(width, position[0] + shape[0]), :] += colour metadata = imeta.ImageMetadata( source_type=imeta.ImageSourceType.SYNTHETIC, hash_=b'\x1f`\xa8\x8aR\xed\x9f\x0b', environment_type=imeta.EnvironmentType.INDOOR_CLOSE, light_level=imeta.LightingLevel.WELL_LIT, time_of_day=imeta.TimeOfDay.DAY, lens_focal_distance=100, aperture=22, camera_pose=tf.Transform()) return core.image_entity.ImageEntity(data=im, metadata=metadata, id_=bson.objectid.ObjectId())
def make_relative_pose(x: float, y: float, z: float, qx: float, qy: float, qz: float, qw: float) -> tf.Transform: """ ORBSLAM2 is using the common CV coordinate frame Z forward, X right, Y down (I think) this function handles the coordinate frame Frame is: z forward, x right, y down Not documented, worked out by trial and error :param x: The x coordinate :param y: The y coordinate :param z: The z coordinate :param qx: The quaternion x coordinate :param qy: The quaternion y coordinate :param qz: The quaternion z coordinate :param qw: The quaternion w coordinate :return: A Transform object representing the pose of the current frame with respect to the previous frame """ tf_matrix = np.matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]]) tf_matrix[0:3, 0:3] = tf3d.quaternions.quat2mat((qw, qx, qy, qz)) coordinate_exchange = np.matrix([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) pose = np.dot(np.dot(coordinate_exchange, tf_matrix), coordinate_exchange.T) return tf.Transform(pose)
def __init__(self, location=None, rotation=None): """ :param location: A 4x4 homogenous transformation matrix, Transform object, or and 3-indexable tuple :param rotation: Any 3-indexable tuple listing rotation in degrees, order (roll, pitch, yaw) """ if isinstance(location, np.ndarray) and location.shape == (4, 4): location = mytf.Transform(location) if isinstance(location, mytf.Transform): location = transform_to_unreal(location) if isinstance(location, UnrealTransform): self._x = location.x self._y = location.y self._z = location.z self._roll = location.roll self._pitch = location.pitch self._yaw = location.yaw else: if location is not None and len(location) >= 3: self._x, self._y, self._z = location else: self._x = self._y = self._z = 0 if rotation is not None and len(rotation) >= 3: self._roll, self._pitch, self._yaw = rotation else: self._roll = self._pitch = self._yaw = 0
def test_benchmark_results_returns_a_benchmark_result(self): trial_result = MockTrialResult(gt_trajectory={ 1.33333: tf.Transform(location=(100, 100, 0), rotation=(0, 0, 0, 1)), 10.66667: tf.Transform(location=(100, 100, 0), rotation=(0, 0, 0, 1)) }, loop_closures={}) benchmark = lc.BenchmarkLoopClosure(distance_threshold=20) result = benchmark.benchmark_results(trial_result) self.assertIsInstance(result, core.benchmark.BenchmarkResult) self.assertNotIsInstance(result, core.benchmark.FailedBenchmark) self.assertIsInstance(result, match_res.MatchBenchmarkResult) self.assertEqual(benchmark.identifier, result.benchmark) self.assertEqual(trial_result.identifier, result.trial_result)
def test_roll_to_unreal(self): # This pose is rotated 45 degrees clockwise around X pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((1, 0, 0), np.pi / 4), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((45, 0, 0), ue_pose.euler) # And this is 30 degrees anticlockwise around X pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((1, 0, 0), -np.pi / 6), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((-30, 0, 0), ue_pose.euler)
def make_instance(self, *args, **kwargs): kwargs = du.defaults( kwargs, { 'system_id': np.random.randint(10, 20), 'keypoints': self.keypoints, 'sequence_type': core.sequence_type.ImageSequenceType.SEQUENTIAL, 'system_settings': { 'a': np.random.randint(20, 30) }, 'keypoints_id': bson.ObjectId() }) if 'timestamps' not in kwargs: kwargs['timestamps'] = { idx + np.random.uniform(0, 1): identifier for idx, identifier in enumerate(kwargs['keypoints'].keys()) } if 'camera_poses' not in kwargs: kwargs['camera_poses'] = { identifier: tf.Transform(location=np.random.uniform(-1000, 1000, 3), rotation=np.random.uniform(-1, 1, 4)) for identifier in kwargs['keypoints'].keys() } return feature_result.FeatureDetectorResult(*args, **kwargs)
def test_yaw_to_unreal(self): # This pose is rotated 45 degrees clockwise around up (Z) pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((0, 0, 1), np.pi / 4), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((0, 0, -45), ue_pose.euler) # This pose is rotated 30 degrees anticlockwise around up pose = mytf.Transform(location=(0, 0, 0), rotation=tf.quaternions.axangle2quat((0, 0, 1), -np.pi / 6), w_first=True) ue_pose = uetrans.transform_to_unreal(pose) self.assert_close((0, 0, 30), ue_pose.euler)