# Copyright 2026 Toyota Research Institute. All rights reserved. import os import cv2 import numpy as np from tqdm import tqdm from glob import glob from anydata.utils.read import read_numpy, read_yaml, read_image, read_depth, read_json from anydata.utils.write import write_json, write_jpg_from_mp4, write_lowdim, write_labels from anydata.converters.utils import run, add_key_to_dict, fill_metadata, parse_dst_seq, frame_name, crawl, prepare_lowdim from anydata.converters.utils import extract_mp4, extract_mp4_shape from anydata.sync.sync_utils import remove_path ####################################################### cam_map1 = { "C10095_rgb": 'exo1', "C10115_rgb": 'exo2', "C10118_rgb": 'exo3', "C10119_rgb": 'exo4', "C10379_rgb": 'exo5', "C10390_rgb": 'exo6', "C10395_rgb": 'exo7', "C10404_rgb": 'exo8', "HMC_84346135_mono10bit": 'ego1', "HMC_84347414_mono10bit": 'ego2', "HMC_84355350_mono10bit": 'ego3', "HMC_84358933_mono10bit": 'ego4', "HMC_21176875_mono10bit": 'ego5', "HMC_21176623_mono10bit": 'ego6', "HMC_21110305_mono10bit": 'ego7', "HMC_21179183_mono10bit": 'ego8', } cam_map2 = { 'exo1': "C10095_rgb", 'exo2': "C10115_rgb", 'exo3': "C10118_rgb", 'exo4': "C10119_rgb", 'exo5': "C10379_rgb", 'exo6': "C10390_rgb", 'exo7': "C10395_rgb", 'exo8': "C10404_rgb", 'ego1': "HMC_84346135_mono10bit", 'ego2': "HMC_84347414_mono10bit", 'ego3': "HMC_84355350_mono10bit", 'ego4': "HMC_84358933_mono10bit", 'ego5': "HMC_21176875_mono10bit", 'ego6': "HMC_21176623_mono10bit", 'ego7': "HMC_21110305_mono10bit", 'ego8': "HMC_21179183_mono10bit", } # C10095_rgb : v1 # C10115_rgb : v2 # C10118_rgb : v3 # C10119_rgb : v4 # C10379_rgb : v5 # C10390_rgb : v6 # C10395_rgb : v7 # C10404_rgb : v8 # HMC_84346135_mono10bit or HMC_21176875_mono10bit : e1 # HMC_84347414_mono10bit or HMC_21176623_mono10bit : e2 # HMC_84355350_mono10bit or HMC_21110305_mono10bit : e3 # HMC_84358933_mono10bit or HMC_21179183_mono10bit : e4 def prep_intrinsics_dict(intrinsics_raw, cameras): params = ['cx','cy','fx','fy', 'k1','k2','k3','k4','k5','k6', 'p1','p2','p3','p4'] intrinsics = {key: {p: [] for p in params} for key in cameras} for i in range(len(intrinsics_raw)): for j in range(len(intrinsics_raw[i])): serialno = intrinsics_raw[i][j]['Camera']['SerialNo'] if serialno.endswith('mono10bit'): serialno = 'HMC_' + serialno if serialno in intrinsics: for p in params: intrinsics[serialno][p].append(intrinsics_raw[i][j]['Camera'][p]) for key in intrinsics.keys(): for p in intrinsics[key].keys(): if len(intrinsics[key][p]) > 0: intrinsics[key][p] = sum(intrinsics[key][p]) / len(intrinsics[key][p]) intrinsics[key] = np.array([intrinsics[key][v] for v in [ 'fx','fy','cx','cy','k1','k2','p1','p2','k3']]) return intrinsics ####################################################### def get_sequences(args): seqs = glob(f'{args.src}/AssemblyPoses/camera_extrinsics_fixed/*.json') return seqs def parse_sequence(seq, args): return parse_dst_seq(seq, args, remove=[0,1], remext=True) ####################################################### def process_sequence(i, seq, dst_all, args): name = os.path.basename(seq)[:-len('.json')] split = seq.split('/') seq = '/'.join(split[:-3] + [name]) ### Initialize lists and dicts cameras_raw = sorted(glob(f'{seq}/*.mp4')) cameras_raw = [os.path.basename(c)[:-4] for c in cameras_raw] cameras = [cam_map1[c] for c in cameras_raw] cameras = [c for c in cameras if c.startswith('exo')] name = os.path.basename(seq) timestamp = f'{args.src}/AssemblyPoses/timestamp/{name}.json' timestamp = read_json(timestamp) extrinsics_ego_path = f'{args.src}/AssemblyPoses/camera_extrinsics_ego/{name}.json' extrinsics_ego = read_json(extrinsics_ego_path) has_ego_extrinsics = extrinsics_ego is not None extrinsics_fixed_path = f'{args.src}/AssemblyPoses/camera_extrinsics_fixed/{name}.json' extrinsics_fixed = read_json(extrinsics_fixed_path) has_fixed_extrinsics = extrinsics_fixed is not None intrinsics_raw = glob(f'{args.src}/AssemblyHands/calib/nimble_json_calib/*.json') intrinsics_raw = [read_json(i) for i in intrinsics_raw] intrinsics = prep_intrinsics_dict(intrinsics_raw, cameras_raw) ############ LOOP OVER CAMERAS video_shapes = {cam: extract_mp4_shape(f'{seq}/{cam_map2[cam]}.mp4') for cam in cameras} length = min([video_shape[0] for video_shape in video_shapes.values()]) if has_ego_extrinsics: num_ego_extrinsics = len(extrinsics_ego) length = min(length, num_ego_extrinsics) max_length = 3000 if length > max_length: intervals = np.linspace(0, length, length // max_length + 2) else: intervals = [0, length] intervals = [int(i) for i in intervals] dsts = [] ############ LOOP OVER CAMERAS for k in tqdm(range(len(intervals) - 1), ncols=96, leave=False): st, fn = intervals[k], intervals[k+1] length_k = fn - st dst = f'{dst_all}/%d_%d' % (st, fn) dsts.append(dst) num_frames = {cam: dict() for cam in cameras} resolution = {cam: dict() for cam in cameras} labels, lowdim = [], {} dense_labels = ['rgb'] for cam in tqdm(cameras, ncols=96, leave=False): dense = {label: dict() for label in dense_labels} cam_mapped = cam_map2[cam] dense['rgb'] = extract_mp4(f'{seq}/{cam_map2[cam]}.mp4', st, fn) for i in range(length_k): frame = frame_name(i) ######## LOWDIM RGB prepare_lowdim(lowdim, dst, cam, frame) ######## INTRINSICS # if cam_mapped in intrinsics: # Try to load intrinsics filename_lowdim = add_key_to_dict(lowdim, f'{dst}/lowdim/{cam}/{frame}.npz') lowdim[filename_lowdim]['intrinsics'] = intrinsics[cam_mapped] # else: # Error, skip this label for this sample # pass ######## EXTRINSICS # try: # Try to load extrinsics if cam.startswith('exo'): extrinsics = extrinsics_fixed[cam_mapped.replace('_',':')] elif cam.startswith('ego'): extrinsics = extrinsics_ego[str(i)][cam_mapped[4:].replace('_',':')] extrinsics = np.array(extrinsics).reshape(4, 4) extrinsics[:3, -1] /= 1000 filename_lowdim = add_key_to_dict(lowdim, f'{dst}/lowdim/{cam}/{frame}.npz') lowdim[filename_lowdim]['extrinsics'] = extrinsics # except: # Error, skip this label for this sample # print('PASSING EXT') # pass ######## 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='Assembly101', tags=['real','tabletop','indoor','human'], raw_id=seq.replace(f'{args.src}/', ''), ), labels=labels, cameras=cameras, resolution=resolution, num_frames=num_frames, framerate=30, rgb=dict(extension='jpg'), intrinsics=dict(model='pinhole'), extrinsics=dict(transform='cam2world',metric=True), depth=None, semantic=None, action=None, language=None, specific=dict( has_ego_extrinsics=has_ego_extrinsics, has_fixed_extrinsics=has_fixed_extrinsics, ), ) write_json(filename, seq_metadata) return dsts ####################################################### if __name__ == '__main__': converter = os.path.basename(__file__) run(converter, get_sequences, parse_sequence, process_sequence) #######################################################