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)