# Copyright 2026 Toyota Research Institute. All rights reserved. import os import json import math import torch import pytorch3d import numpy as np from glob import glob from scipy.spatial.transform import Rotation from pytorch3d.renderer import FoVPerspectiveCameras from anydata.utils.data import flatten from anydata.utils.geometry import invert_extrinsics from anydata.utils.read import read_numpy, read_yaml, read_image, read_depth from anydata.utils.write import write_json, write_lowdim, write_labels from anydata.converters.utils import run, add_key_to_dict, fill_metadata, parse_dst_seq, frame_name, crawl, prepare_lowdim ###################################################### def get_intrinsics_from_data(width, height, fov): znear, zfar = 0.001, 512.0 Kfov = FoVPerspectiveCameras( device='cpu', fov=fov, degrees=False, R=None, T=None, ).compute_projection_matrix( znear=znear, zfar=zfar, fov=fov, aspect_ratio=1.0, degrees=False )[0] K = np.eye(3) K[0, 0] = Kfov[0, 0] * width / 2 K[1, 1] = Kfov[1, 1] * height / 2 K[0, 2] = width / 2 K[1, 2] = height / 2 return K def get_intrinsics(filename): data = json.load(open(filename, 'r')) width = height = data['resolution'] fov = data['field_of_view_rads'] return get_intrinsics_from_data(width, height, fov) def eul2rot(theta) : R = np.array([ [np.cos(theta[1])*np.cos(theta[2]), np.sin(theta[0])*np.sin(theta[1])*np.cos(theta[2]) - np.sin(theta[2])*np.cos(theta[0]), np.sin(theta[1])*np.cos(theta[0])*np.cos(theta[2]) + np.sin(theta[0])*np.sin(theta[2])], [np.sin(theta[2])*np.cos(theta[1]), np.sin(theta[0])*np.sin(theta[1])*np.sin(theta[2]) + np.cos(theta[0])*np.cos(theta[2]), np.sin(theta[1])*np.sin(theta[2])*np.cos(theta[0]) - np.sin(theta[0])*np.cos(theta[2])], [-np.sin(theta[1]), np.sin(theta[0])*np.cos(theta[1]), np.cos(theta[0])*np.cos(theta[1])], ]) return R def get_pose(filename): data = json.load(open(filename, 'r')) if 'camera_location' in data: location = data['camera_location'] rotation = data['camera_rotation_final'] elif 'location' in data: location = data['location'] rotation = data['rotation'] else: raise ValueError(f'Wrong data format {seq}') rot_roll = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) Tx, Ty, Tz = location ex, ey, ez = rotation EULER_X_OFFSET_RADS = math.radians(90.0) R_inv = pytorch3d.transforms.euler_angles_to_matrix(torch.tensor( [((ex - EULER_X_OFFSET_RADS), -ey, -ez)], dtype=torch.double, device='cpu'), 'XZY') # R_inv = eul2rot(torch.tensor([((ex - EULER_X_OFFSET_RADS), -ey, -ez)])) t_inv = torch.tensor([[-Tx, Tz, Ty]], dtype=torch.double, device='cpu') pose = np.eye(4) pose[:3, :3] = R_inv.transpose(1,2) pose[-1, :3] = - R_inv.bmm(t_inv.unsqueeze(-1)).squeeze(-1) pose = np.transpose(pose) pose = invert_extrinsics(pose) pose[:3, :3] = pose[:3, :3] @ rot_roll return pose ####################################################### def get_sequences(args): seqs = glob(f'{args.src}/rgb/*') seqs = flatten([glob(f'{s}/*') for s in seqs]) seqs = [s for s in seqs if 'hypersim' not in s] return seqs def parse_sequence(seq, args): return parse_dst_seq(seq, args, remove=[0]) ####################################################### def get_point(f): return int(os.path.basename(f).split('_')[1]) def process_sequence(i, seq, dst_all, args): ### Get filenames split = seq.split('/')[-2].split('__') # seq = f'{seq}/{split[1]}/{split[2]}' filename_rgbs_all = sorted(glob(f'{seq}/**/*.png', recursive=True)) filename_depths_all = sorted(glob(f'{seq.replace("rgb","depth_zbuffer")}/**/*.png', recursive=True)) filename_points_all = sorted(glob(f'{seq.replace("rgb","point_info")}/**/*.json', recursive=True)) points = sorted(list(set([get_point(f) for f in filename_rgbs_all]))) dsts = [] for p in points: dst = f'{dst_all}/%04d' % p dsts.append(dst) filename_rgbs = [f for f in filename_rgbs_all if get_point(f) == p] filename_depths = [f for f in filename_depths_all if get_point(f) == p] filename_points = [f for f in filename_points_all if get_point(f) == p] ### Initialize lists and dicts cameras = ['0'] num_frames = {cam: dict() for cam in cameras} resolution = {cam: dict() for cam in cameras} labels, lowdim = [], {} dense_labels = ['rgb','depth'] ############ LOOP OVER CAMERAS for cam in cameras: dense = {label: dict() for label in dense_labels} ######## RGB FILENAMES for i, filename_rgb in enumerate(filename_rgbs): frame = frame_name(i) ######## RGB rgb = np.array(read_image(filename_rgb)) dense['rgb'][frame] = rgb ######## LOWDIM RGB prepare_lowdim(lowdim, dst, cam, frame) ######## DEPTH FILENAMES for i, filename_depth in enumerate(filename_depths): frame = frame_name(i) ######## DEPTH depth = read_depth(filename_depth, div=512) dense['depth'][frame] = depth ######## POINT FILENAMES for i, filename_point in enumerate(filename_points): frame = frame_name(i) ######## INTRINSICS + EXTRINSICS filename_lowdim = add_key_to_dict(lowdim, f'{dst}/lowdim/{cam}/{frame}.npz') lowdim[filename_lowdim]['extrinsics'] = get_pose(filename_point) lowdim[filename_lowdim]['intrinsics'] = get_intrinsics(filename_point) ######## WRITE LABELS write_labels(dst, cam, args.storage, dense, labels, resolution, num_frames) ######## WRITE LOWDIM write_lowdim(args, dst, labels, num_frames, lowdim) ############ METADATA filename = f'{dst}/metadata.json' seq_metadata = fill_metadata( args=args, info=dict( name='OmnidataStarter', tags=['sim','static'], 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=False), semantic=None, action=None, language=None, specific=None, ) write_json(filename, seq_metadata) return dsts ####################################################### if __name__ == '__main__': converter = os.path.basename(__file__) run(converter, get_sequences, parse_sequence, process_sequence) #######################################################