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))
base_splitted = base.split('_')
base_number = base_splitted[-1]
base_camera = base_splitted[1][6:]
origin_frame = os.path.join(
os.path.dirname(image_file), '_'.join(base_splitted[:-1]) + '_' +
str(0).zfill(len(base_number)) + 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_extrinsics_pose_matrix_fisheye(image_file, calib_data):
"""Get extrinsics from the calib_data dictionary (fisheye cam)."""
cam = get_camera_name(image_file)
extr = calib_data[cam]['extrinsics']
t = np.array([
float(extr['pos_x_m']),
float(extr['pos_y_m']),
float(extr['pos_z_m'])
])
x_rad = np.pi / 180. * float(extr['rot_x_deg'])
z1_rad = np.pi / 180. * float(extr['rot_z1_deg'])
z2_rad = np.pi / 180. * float(extr['rot_z2_deg'])
x_rad += np.pi # gcam
cosx, sinx = np.cos(x_rad), np.sin(x_rad)
cosz1, sinz1 = np.cos(z1_rad), np.sin(z1_rad)
cosz2, sinz2 = np.cos(z2_rad), np.sin(z2_rad)
Rx = np.array([[1, 0, 0], [0, cosx, sinx], [0, -sinx, cosx]])
Rz1 = np.array([[cosz1, sinz1, 0], [-sinz1, cosz1, 0], [0, 0, 1]])
Rz2 = np.array([[cosz2, -sinz2, 0], [sinz2, cosz2, 0], [0, 0, 1]])
R = np.matmul(Rz2, np.matmul(Rx, Rz1))
T_other_convention = -np.dot(R, t)
pose_matrix = transform_from_rot_trans(R, T_other_convention).astype(
np.float32)
return pose_matrix
def get_extrinsics_pose_matrix_distorted(image_file, calib_data):
"""Get extrinsics from the calib_data dictionary (distorted perspective cam)."""
cam = get_camera_name(image_file)
extr = calib_data[cam]['extrinsics']
T_other_convention = np.array([float(extr['t_x_m']), float(extr['t_y_m']), float(extr['t_z_m'])])
R = np.array(extr['R'])
pose_matrix = transform_from_rot_trans(R, T_other_convention).astype(np.float32)
return pose_matrix
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
def _get_extrinsics_pose_matrix_distorted(self, image_file, calib_data):
"""Get intrinsics from the calib_data dictionary."""
cam = self._get_camera_name(image_file)
if image_file in self.pose_cache:
return self.pose_cache[image_file]
extr = calib_data[cam]['extrinsics']
T_other_convention = np.array([float(extr['t_x_m']), float(extr['t_y_m']), float(extr['t_z_m'])])
R = np.array(extr['R'])
pose_matrix = transform_from_rot_trans(R, T_other_convention).astype(np.float32)
self.pose_cache[image_file] = pose_matrix
return pose_matrix
def _get_extrinsics_pose_matrix_v2(self, image_file, calib_data):
"""Get intrinsics from the calib_data dictionary."""
cam = self._get_camera_name(image_file)
if image_file in self.pose_cache:
return self.pose_cache[image_file]
extr = calib_data[cam]['extrinsics']
#t = np.array([float(extr['pos_x_m']), float(extr['pos_y_m']), float(extr['pos_z_m'])])
T_other_convention = np.array(
[float(extr['t_x_m']),
float(extr['t_y_m']),
float(extr['t_z_m'])])
R = np.array(extr['R'])
# x_rad = np.pi / 180. * float(extr['rot_x_deg'])
# z1_rad = np.pi / 180. * float(extr['rot_z1_deg'])
# z2_rad = np.pi / 180. * float(extr['rot_z2_deg'])
# x_rad += np.pi # gcam
# cosx = np.cos(x_rad)
# sinx = np.sin(x_rad)
# cosz1 = np.cos(z1_rad)
# sinz1 = np.sin(z1_rad)
# cosz2 = np.cos(z2_rad)
# sinz2 = np.sin(z2_rad)
#
# Rx = np.array([[ 1, 0, 0],
# [ 0, cosx, sinx],
# [ 0, -sinx, cosx]])
# Rz1 = np.array([[ cosz1, sinz1, 0],
# [-sinz1, cosz1, 0],
# [ 0, 0, 1]])
# Rz2 = np.array([[cosz2, -sinz2, 0],
# [sinz2, cosz2, 0],
# [ 0, 0, 1]])
#
# R = np.matmul(Rz2, np.matmul(Rx, Rz1))
#T_other_convention = -np.dot(R,t)
pose_matrix = transform_from_rot_trans(R, T_other_convention).astype(
np.float32)
self.pose_cache[image_file] = pose_matrix
return pose_matrix
Rx_right = np.array([[1, 0, 0], [0, cx_right, sx_right],
[0, -sx_right, cx_right]])
Rz1_right = np.array([[cz1_right, sz1_right, 0], [-sz1_right, cz1_right, 0],
[0, 0, 1]])
Rz2_right = np.array([[cz2_right, -sz2_right, 0], [sz2_right, cz2_right, 0],
[0, 0, 1]])
R_front = np.matmul(Rz2_front, np.matmul(Rx_front, Rz1_front))
R_left = np.matmul(Rz2_left, np.matmul(Rx_left, Rz1_left))
R_right = np.matmul(Rz2_right, np.matmul(Rx_right, Rz1_right))
T_other_front = -np.dot(R_front, t_front)
T_other_left = -np.dot(R_left, t_left)
T_other_right = -np.dot(R_right, t_right)
pose_matrix_front = transform_from_rot_trans(R_front,
T_other_front).astype(np.float32)
pose_matrix_left = transform_from_rot_trans(R_left,
T_other_left).astype(np.float32)
pose_matrix_right = transform_from_rot_trans(R_right,
T_other_right).astype(np.float32)
pose_matrix_left = pose_matrix_left @ invert_pose_numpy(pose_matrix_front)
pose_matrix_right = pose_matrix_right @ invert_pose_numpy(pose_matrix_front)
pose_matrix_front_torch = torch.zeros(1, 4, 4)
pose_matrix_left_torch = torch.zeros(1, 4, 4)
pose_matrix_right_torch = torch.zeros(1, 4, 4)
pose_matrix_front_torch[0, :, :] = torch.from_numpy(pose_matrix_front)
pose_matrix_left_torch[0, :, :] = torch.from_numpy(pose_matrix_left)
pose_matrix_right_torch[0, :, :] = torch.from_numpy(pose_matrix_right)
