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)
