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)