def create_random_trajectory(random_state, duration=600, length=10):
trajectory = {}
current_pose = tf.Transform(
random_state.uniform(-1000, 1000, 3),
random_state.uniform(-1, 1, 4)
)
velocity = random_state.uniform(-10, 10, 3)
angular_velocity = tf3d.quaternions.axangle2quat(
vector=random_state.uniform(-1, 1, 3),
theta=random_state.uniform(-np.pi / 30, np.pi / 30)
)
for time in range(duration):
current_pose = tf.Transform(
location=current_pose.location + velocity,
rotation=tf3d.quaternions.qmult(current_pose.rotation_quat(w_first=True), angular_velocity)
)
velocity += random_state.normal(0, 1, 3)
angular_velocity = tf3d.quaternions.qmult(angular_velocity, tf3d.quaternions.axangle2quat(
vector=random_state.uniform(-1, 1, 3),
theta=random_state.normal(0, np.pi / 30)
))
trajectory[time + random_state.normal(0, 0.1)] = current_pose
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 test_removes_comments_from_the_end_of_lines(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 = "# Starting with a comment\n # Another comment\n\n"
line_template = "{time},{x},{y},{z},{qw},{qx},{qy},{qz} # This is a comment\n"
for time in range(0, 10):
timestamp = time * 4999936 + 1403638128940097024
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[timestamp] = pose
absolute_pose = first_pose.find_independent(pose)
trajectory_text += self.format_line(timestamp, absolute_pose,
line_template)
mock_open = mock.mock_open(read_data=trajectory_text)
extend_mock_open(mock_open)
with mock.patch('arvet_slam.dataset.euroc.euroc_loader.open',
mock_open,
create=True):
trajectory = euroc_loader.read_trajectory(
'test_filepath', relative_trajectory.keys())
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 test_skips_comments_and_blank_lines(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 = "# Starting with a comment\n # Another comment\n\n"
for time in range(0, 10):
timestamp = time * 4999936 + 1403638128940097024
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[timestamp] = pose
absolute_pose = first_pose.find_independent(pose)
trajectory_text += self.format_line(timestamp, absolute_pose)
# Add incorrect trajectory data, preceeded by a hash to indicate it's a comment
trajectory_text += "# " + self.format_line(timestamp, pose)
mock_open = mock.mock_open(read_data=trajectory_text)
extend_mock_open(mock_open)
with mock.patch('arvet_slam.dataset.euroc.euroc_loader.open',
mock_open,
create=True):
trajectory = euroc_loader.read_trajectory(
'test_filepath', relative_trajectory.keys())
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 test_reads_trajectory_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 range(0, 10):
timestamp = time * 4999.936 + 1403638128.940097024
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[timestamp] = pose
absolute_pose = first_pose.find_independent(pose)
trajectory_text += self.format_line(timestamp, absolute_pose)
mock_open = mock.mock_open(read_data=trajectory_text)
extend_mock_open(mock_open)
with mock.patch('arvet_slam.dataset.tum.tum_loader.open',
mock_open,
create=True):
trajectory = tum_loader.read_trajectory('test_filepath',
relative_trajectory.keys())
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=10,
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 load_ref_trajectory(filename: str, exchange_coordinates=True, ref_timestamps=None) \
-> typing.Mapping[float, tf.Transform]:
trajectory = {}
if exchange_coordinates:
coordinate_exchange = np.matrix([[0, 0, 1, 0], [-1, 0, 0, 0],
[0, -1, 0, 0], [0, 0, 0, 1]])
else:
coordinate_exchange = np.identity(4)
first_stamp = None
line_num = 0
with open(filename, 'r') as trajectory_file:
for line in trajectory_file:
line_num += 1
parts = line.split(' ')
if len(parts) >= 12:
# Line is pose as homogenous matrix
if len(parts) >= 13:
stamp, r00, r01, r02, t0, r10, r11, r12, t1, r20, r21, r22, t2 = parts[
0:13]
else:
# Sometimes the stamp is missing, presumably an error in orbslam
r00, r01, r02, t0, r10, r11, r12, t1, r20, r21, r22, t2 = parts[
0:12]
if ref_timestamps is not None and line_num < len(
ref_timestamps):
stamp = float(ref_timestamps[line_num])
else:
print("Missing timestamp number {0}".format(line_num))
continue
if first_stamp is None:
first_stamp = float(stamp)
pose = np.matrix(
[[float(r00),
float(r01),
float(r02),
float(t0)],
[float(r10),
float(r11),
float(r12),
float(t1)],
[float(r20),
float(r21),
float(r22),
float(t2)], [0, 0, 0, 1]])
pose = np.dot(np.dot(coordinate_exchange, pose),
coordinate_exchange.T)
trajectory[float(stamp) - first_stamp] = tf.Transform(pose)
elif len(parts) >= 8:
# Line is pose as transform, followed by quaternion orientation
stamp, tx, ty, tz, qx, qy, qz, qw = parts[0:8]
if first_stamp is None:
first_stamp = float(stamp)
trajectory[float(stamp) - first_stamp] = tf.Transform(
location=(float(tz), -float(tx), -float(ty)),
rotation=(float(qw), float(qz), -float(qx), -float(qy)),
w_first=True)
return th.zero_trajectory(trajectory)
def make_metadata(index=1, **kwargs):
kwargs = du.defaults(
kwargs, {
'img_hash':
b'\xf1\x9a\xe2|' +
np.random.randint(0, 0xFFFFFFFF).to_bytes(4, 'big'),
'source_type':
imeta.ImageSourceType.SYNTHETIC,
'camera_pose':
tf.Transform(
location=(1 + 100 * index, 2 + np.random.uniform(-1, 1), 3),
rotation=(4, 5, 6, 7 + np.random.uniform(-4, 4))),
'intrinsics':
cam_intr.CameraIntrinsics(800, 600, 550.2, 750.2, 400, 300),
'environment_type':
imeta.EnvironmentType.INDOOR_CLOSE,
'light_level':
imeta.LightingLevel.WELL_LIT,
'time_of_day':
imeta.TimeOfDay.DAY,
'lens_focal_distance':
5,
'aperture':
22,
'simulation_world':
'TestSimulationWorld',
'lighting_model':
imeta.LightingModel.LIT,
'texture_mipmap_bias':
1,
'normal_maps_enabled':
2,
'roughness_enabled':
True,
'geometry_decimation':
0.8,
'procedural_generation_seed':
16234,
'labelled_objects':
(imeta.LabelledObject(class_names=('car', ),
x=12,
y=144,
width=67,
height=43,
relative_pose=tf.Transform(
(12, 3, 4), (0.5, 0.1, 1, 1.7)),
instance_name='Car-002'),
imeta.LabelledObject(class_names=('cat', ),
x=125,
y=244,
width=117,
height=67,
relative_pose=tf.Transform(
(378, -1890, 38), (0.3, 1.12, 1.1, 0.2)),
instance_name='cat-090'))
})
return imeta.ImageMetadata(**kwargs)
def get_circle_yaw_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, 0, time * angular_vel))).find_independent(relative_pose)
for time in np.arange(0, 2 * np.pi / angular_vel, 1 / framerate)}
def process_image(self, image: Image, timestamp: float) -> None:
start_time = time.time()
logging.getLogger(__name__).debug(
"Processing image at time {0} ...".format(timestamp))
# This is the pose of the previous pose relative to the next one
tracking, estimated_motion = self.handle_process_image(
self._viso, image, timestamp)
logging.getLogger(__name__).debug(" got estimated motion ...")
end_time = time.time()
frame_result = FrameResult(
timestamp=timestamp,
image=image.pk,
processing_time=end_time - start_time,
pose=image.camera_pose,
tracking_state=TrackingState.OK if tracking else TrackingState.LOST
if self._has_chosen_origin else TrackingState.NOT_INITIALIZED,
estimated_motion=estimated_motion,
num_matches=self._viso.getNumberOfMatches())
if tracking and not self._has_chosen_origin:
# set the intial pose estimate to 0, so we can infer the later ones from the motions
self._has_chosen_origin = True
frame_result.estimated_pose = tf.Transform()
frame_result.estimated_motion = None # This will always be the identity on the first valid frame
self._frame_results.append(frame_result)
logging.getLogger(__name__).debug(" Processing done.")
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 get_camera_calibration(
sensor_yaml_path: str) -> typing.Tuple[tf.Transform, CameraIntrinsics]:
with open(sensor_yaml_path, 'r') as sensor_file:
sensor_data = yaml.load(sensor_file, YamlLoader)
d = sensor_data['T_BS']['data']
extrinsics = tf.Transform(
np.array([
[d[0], d[1], d[2], d[3]],
[d[4], d[5], d[6], d[7]],
[d[8], d[9], d[10], d[11]],
[d[12], d[13], d[14], d[15]],
]))
resolution = sensor_data['resolution']
intrinsics = CameraIntrinsics(width=resolution[0],
height=resolution[1],
fx=sensor_data['intrinsics'][0],
fy=sensor_data['intrinsics'][1],
cx=sensor_data['intrinsics'][2],
cy=sensor_data['intrinsics'][3],
k1=sensor_data['distortion_coefficients'][0],
k2=sensor_data['distortion_coefficients'][1],
p1=sensor_data['distortion_coefficients'][2],
p2=sensor_data['distortion_coefficients'][3])
return extrinsics, intrinsics
def get_left_trajectory() -> typing.Mapping[float, tf.Transform]:
"""
Get a simple linear trajectory, moving in a straight line left from -10m to 10m
:return: A map from timestamps to camera poses, with which we can create a controller.
"""
framerate = 30 # frames per second
speed = 1.2 # meters per second
return {time: tf.Transform(location=(0, time * speed - 10, 0)) for time in np.arange(0, 20 / speed, 1 / framerate)}
def get_on_the_spot_yaw_trajectory():
"""
Get a trajectory that turns a circle on the spot. Pure yaw, no translational component.
:return:
"""
framerate = 30 # frames per second
angular_vel = np.pi / 36 # radians per second
return {time: tf.Transform(rotation=(0, 0, time * angular_vel))
for time in np.arange(0, 2 * np.pi / angular_vel, 1 / framerate)}
def get_line_yaw_trajectory():
"""
Get a simple linear trajectory, moving in a straight line forwards from -10m to 10m
and yawing through -np.pi to np.pi
:return: A map from timestamps to camera poses, with which we can create a controller.
"""
framerate = 30 # frames per second
speed = 1.2 # meters per second (walking pace)
angular_vel = np.pi * speed / 10 # (2pi rad * 1.2 m/s) / (20m) = (0.03 pi rad / s)
return {time: tf.Transform(location=(time * speed - 10, 0, 0), rotation=(0, 0, time * angular_vel - np.pi))
for time in np.arange(0, 20 / speed, 1 / framerate)}
def test_image_field_stores_and_loads(self):
pose = tf.Transform(location=[1, 2, 3], rotation=[4, 5, 6])
# Save the model
model = TestTransformFieldMongoModel()
model.transform = pose
model.save()
# Load all the entities
all_entities = list(TestTransformFieldMongoModel.objects.all())
self.assertGreaterEqual(len(all_entities), 1)
self.assertEqual(all_entities[0].transform, pose)
all_entities[0].delete()
def test_infers_properties_from_mono_images(self):
images = [make_image(idx, depth=None) for idx in range(10)]
collection = ic.ImageCollection(
images=images,
timestamps=[1.1 * idx for idx in range(10)],
sequence_type=ImageSequenceType.SEQUENTIAL)
self.assertEqual(images[0].image_group, collection.image_group)
self.assertFalse(collection.is_depth_available)
self.assertTrue(collection.is_normals_available)
self.assertFalse(collection.is_stereo_available)
self.assertTrue(collection.is_labels_available)
self.assertFalse(collection.is_masks_available)
self.assertEqual(collection.camera_intrinsics,
images[0].metadata.intrinsics)
self.assertIsNone(collection.stereo_offset)
self.assertIsNone(collection.right_camera_intrinsics)
images = [
make_image(idx,
normals=None,
metadata=make_metadata(
idx,
labelled_objects=[
imeta.MaskedObject(
('class_1', ), 152, 239, 14, 78,
tf.Transform(location=(123, -45, 23),
rotation=(0.5, 0.23, 0.1)),
'LabelledObject-18569',
np.random.choice((True, False),
size=(78, 14)))
])) for idx in range(10)
]
collection = ic.ImageCollection(
images=images,
timestamps=[1.1 * idx for idx in range(10)],
sequence_type=ImageSequenceType.SEQUENTIAL)
self.assertTrue(collection.is_depth_available)
self.assertFalse(collection.is_normals_available)
self.assertFalse(collection.is_stereo_available)
self.assertTrue(collection.is_labels_available)
self.assertTrue(collection.is_masks_available)
self.assertEqual(collection.camera_intrinsics,
images[0].metadata.intrinsics)
self.assertIsNone(collection.stereo_offset)
self.assertIsNone(collection.right_camera_intrinsics)
def make_relative_pose(frame_delta):
"""
LibVisO2 uses a different coordinate frame to the one I'm using,
this function is to convert computed frame deltas as libviso estimates them to usable poses.
Thankfully, its still a right-handed coordinate frame, which makes this easier.
Frame is: z forward, x right, y down
Documentation at: http://www.cvlibs.net/software/libviso/
:param frame_delta: A 4x4 matrix (possibly in list form)
:return: A Transform object representing the pose of the current frame with respect to the previous frame
"""
frame_delta = np.asarray(frame_delta)
coordinate_exchange = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0],
[0, 0, 0, 1]])
pose = np.dot(np.dot(coordinate_exchange, frame_delta),
coordinate_exchange.T)
return tf.Transform(pose)
def make_relative_pose(pose_matrix: np.ndarray) -> 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 pose_matrix: The homogenous pose matrix, as a 4x4 matrix
:return: A Transform object representing the pose of the current frame with respect to the previous frame
"""
coordinate_exchange = np.array([[0, 0, 1, 0],
[-1, 0, 0, 0],
[0, -1, 0, 0],
[0, 0, 0, 1]])
pose = np.dot(np.dot(coordinate_exchange, pose_matrix), coordinate_exchange.T)
return tf.Transform(pose)
def test_kinect_noise_works_when_not_640_by_480(self):
left_true_depth = get_test_image('left')[0:64, 0:64]
right_true_depth = get_test_image('right')[0:64, 0:64]
focal_length = 1 / (2 * np.tan(np.pi / 4))
camera_intrinsics = cam_intr.CameraIntrinsics(
left_true_depth.shape[1], left_true_depth.shape[0],
focal_length * left_true_depth.shape[1],
focal_length * left_true_depth.shape[1],
0.5 * left_true_depth.shape[1], 0.5 * left_true_depth.shape[0])
relative_pose = tf.Transform((0, -0.15, 0))
noisy_depth = depth_noise.kinect_depth_model(left_true_depth,
right_true_depth,
camera_intrinsics,
relative_pose)
self.assertIsNotNone(noisy_depth)
self.assertNotEqual(np.dtype, noisy_depth.dtype)
def read_camera_pose(frame_data: dict) -> tf.Transform:
"""
Read the camera pose from the frame data
This is in Unreal coordinates, so z is up, y right, x forward.
Converting requires flipping the Y axis and any angles.
(for a quaternion, this is the same as inverting the x and z axes, since q = -q)
Scale is 1 unit ~= 1cm, so we want to divide by 100 to have scale in meters
:param frame_data:
:return:
"""
camera_data = frame_data['camera_data']
tx, ty, tz = camera_data['location_worldframe']
qx, qy, qz, qw = camera_data['quaternion_xyzw_worldframe']
return tf.Transform(
location=(tx / 100, -ty / 100, tz / 100),
rotation=(qw, -qx, qy, -qz),
w_first=True
)
def make_camera_pose(tx: float, ty: float, tz: float, qw: float, qx: float,
qy: float, qz: float) -> tf.Transform:
"""
As far as I can tell, EuRoC uses Z forward coordinates, the same as everything else.
I need to switch it to X-forward coordinates.
:param tx: The x coordinate of the location
:param ty: The y coordinate of the location
:param tz: The z coordinate of the location
:param qw: The scalar part of the quaternion orientation
:param qx: The x imaginary part of the quaternion orientation
:param qy: The y imaginary part of the quaternion orientation
:param qz: The z imaginary part of the quaternion orientation
:return: A Transform object representing the world pose of the current frame
"""
return tf.Transform(location=(tz, -tx, -ty),
rotation=(qw, qz, -qx, -qy),
w_first=True)
def align_point(pose: tf.Transform,
shift: np.ndarray,
rotation: np.ndarray,
scale: float = 1.0) -> tf.Transform:
"""
Transform an estimated point
:param pose:
:param shift:
:param rotation:
:param scale:
:return:
"""
return tf.Transform(
location=shift +
scale * t3.quaternions.rotate_vector(pose.location, rotation),
rotation=t3.quaternions.qmult(rotation,
pose.rotation_quat(w_first=True)),
w_first=True)
def test_kinect_noise_is_quick(self):
left_true_depth = get_test_image('left')
right_true_depth = get_test_image('right')
focal_length = 1 / (2 * np.tan(np.pi / 4))
camera_intrinsics = cam_intr.CameraIntrinsics(
left_true_depth.shape[1], left_true_depth.shape[0],
focal_length * left_true_depth.shape[1],
focal_length * left_true_depth.shape[1],
0.5 * left_true_depth.shape[1], 0.5 * left_true_depth.shape[0])
relative_pose = tf.Transform((0, -0.15, 0))
number = 20
time = timeit.timeit(lambda: depth_noise.kinect_depth_model(
left_true_depth, right_true_depth, camera_intrinsics, relative_pose
),
number=number)
# print("Noise time: {0}, total time: {1}".format(time / number, time))
self.assertLess(time / number, 1)
def read_relative_pose(object_frame_data: dict) -> tf.Transform:
"""
Read the pose of an object relative to the camera, from the frame data.
For reasons (known only to the developer), these poses are in OpenCV convention.
So x is right, y is down, z is forward.
Scale is still 1cm, so we divide by 100 again.
see
https://github.com/jskinn/Dataset_Synthesizer/blob/local-devel/Source/Plugins/NVSceneCapturer/Source/NVSceneCapturer/Private/NVSceneFeatureExtractor_DataExport.cpp#L143
:param object_frame_data: The frame data dict from the matching object in the objects array
:return: The relative pose of the object, as a Transform
"""
tx, ty, tz = object_frame_data['location']
qx, qy, qz, qw = object_frame_data['quaternion_xyzw']
return tf.Transform(
location=(tz / 100, -tx / 100, -ty / 100),
rotation=(qw, qz, -qx, -qy),
w_first=True
)
def make_camera_pose(tx: float, ty: float, tz: float, qw: float, qx: float,
qy: float, qz: float) -> tf.Transform:
"""
TUM dataset use a different coordinate frame to the one I'm using, which is the same as the Libviso2 frame.
This function is to convert dataset ground-truth poses to transform objects.
Thankfully, its still a right-handed coordinate frame, which makes this easier.
Frame is: z forward, y right, x down
:param tx: The x coordinate of the location
:param ty: The y coordinate of the location
:param tz: The z coordinate of the location
:param qx: The x part of the quaternion orientation
:param qy: The y part of the quaternion orientation
:param qz: The z part of the quaternion orientation
:param qw: The scalar part of the quaternion orientation
:return: A Transform object representing the world pose of the current frame
"""
return tf.Transform(location=(tz, -tx, -ty),
rotation=(qw, qz, -qx, -qy),
w_first=True)
def test_kinect_noise_produces_reasonable_output(self):
left_true_depth = get_test_image('left')
right_true_depth = get_test_image('right')
focal_length = 1 / (2 * np.tan(np.pi / 4))
camera_intrinsics = cam_intr.CameraIntrinsics(
left_true_depth.shape[1], left_true_depth.shape[0],
focal_length * left_true_depth.shape[1],
focal_length * left_true_depth.shape[1],
0.5 * left_true_depth.shape[1], 0.5 * left_true_depth.shape[0])
relative_pose = tf.Transform((0, -0.15, 0))
noisy_depth = depth_noise.kinect_depth_model(left_true_depth,
right_true_depth,
camera_intrinsics,
relative_pose)
self.assertLessEqual(np.max(noisy_depth),
4.1) # A little leeway for noise
self.assertGreaterEqual(np.min(noisy_depth[np.nonzero(noisy_depth)]),
0.7)
self.assertGreater(np.mean(noisy_depth),
0) # Assert that something is visible at all
def make_stereo_image(index=1, **kwargs):
kwargs = du.defaults(
kwargs, {
'pixels':
np.random.uniform(0, 255, (32, 32, 3)),
'right_pixels':
np.random.uniform(0, 255, (32, 32, 3)),
'image_group':
'test',
'metadata':
make_metadata(index),
'right_metadata':
make_metadata(
index,
camera_pose=tf.Transform(
location=(1 + 100 * index, 2 + np.random.uniform(-1, 1),
4),
rotation=(4, 5, 6, 7 + np.random.uniform(-4, 4))),
),
'additional_metadata': {
'Source': 'Generated',
'Resolution': {
'width': 1280,
'height': 720
},
'Material Properties': {
'BaseMipMapBias': 0,
'RoughnessQuality': True
}
},
'depth':
np.random.uniform(0, 1, (32, 32)),
'right_depth':
np.random.uniform(0, 1, (32, 32)),
'normals':
np.random.uniform(0, 1, (32, 32, 3)),
'right_normals':
np.random.uniform(0, 1, (32, 32, 3))
})
return im.StereoImage(**kwargs)
def test_reads_from_given_file(self):
trajectory_text = ""
timestamps = []
for time in range(0, 10):
timestamp = time * 101 + 10000
pose = tf.Transform(location=(0.122 * time, -0.53112 * time,
1.893 * time),
rotation=(0.772 * time, -0.8627 * time,
-0.68782 * time))
trajectory_text += self.format_line(timestamp, pose)
timestamps.append(timestamp)
mock_open = mock.mock_open(read_data=trajectory_text)
extend_mock_open(mock_open)
with mock.patch('arvet_slam.dataset.euroc.euroc_loader.open',
mock_open,
create=True):
trajectory = euroc_loader.read_trajectory('test_filepath',
timestamps)
self.assertTrue(mock_open.called)
self.assertEqual('test_filepath', mock_open.call_args[0][0])
self.assertEqual('r', mock_open.call_args[0][1])
self.assertEqual(len(trajectory), len(timestamps))
def make_pose(translation, rotation) -> 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 translation: The translation 3-vector
:param rotation: The rotation quaternion, w-last
:return: A Transform object representing the pose of the current frame with respect to the previous frame
"""
# coordinate_exchange = np.array([[0, 0, 1, 0],
# [-1, 0, 0, 0],
# [0, -1, 0, 0],
# [0, 0, 0, 1]])
# pose = np.dot(np.dot(coordinate_exchange, pose_matrix), coordinate_exchange.T)
x, y, z = translation
qx, qy, qz, qw = rotation
return tf.Transform(
location=(z, -x, -y),
rotation=(qw, qz, -qx, -qy),
w_first=True
)
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)
