def _get_pose(self, image_file):
"""Gets the pose information from an image file."""
if image_file in self.pose_cache:
return self.pose_cache[image_file]
# Find origin frame in this sequence to determine scale & origin translation
base, ext = os.path.splitext(os.path.basename(image_file))
origin_frame = os.path.join(os.path.dirname(image_file),
str(0).zfill(len(base)) + ext)
# Get origin data
origin_oxts_data = self._get_oxts_data(origin_frame)
lat = origin_oxts_data[0]
scale = np.cos(lat * np.pi / 180.)
# Get origin pose
origin_R, origin_t = pose_from_oxts_packet(origin_oxts_data, scale)
origin_pose = transform_from_rot_trans(origin_R, origin_t)
# Compute current pose
oxts_data = self._get_oxts_data(image_file)
R, t = pose_from_oxts_packet(oxts_data, scale)
pose = transform_from_rot_trans(R, t)
# Compute odometry pose
imu2cam = self._get_imu2cam_transform(image_file)
odo_pose = (imu2cam @ np.linalg.inv(origin_pose) @ pose
@ np.linalg.inv(imu2cam)).astype(np.float32)
# Cache and return pose
self.pose_cache[image_file] = odo_pose
return odo_pose
def _get_imu2cam_transform(self, image_file):
"""Gets the transformation between IMU an camera from an image file"""
parent_folder = self._get_parent_folder(image_file)
if image_file in self.imu2velo_calib_cache:
return self.imu2velo_calib_cache[image_file]
cam2cam = read_calib_file(os.path.join(parent_folder, CALIB_FILE['cam2cam']))
imu2velo = read_calib_file(os.path.join(parent_folder, CALIB_FILE['imu2velo']))
velo2cam = read_calib_file(os.path.join(parent_folder, CALIB_FILE['velo2cam']))
velo2cam_mat = transform_from_rot_trans(velo2cam['R'], velo2cam['T'])
imu2velo_mat = transform_from_rot_trans(imu2velo['R'], imu2velo['T'])
cam_2rect_mat = transform_from_rot_trans(cam2cam['R_rect_00'], np.zeros(3))
imu2cam = cam_2rect_mat @ velo2cam_mat @ imu2velo_mat
self.imu2velo_calib_cache[image_file] = imu2cam
return imu2cam