# Copyright 2026 Toyota Research Institute. All rights reserved. import os import cv2 import numpy as np from glob import glob from collections import defaultdict from pyquaternion import Quaternion from lyft_dataset_sdk.lyftdataset import LyftDataset from anydata.utils.read import read_numpy, read_yaml, read_image, read_depth from anydata.utils.write import write_image, write_npz, write_json from anydata.converters.utils import run, add_key_to_dict, fill_metadata, parse_dst_seq, frame_name, crawl ####################################################### CAMERAS = { 0: "CAM_FRONT", 1: "CAM_FRONT_RIGHT", 2: "CAM_BACK_RIGHT", 3: "CAM_BACK", 4: "CAM_BACK_LEFT", 5: "CAM_FRONT_LEFT", 6: "CAM_FRONT_ZOOMED", } def compose_transformation(rot, tvec): pose = np.concatenate([rot, tvec[:, None]], axis=1) pose = np.concatenate([pose, np.array([[0, 0, 0, 1]])], axis=0) return pose def get_extrinsics(sensor_meta): sensor = sensor_meta['sensor'] s2e_tvec = np.array(sensor['translation']) s2e_rot = Quaternion(sensor['rotation']).rotation_matrix s2e_pose = compose_transformation(s2e_rot, s2e_tvec) # global pose ego_pose = sensor_meta['ego_pose'] e2g_tvec = np.array(ego_pose['translation']) e2g_rot = Quaternion(ego_pose['rotation']).rotation_matrix e2g_pose = compose_transformation(e2g_rot, e2g_tvec) s2g_pose = np.matmul(e2g_pose, s2e_pose) return s2g_pose def read_cam_meta(cam_meta, nusc): cam_dict = dict() cam_dict['filename'] = cam_meta.get('filename', None) cam_dict['sensor'] = nusc.get('calibrated_sensor', cam_meta['calibrated_sensor_token']) cam_dict['ego_pose'] = nusc.get('ego_pose', cam_meta['ego_pose_token']) cam_dict['is_key_frame'] = cam_meta.get('is_key_frame', False) if cam_dict['is_key_frame']: sample = nusc.get('sample', cam_meta['sample_token']) lidar_meta = nusc.get('sample_data', sample['data']['LIDAR_TOP']) lidar_dict = dict() lidar_dict['filename'] = lidar_meta.get('filename', None) lidar_dict['sensor'] = nusc.get( 'calibrated_sensor', lidar_meta['calibrated_sensor_token']) lidar_dict['ego_pose'] = nusc.get('ego_pose', lidar_meta['ego_pose_token']) cam_dict['lidar_dict'] = lidar_dict return cam_dict def get_intrinsics(cam_dict, is_context=False): sensor = cam_dict['sensor'] intrinsics = np.array(sensor['camera_intrinsic']) return intrinsics def get_pointcloud(path, split, cam_dict): lidar_dict = cam_dict['lidar_dict'] filename = lidar_dict['filename'] filename = os.path.join(path, filename) filename = filename.replace(f'/lidar/', f'/{split}/lidar/') filename = filename.replace(f'{split}_lidar', f'{split}/lidar') pointcloud = np.fromfile(filename, np.float32).reshape(-1, 5) pointcloud = pointcloud[..., :3] extrinsics = get_extrinsics(lidar_dict) return pointcloud, extrinsics def project_depth(cam_pose, lidar_pose, intrinsics, points, H, W): if points.shape[-1] == 4: points[..., -1] = 1.0 if points.shape[-1] == 3: points = np.concatenate( [points, np.ones_like(points[..., :1])], axis=-1) lidar2cam = np.matmul(np.linalg.inv(cam_pose), lidar_pose) points = np.matmul(lidar2cam, points.transpose()) # (4, N) uvs = np.matmul(intrinsics, points[:3, :]) uvs = uvs / (uvs[2:3, :] + 1e-6) uvs = uvs[:2, :] depth_map = np.zeros((H, W), dtype=points.dtype) valid_mask = points[2, :] > 0 valid_pts = points[:, valid_mask] uvs = uvs[:, valid_mask] valid_mask = (uvs[0] > 0) & (uvs[0] < W) & (uvs[1] > 0) & (uvs[1] < H) valid_pts = valid_pts[:, valid_mask] uvs = uvs[:, valid_mask].astype(int) depth_map[uvs[1], uvs[0]] = valid_pts[2] return depth_map def build_index(path, split): nusc = LyftDataset( data_path=path, json_path=f'{path}/{split}/data', verbose=False) cameras = list(range(len(CAMERAS))) camera_dicts_all = defaultdict(list) scene_tokens = {scene['name']: scene['token'] for scene in nusc.scene} for scene_name in scene_tokens.keys(): scene = nusc.get('scene', scene_tokens[scene_name]) first_sample = nusc.get('sample', scene['first_sample_token']) for cam in cameras: camera_dicts = [] cam_name = CAMERAS[int(cam)] cam_meta = nusc.get('sample_data', first_sample['data'][cam_name]) cam_dict = read_cam_meta(cam_meta, nusc) camera_dicts.append(cam_dict) while cam_meta['next']: cam_meta = nusc.get('sample_data', cam_meta['next']) cam_dict = read_cam_meta(cam_meta, nusc) camera_dicts.append(cam_dict) camera_dicts_all[cam].append(camera_dicts) return camera_dicts_all ####################################################### def get_sequences(args): seqs = [f'{args.src}/{split}' for split in ['test']] return seqs def parse_sequence(seq, args): return parse_dst_seq(seq, args) ####################################################### def process_sequence(i, seq, dst_all, args): ### Initialize lists and dicts cameras = list(CAMERAS.values()) num_frames = {cam: dict() for cam in cameras} resolution = {cam: dict() for cam in cameras} labels, lowdim = [], {} path = os.path.dirname(seq) split = os.path.basename(seq) indexes = build_index(path, split) scenes0 = indexes[0] ############ LOOP OVER CAMERAS for i, _ in enumerate(scenes0): for cam_idx, cam in enumerate(cameras): scenes_cam = indexes[cam_idx] dst = f'{dst_all}/{frame_name(i)}' for j, idx in enumerate(scenes_cam[i]): frame = frame_name(j) ######## RGB if 'rgb' not in labels: labels.append('rgb') filename_rgb = f'{path}/{split}/{idx["filename"]}' rgb = np.array(read_image(filename_rgb)) filename_rgb_out = f'{dst}/rgb/{cam}/{frame}.jpg' write_image(filename_rgb_out, rgb) ######## LOWDIM RGB filename_lowdim = add_key_to_dict(lowdim, f'{dst}/lowdim/{cam}/{frame}.npz') lowdim[filename_lowdim]['camera'] = cam lowdim[filename_lowdim]['timestep'] = int(frame) ######## INTRINSICS + EXTRINSICS if 'intrinsics' not in labels: labels.append('intrinsics') if 'extrinsics' not in labels: labels.append('extrinsics') intrinsics = get_intrinsics(idx) extrinsics = get_extrinsics(idx) filename_lowdim = add_key_to_dict(lowdim, f'{dst}/lowdim/{cam}/{frame}.npz') lowdim[filename_lowdim]['extrinsics'] = extrinsics lowdim[filename_lowdim]['intrinsics'] = intrinsics ######## DEPTH if 'depth' not in labels: labels.append('depth') pointcloud, pointcloud_pose = get_pointcloud(path, split, idx) depth = project_depth(extrinsics, pointcloud_pose, intrinsics, pointcloud, *rgb.shape[:2]) filename_depth_out = f'{dst}/depth/{cam}/{frame}.npz' write_npz(filename_depth_out, dict(depth=depth)) resolution[cam]['rgb'] = rgb.shape[:2] num_frames[cam]['rgb'] = len(scenes_cam) resolution[cam]['depth'] = depth.shape[:2] num_frames[cam]['depth'] = len(scenes_cam) ######## WRITE LOWDIM for key, val in lowdim.items(): write_npz(key, val) ############ METADATA filename = f'{dst}/metadata.json' seq_metadata = fill_metadata( args=args, info=dict( name='LyftL5', tags=['real','dynamic','driving'], raw_id=seq.replace(f'{args.src}/', ''), ), labels=labels, cameras=cameras, resolution=resolution, num_frames=num_frames, framerate=10, rgb=dict(extension='jpg'), intrinsics=dict(model='pinhole'), extrinsics=dict(transform='cam2world',metric=True), depth=dict(extension='npz',metric=True,sparse=True), semantic=None, action=None, language=None, specific=None, ) write_json(filename, seq_metadata) return dst_all ####################################################### if __name__ == '__main__': converter = os.path.basename(__file__) run(converter, get_sequences, parse_sequence, process_sequence) #######################################################