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