def test_trivial(self):
# Some arbitrary intrinsics, with distortion
intrinsics = CameraIntrinsics(width=320,
height=240,
fx=160,
fy=160,
cx=160,
cy=120)
expected_v, expected_u = np.indices(
(intrinsics.height, intrinsics.width))
left_u, left_v, left_intrinsics, right_u, right_v, right_intrinsics = euroc_loader.rectify(
tf.Transform(), intrinsics, tf.Transform([1, 0, 0]), intrinsics)
self.assertNPEqual(expected_u, left_u)
self.assertNPEqual(expected_v, left_v)
self.assertNPEqual(expected_u, right_u)
self.assertNPEqual(expected_v, right_v)
def test_matches_orbslam_example(self):
# The actual intrinsics and extrinsics taken from the dataset
left_extrinsics = tf.Transform(
np.array([[
0.0148655429818, -0.999880929698, 0.00414029679422,
-0.0216401454975
],
[
0.999557249008, 0.0149672133247, 0.025715529948,
-0.064676986768
],
[
-0.0257744366974, 0.00375618835797, 0.999660727178,
0.00981073058949
], [0, 0, 0, 1]]))
left_intrinsics = CameraIntrinsics(width=752,
height=480,
fx=458.654,
fy=457.296,
cx=367.215,
cy=248.375,
k1=-0.28340811,
k2=0.07395907,
p1=0.00019359,
p2=1.76187114e-05,
k3=0)
right_extrinsics = tf.Transform(
np.array([[
0.0125552670891, -0.999755099723, 0.0182237714554,
-0.0198435579556
],
[
0.999598781151, 0.0130119051815, 0.0251588363115,
0.0453689425024
],
[
-0.0253898008918, 0.0179005838253, 0.999517347078,
0.00786212447038
], [0, 0, 0, 1]]))
right_intrinsics = CameraIntrinsics(width=752,
height=480,
fx=457.587,
fy=456.134,
cx=379.999,
cy=255.238,
k1=-0.28368365,
k2=0.07451284,
p1=-0.00010473,
p2=-3.55590700e-05,
k3=0)
# These are the orbslam numbers
oheight = 480
owidth = 752
od_left = np.array(
[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0])
ok_left = np.array([[458.654, 0.0, 367.215], [0.0, 457.296, 248.375],
[0.0, 0.0, 1.0]])
or_left = np.array([
[0.999966347530033, -0.001422739138722922, 0.008079580483432283],
[0.001365741834644127, 0.9999741760894847, 0.007055629199258132],
[-0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
])
op_left = np.array([[435.2046959714599, 0, 367.4517211914062, 0],
[0, 435.2046959714599, 252.2008514404297, 0],
[0, 0, 1, 0]])
od_right = np.array(
[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0])
ok_right = np.array([[457.587, 0.0, 379.999], [0.0, 456.134, 255.238],
[0.0, 0.0, 1]])
or_right = np.array(
[[0.9999633526194376, -0.003625811871560086, 0.007755443660172947],
[0.003680398547259526, 0.9999684752771629, -0.007035845251224894],
[-0.007729688520722713, 0.007064130529506649, 0.999945173484644]])
op_right = np.array(
[[435.2046959714599, 0, 367.4517211914062, -47.90639384423901],
[0, 435.2046959714599, 252.2008514404297, 0], [0, 0, 1, 0]])
orbslam_m1l, orbslam_m2l = cv2.initUndistortRectifyMap(
ok_left, od_left, or_left, op_left[0:3, 0:3], (owidth, oheight),
cv2.CV_32F)
orbslam_m1r, orbslam_m2r = cv2.initUndistortRectifyMap(
ok_right, od_right, or_right, op_right[0:3, 0:3],
(owidth, oheight), cv2.CV_32F)
left_u, left_v, _, right_u, right_v, _ = euroc_loader.rectify(
left_extrinsics, left_intrinsics, right_extrinsics,
right_intrinsics)
self.assertLess(np.max(np.abs(orbslam_m1l - left_u)), 0.06)
self.assertLess(np.max(np.abs(orbslam_m2l - left_v)), 0.06)
self.assertLess(np.max(np.abs(orbslam_m1r - right_u)), 0.06)
self.assertLess(np.max(np.abs(orbslam_m2r - right_v)), 0.06)
def test_undistorts_right_image(self):
# Some arbitrary intrinsics, with distortion
intrinsics = CameraIntrinsics(width=100,
height=100,
fx=123,
fy=122,
cx=51,
cy=49.5,
k1=0.28340811,
k2=0.07395907,
p1=0.00019359,
p2=1.76187114e-05,
k3=-0.0212445)
input_image = np.zeros((100, 100, 3), dtype=np.uint8)
point_ul = (-40 / intrinsics.fx, -40 / intrinsics.fy)
point_ur = (-40 / intrinsics.fx, 40 / intrinsics.fy)
point_ll = (40 / intrinsics.fx, -40 / intrinsics.fy)
point_lr = (40 / intrinsics.fx, 40 / intrinsics.fy)
point = world_point_to_pixel(point_ul[0], point_ul[1], intrinsics)
set_subpixel(input_image, point[0], point[1], np.array([0, 0, 255]))
point = world_point_to_pixel(point_ur[0], point_ur[1], intrinsics)
set_subpixel(input_image, point[0], point[1], np.array([0, 255, 0]))
point = world_point_to_pixel(point_ll[0], point_ll[1], intrinsics)
set_subpixel(input_image, point[0], point[1], np.array([255, 0, 0]))
point = world_point_to_pixel(point_lr[0], point_lr[1], intrinsics)
set_subpixel(input_image, point[0], point[1], np.array([255, 255,
255]))
_, _, _, right_u, right_v, right_intr = euroc_loader.rectify(
tf.Transform(), intrinsics, tf.Transform([1, 0, 0]), intrinsics)
undistorted_image = cv2.remap(input_image, right_u, right_v,
cv2.INTER_LINEAR)
point = world_point_to_pixel(point_ul[0], point_ul[1], right_intr)
colour = get_subpixel(undistorted_image, point[0], point[1])
self.assertEqual(0, colour[0])
self.assertEqual(0, colour[1])
self.assertGreater(
colour[2], 50) # Color is spread around by subpixel interpolation
point = world_point_to_pixel(point_ur[0], point_ur[1], right_intr)
colour = get_subpixel(undistorted_image, point[0], point[1])
self.assertEqual(0, colour[0])
self.assertGreater(colour[1], 50)
self.assertEqual(0, colour[2])
point = world_point_to_pixel(point_ll[0], point_ll[1], right_intr)
colour = get_subpixel(undistorted_image, point[0], point[1])
self.assertGreater(colour[0], 50)
self.assertEqual(0, colour[1])
self.assertEqual(0, colour[2])
point = world_point_to_pixel(point_lr[0], point_lr[1], right_intr)
colour = get_subpixel(undistorted_image, point[0], point[1])
self.assertGreater(colour[0], 20)
self.assertGreater(colour[1], 20)
self.assertGreater(colour[2], 20)
self.assertEqual(colour[0], colour[1])
self.assertEqual(colour[0], colour[2])
def verify_dataset(image_collection: ImageCollection,
root_folder: typing.Union[str, PurePath],
dataset_name: str,
repair: bool = False):
"""
Examine an existing Autonomous Systems Lab dataset in the database, and check if for errors
See http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets#downloads
Some information drawn from the ethz_asl dataset tools, see: https://github.com/ethz-asl/dataset_tools
:param image_collection: The existing image collection from the loader
:param root_folder: The body folder, containing body.yaml (i.e. the extracted mav0 folder)
:param dataset_name: The name of the dataset, see the manager for the list of valid values.
:param repair: If possible, fix missing images in the dataset
:return:
"""
root_folder = Path(root_folder)
dataset_name = str(dataset_name)
repair = bool(repair)
if not root_folder.is_dir():
raise NotADirectoryError(
"'{0}' is not a directory".format(root_folder))
image_group = dataset_name
valid = True
irreparable = False
# Check the image group on the image collection
if image_collection.image_group != image_group:
if repair:
image_collection.image_group = image_group
image_collection.save()
logging.getLogger(__name__).info(
f"Fixed incorrect image group for {image_collection.sequence_name}"
)
else:
logging.getLogger(__name__).warning(
f"{image_collection.sequence_name} has incorrect image group {image_group}"
)
valid = False
# Find the various files containing the data (that's a 6-element tuple unpack for the return value)
(root_folder, left_rgb_path, left_camera_intrinsics_path, right_rgb_path,
right_camera_intrinsics_path,
trajectory_path) = euroc_loader.find_files(root_folder)
# Read the meta-information from the files (that's a 6-element tuple unpack for the return value)
left_image_files = euroc_loader.read_image_filenames(left_rgb_path)
left_extrinsics, left_intrinsics = euroc_loader.get_camera_calibration(
left_camera_intrinsics_path)
right_image_files = euroc_loader.read_image_filenames(left_rgb_path)
right_extrinsics, right_intrinsics = euroc_loader.get_camera_calibration(
right_camera_intrinsics_path)
# Create stereo rectification matrices from the intrinsics
left_x, left_y, left_intrinsics, right_x, right_y, right_intrinsics = euroc_loader.rectify(
left_extrinsics, left_intrinsics, right_extrinsics, right_intrinsics)
# Associate the different data types by timestamp. Trajectory last because it's bigger than the stereo.
all_metadata = euroc_loader.associate_data(left_image_files,
right_image_files)
# Load the images from the metadata
total_invalid_images = 0
total_fixed_images = 0
image_index = 0
with arvet.database.image_manager.get().get_group(image_group,
allow_write=repair):
for timestamp, left_image_file, right_image_file in all_metadata:
changed = False
img_valid = True
# Skip if we've hit the end of the data
if image_index >= len(image_collection):
logging.getLogger(__name__).error(
f"Image {image_index} is missing from the dataset")
irreparable = True
valid = False
total_invalid_images += 1
continue
left_img_path = root_folder / 'cam0' / 'data' / left_image_file
right_img_path = root_folder / 'cam1' / 'data' / right_image_file
left_pixels = image_utils.read_colour(left_img_path)
right_pixels = image_utils.read_colour(right_img_path)
# Error check the loaded image data
# The EuRoC Sequences MH_04_difficult and V2_03_difficult are missing the first right frame
# So we actually start loading from
# In general, frames that are missing are skipped, and do not increment image index
if left_pixels is None or left_pixels.size is 0:
logging.getLogger(__name__).warning(
f"Could not read left image \"{left_img_path}\", result is empty. Image is skipped."
)
continue
if right_pixels is None or right_pixels.size is 0:
logging.getLogger(__name__).warning(
f"Could not read right image \"{right_img_path}\", result is empty. Image is skipped."
)
continue
left_pixels = cv2.remap(left_pixels, left_x, left_y,
cv2.INTER_LINEAR)
right_pixels = cv2.remap(right_pixels, right_x, right_y,
cv2.INTER_LINEAR)
left_hash = bytes(xxhash.xxh64(left_pixels).digest())
right_hash = bytes(xxhash.xxh64(right_pixels).digest())
# Load the image from the database
try:
_, image = image_collection[image_index]
except (KeyError, IOError, RuntimeError):
logging.getLogger(__name__).exception(
f"Error loading image object {image_index}")
valid = False
image_index += 1 # Index is valid, increment when done
continue
# First, check the image group
if image.image_group != image_group:
if repair:
image.image_group = image_group
changed = True
logging.getLogger(__name__).warning(
f"Image {image_index} has incorrect group {image.image_group}"
)
valid = False
img_valid = False
# Load the pixels from the image
try:
left_actual_pixels = image.left_pixels
except (KeyError, IOError, RuntimeError):
left_actual_pixels = None
try:
right_actual_pixels = image.right_pixels
except (KeyError, IOError, RuntimeError):
right_actual_pixels = None
# Compare the loaded image data to the data read from disk
if left_actual_pixels is None or not np.array_equal(
left_pixels, left_actual_pixels):
if repair:
image.store_pixels(left_pixels)
changed = True
else:
logging.getLogger(__name__).error(
f"Image {image_index}: Left pixels do not match data read from {left_img_path}"
)
img_valid = False
valid = False
if left_hash != bytes(image.metadata.img_hash):
if repair:
image.metadata.img_hash = left_hash
changed = True
else:
logging.getLogger(__name__).error(
f"Image {image_index}: Left hash does not match metadata"
)
valid = False
img_valid = False
if right_actual_pixels is None or not np.array_equal(
right_pixels, right_actual_pixels):
if repair:
image.store_right_pixels(right_pixels)
changed = True
else:
logging.getLogger(__name__).error(
f"Image {image_index}: Right pixels do not match data read from {right_img_path}"
)
valid = False
img_valid = False
if right_hash != bytes(image.right_metadata.img_hash):
if repair:
image.right_metadata.img_hash = right_hash
changed = True
else:
logging.getLogger(__name__).error(
f"Image {image_index}: Right hash does not match metadata"
)
valid = False
img_valid = False
if changed and repair:
logging.getLogger(__name__).warning(
f"Image {image_index}: repaired")
image.save()
total_fixed_images += 1
if not img_valid:
total_invalid_images += 1
image_index += 1
if irreparable:
# Images are missing entirely, needs re-import
logging.getLogger(__name__).error(
f"Image Collection {image_collection.pk} for sequence {dataset_name} "
"is IRREPARABLE, invalidate and re-import")
elif repair:
# Re-save the modified image collection
logging.getLogger(__name__).info(
f"{image_collection.sequence_name} repaired successfully "
f"({total_fixed_images} image files fixed).")
elif valid:
logging.getLogger(__name__).info(
f"Verification of {image_collection.sequence_name} successful.")
else:
logging.getLogger(__name__).error(
f"Verification of {image_collection.sequence_name} ({image_collection.pk}) "
f"FAILED, ({total_invalid_images} images failed)")
return valid