def test_is_different_with_changed_seed(self):
# Actually run the system using mocked images
num_frames = 50
max_time = 50
speed = 0.1
image_builder = DemoImageBuilder(
mode=ImageMode.MONOCULAR,
width=640, height=480, num_stars=150,
length=max_time * speed, speed=speed,
close_ratio=0.6, min_size=10, max_size=100
)
subject = LibVisOMonoSystem(motion_threshold=1000)
subject.set_camera_intrinsics(image_builder.get_camera_intrinsics(), max_time / num_frames)
subject.set_stereo_offset(image_builder.get_stereo_offset())
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=0)
for idx in range(num_frames):
time = max_time * idx / num_frames
image = image_builder.create_frame(time)
subject.process_image(image, time)
result1 = subject.finish_trial()
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=2)
for idx in range(num_frames):
time = max_time * idx / num_frames
image = image_builder.create_frame(time)
subject.process_image(image, time)
result2 = subject.finish_trial()
self.assertEqual(len(result1.results), len(result2.results))
different_tracking = 0
loc_diff = np.zeros(3)
quat_diff = np.zeros(4)
for frame_result_1, frame_result_2 in zip(result1.results, result2.results):
self.assertEqual(frame_result_1.timestamp, frame_result_2.timestamp)
if frame_result_1.tracking_state != frame_result_2.tracking_state:
different_tracking += 1
elif frame_result_1.estimated_motion is not None and frame_result_2.estimated_motion is not None:
motion1 = frame_result_1.estimated_motion
motion2 = frame_result_2.estimated_motion
loc_diff += np.abs(motion1.location - motion2.location)
quat_diff += np.abs(motion1.rotation_quat(True) - motion2.rotation_quat(True))
if different_tracking <= 0:
# If the tracking is the same, make sure the estimates are at least different
self.assertNotNPClose(loc_diff, np.zeros(3), rtol=0, atol=1e-10)
self.assertNotNPClose(quat_diff, np.zeros(4), rtol=0, atol=1e-10)
def test_is_consistent_with_fixed_seed(self):
# Actually run the system using mocked images
num_frames = 20
max_time = 50
speed = 0.1
image_builder = DemoImageBuilder(
mode=ImageMode.MONOCULAR,
width=640, height=480, num_stars=150,
length=max_time * speed, speed=speed,
close_ratio=0.6, min_size=10, max_size=100
)
subject = LibVisOMonoSystem(motion_threshold=1000)
subject.set_camera_intrinsics(image_builder.get_camera_intrinsics(), max_time / num_frames)
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=0)
for idx in range(num_frames):
time = max_time * idx / num_frames
image = image_builder.create_frame(time)
subject.process_image(image, time)
result1 = subject.finish_trial()
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=0)
for idx in range(num_frames):
time = max_time * idx / num_frames
image = image_builder.create_frame(time)
subject.process_image(image, time)
result2 = subject.finish_trial()
has_any_estimate = False
self.assertEqual(len(result1.results), len(result2.results))
for frame_result_1, frame_result_2 in zip(result1.results, result2.results):
self.assertEqual(frame_result_1.timestamp, frame_result_2.timestamp)
self.assertEqual(frame_result_1.tracking_state, frame_result_2.tracking_state)
if frame_result_1.estimated_motion is None or frame_result_2.estimated_motion is None:
self.assertEqual(frame_result_1.estimated_motion, frame_result_2.estimated_motion)
else:
has_any_estimate = True
motion1 = frame_result_1.estimated_motion
motion2 = frame_result_2.estimated_motion
loc_diff = motion1.location - motion2.location
self.assertNPClose(loc_diff, np.zeros(3), rtol=0, atol=1e-14)
quat_diff = motion1.rotation_quat(True) - motion2.rotation_quat(True)
self.assertNPClose(quat_diff, np.zeros(4), rtol=0, atol=1e-14)
self.assertTrue(has_any_estimate)
def test_simple_trial_run_generated(self):
sequence_folder, left_path, right_path = ndds_loader.find_files(
NDDS_SEQUENCE)
camera_intrinsics = ndds_loader.read_camera_intrinsics(
left_path / '_camera_settings.json')
max_img_id = ndds_loader.find_max_img_id(
lambda idx: left_path / ndds_loader.IMG_TEMPLATE.format(idx))
with (NDDS_SEQUENCE / 'timestamps.json').open('r') as fp:
timestamps = json.load(fp)
subject = LibVisOMonoSystem(
matcher_nms_n=10,
matcher_nms_tau=66,
matcher_match_binsize=50,
matcher_match_radius=245,
matcher_match_disp_tolerance=2,
matcher_outlier_disp_tolerance=5,
matcher_outlier_flow_tolerance=2,
matcher_multi_stage=False,
matcher_half_resolution=False,
matcher_refinement=MatcherRefinement.SUBPIXEL,
bucketing_max_features=6,
bucketing_bucket_width=136,
bucketing_bucket_height=102,
height=1.0,
pitch=0.0,
ransac_iters=439,
inlier_threshold=4.921875,
motion_threshold=609.375)
subject.set_camera_intrinsics(camera_intrinsics, 0.1)
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=0)
image_group = 'test'
with image_manager.get().get_group(image_group, allow_write=True):
for img_idx in range(max_img_id + 1):
pixels = image_utils.read_colour(
left_path / ndds_loader.IMG_TEMPLATE.format(img_idx))
image = Image(
_id=bson.ObjectId(),
pixels=pixels,
image_group=image_group,
metadata=imeta.ImageMetadata(camera_pose=Transform()))
subject.process_image(image, timestamps[img_idx])
result = subject.finish_trial()
self.assertIsInstance(result, SLAMTrialResult)
self.assertEqual(subject, result.system)
self.assertTrue(result.success)
self.assertFalse(result.has_scale)
self.assertIsNotNone(result.run_time)
self.assertEqual(max_img_id + 1, len(result.results))
def test_result_saves(self):
# Make an image collection with some number of images
images = []
image_builder = DemoImageBuilder(mode=ImageMode.MONOCULAR, width=160, height=120)
num_images = 10
for time in range(num_images):
image = image_builder.create_frame(time / num_images)
image.save()
images.append(image)
image_collection = ImageCollection(
images=images,
timestamps=list(range(len(images))),
sequence_type=ImageSequenceType.SEQUENTIAL
)
image_collection.save()
subject = LibVisOMonoSystem()
subject.save()
# Actually run the system using mocked images
subject.set_camera_intrinsics(image_builder.get_camera_intrinsics(), 1 / 10)
subject.start_trial(ImageSequenceType.SEQUENTIAL)
for time, image in enumerate(images):
subject.process_image(image, time)
result = subject.finish_trial()
self.assertIsInstance(result, SLAMTrialResult)
self.assertEqual(len(image_collection), len(result.results))
result.image_source = image_collection
result.save()
# Load all the entities
all_entities = list(SLAMTrialResult.objects.all())
self.assertGreaterEqual(len(all_entities), 1)
self.assertEqual(all_entities[0], result)
all_entities[0].delete()
SLAMTrialResult._mongometa.collection.drop()
ImageCollection._mongometa.collection.drop()
Image._mongometa.collection.drop()
def test_can_run_on_colour_images(self):
# Actually run the system using mocked images
num_frames = 50
max_time = 50
speed = 0.1
image_builder = DemoImageBuilder(
mode=ImageMode.MONOCULAR,
width=640, height=480, num_stars=150,
length=max_time * speed, speed=speed,
close_ratio=0.6, min_size=1, max_size=50, colour=True
)
subject = LibVisOMonoSystem(motion_threshold=1000)
subject.set_camera_intrinsics(image_builder.get_camera_intrinsics(), max_time / num_frames)
subject.start_trial(ImageSequenceType.SEQUENTIAL, seed=0)
for idx in range(num_frames):
time = max_time * idx / num_frames
image = image_builder.create_frame(time)
subject.process_image(image, time)
result = subject.finish_trial()
self.assertIsInstance(result, SLAMTrialResult)
self.assertEqual(subject, result.system)
self.assertTrue(result.success)
self.assertFalse(result.has_scale)
self.assertIsNotNone(result.run_time)
self.assertEqual({
'seed': 0,
'in_fx': image_builder.focal_length,
'in_fy': image_builder.focal_length,
'in_cu': image_builder.width / 2,
'in_cv': image_builder.height / 2,
'in_width': image_builder.width,
'in_height': image_builder.height
}, result.settings)
self.assertEqual(num_frames, len(result.results))
has_been_found = False
has_been_lost = False
for idx, frame_result in enumerate(result.results):
self.assertEqual(max_time * idx / num_frames, frame_result.timestamp)
self.assertIsNotNone(frame_result.pose)
self.assertIsNotNone(frame_result.motion)
# If we're lost, our tracking state should depend of if we've been lost before
is_first_frame = False
if frame_result.tracking_state != TrackingState.OK:
if has_been_found:
has_been_lost = True
self.assertEqual(frame_result.tracking_state, TrackingState.LOST)
else:
self.assertEqual(frame_result.tracking_state, TrackingState.NOT_INITIALIZED)
elif has_been_found is False:
is_first_frame = True
has_been_found = True
# Motion should be none when we are lost, and on the first found frame
if is_first_frame or frame_result.tracking_state != TrackingState.OK:
self.assertIsNone(frame_result.estimated_motion)
else:
self.assertIsNotNone(frame_result.estimated_motion)
# Estimates will be none until we get a successful estimate, or after it has lost
if not has_been_found or has_been_lost:
self.assertIsNone(frame_result.estimated_pose)
else:
self.assertIsNotNone(frame_result.estimated_pose)
self.assertTrue(has_been_found)
def test_can_start_and_stop_trial(self):
subject = LibVisOMonoSystem()
subject.start_trial(ImageSequenceType.SEQUENTIAL)
result = subject.finish_trial()
self.assertIsInstance(result, SLAMTrialResult)