原路径：/SSD2/yangzijing/egoview-v9-foundationhand-s9l10-20260901/v9_benchmark/hf_0910_slam.py
SHA256：abad1d4bbf2b548f125e9a0b4ece83cd6bd34672415d3ff75c39e9967ff52a9f
1 """A-003 v3 raw stereo/IMU -> isolated SpectacularAI replay and real poses.""" 2 import argparse 3 import hashlib 4 import json 5 import os 6 from pathlib import Path 7 import subprocess 8 import sys 9 import time 10 from concurrent.futures import ThreadPoolExecutor 11 12 import numpy as np 13 14 ROOT=Path('/home/yangzijing/v9_p3r1_hf_20260912') 15 CAL=ROOT/'calibration_20260913/A-003_deployment' 16 SLAMA=Path(__file__).resolve().parents[1]/'artifacts/external/SLAMA' 17 18 19 def read(path):return json.loads(Path(path).read_text()) 20 21 22 def write(path, data): 23 path=Path(path);path.parent.mkdir(parents=True,exist_ok=True) 24 temp=path.with_name(path.name+'.tmp') 25 temp.write_text(json.dumps(data,indent=2,ensure_ascii=False)+'\n') 26 os.replace(temp,path) 27 28 29 def sha(path): 30 h=hashlib.sha256() 31 with Path(path).open('rb') as f: 32 for block in iter(lambda:f.read(4*1024*1024),b''):h.update(block) 33 return h.hexdigest() 34 35 36 def imu_samples(raw, origin, shift, intrinsic_path=None): 37 # Same count decoder and packet clock used by the original calibration 38 # recorder, not a gravity-normalizing scale estimated from this sequence. 39 sys.path.insert(0,str(SLAMA)) 40 from uvc_xu_sixaxis_imu import ImuPacketClock,decode_left_aligned_signed12 41 rows=[json.loads(x) for x in (raw/'imu.ndjson').read_text().splitlines()] 42 intrinsic_path=Path(intrinsic_path) if intrinsic_path is not None else CAL/'imu_intrinsics.json' 43 intrinsic=read(intrinsic_path) 44 clock=ImuPacketClock(120.) 45 packets=[] 46 for row in rows: 47 if row['sample_index']==0:packets.append([]) 48 packets[-1].append(row) 49 result=[];duplicates=0;missed=0 50 for packet in packets: 51 first=packet[0] 52 timing=clock.timestamp_packet(first['device_ticks'],first['host_monotonic_ns']*1e-9,len(packet)) 53 if timing.duplicate:duplicates+=len(packet);continue 54 missed+=timing.missed_packets 55 for row,t in zip(packet,timing.timestamps): 56 vectors=[] 57 for sensor, scale in (('accelerometer','mps2_per_count_xyz'),('gyroscope','rad_per_count_xyz')): 58 counts=np.array([decode_left_aligned_signed12(v & 0xffff) for v in row['raw'][sensor]],float) 59 vectors.append((counts-np.array(intrinsic[sensor]['bias_counts_xyz']))*intrinsic[sensor][scale]) 60 result.append((t-origin+shift,*vectors)) 61 timestamps=np.array([x[0] for x in result]) 62 if not np.all(np.diff(timestamps)>0):raise ValueError('IMU time is not increasing') 63 accel=np.array([x[1] for x in result]);gyro=np.array([x[2] for x in result]) 64 return result,dict(raw_samples=len(rows),used_samples=len(result),duplicate_polled_samples=duplicates, 65 missing_packet_count=missed,effective_sample_hz=len(result)/(timestamps[-1]-timestamps[0]), 66 accel_norm_quantiles=np.percentile(np.linalg.norm(accel,axis=1),[0,50,95,100]).tolist(), 67 gyro_norm_quantiles=np.percentile(np.linalg.norm(gyro,axis=1),[0,50,95,100]).tolist(), 68 time_method='SLAMA ImuPacketClock: unique device_ticks, host packet interval de-batching, 120Hz fallback', 69 units=['m/s^2','rad/s'],count_transform='signed 12-bit from high bits of supplied signed int16', 70 imu_event_shift_seconds=shift,additional_accel_norm_scale=1., 71 source_decoder_sha256=sha(SLAMA/'uvc_xu_sixaxis_imu.py'),intrinsics_sha256=sha(intrinsic_path)) 72 73 74 def build(entry,out): 75 session=out/'session';receipt=out/'session_report.json' 76 if receipt.exists():return read(receipt) 77 checks=read(ROOT/'0910_calibration_a003/report.json') 78 samples=[x for x in checks['rows'] if x.get('session_id')==entry['session_id']] 79 if not samples or any(x['status']!='pass' for x in samples):raise ValueError('stereo validation not passed') 80 if sha(CAL/'stereo_fisheye_calibration.json')!=checks['calibration_sha256']:raise ValueError('calibration changed') 81 raw=Path(entry['raw_root']);manifest=read(raw/'manifest.json') 82 if sha(raw/'manifest.json')!=read(Path(entry['synced_video']).parent/'processing-manifest.json')['group']['ego']['manifest_sha256']: 83 raise ValueError('source group identity mismatch') 84 frames=[json.loads(x) for x in (raw/'frames.ndjson').read_text().splitlines()] 85 origin=frames[0]['host_monotonic_ns']*1e-9 86 calibration=read(CAL/'camera_imu_calibration.json') 87 shift=float(calibration['cameras'][0]['imuToCameraShiftSeconds']) 88 imu,imu_report=imu_samples(raw,origin,shift) 89 # Apply shift exactly once to events, preserve the original artifact separately. 90 for camera in calibration['cameras']:camera['imuToCameraShiftSeconds']=0. 91 session.mkdir(parents=True,exist_ok=True) 92 write(session/'calibration.json',calibration) 93 source_files={} 94 for side,name in (('left','data.mp4'),('right','data2.mp4')): 95 source=[] 96 for segment in manifest['video']['segments']: 97 ref=segment['artifacts'][side];path=raw/ref['path'] 98 if path.stat().st_size!=ref['bytes'] or sha(path)!=ref['sha256']:raise ValueError('raw video hash mismatch') 99 source.append(path) 100 listing=out/(side+'_concat.txt') 101 listing.write_text(''.join("file '"+str(p).replace("'","'\\''")+"'\n" for p in source)) 102 target=session/name 103 subprocess.run(['ffmpeg','-v','error','-nostdin','-f','concat','-safe','0','-i',str(listing),'-map','0:v:0','-c','copy','-y',str(target)],check=True) 104 info=json.loads(subprocess.check_output(['ffprobe','-v','error','-select_streams','v:0','-show_entries','stream=nb_frames,width,height','-of','json',str(target)]))['streams'][0] 105 if int(info['nb_frames'])!=len(frames) or (info['width'],info['height'])!=(1920,1080):raise ValueError('replay video frame identity mismatch') 106 source_files[name]=dict(sha256=sha(target),frames=len(frames),sources=[str(p) for p in source]) 107 events=[] 108 for t,acc,gyro in imu: 109 for sensor,v in (('accelerometer',acc),('gyroscope',gyro)): 110 events.append(dict(time=float(t),sensor=dict(type=sensor,values=v.tolist()))) 111 for index,frame in enumerate(frames): 112 if frame['frame']!=index:raise ValueError('source frame index differs') 113 t=frame['host_monotonic_ns']*1e-9-origin 114 events.append(dict(time=float(t),number=index,frames=[dict(cameraInd=0),dict(cameraInd=1)])) 115 events.sort(key=lambda e:e['time']) 116 with (session/'data.jsonl').open('w') as f: 117 for event in events:f.write(json.dumps(event,separators=(',',':'))+'\n') 118 report=dict(sequence_id=entry['sequence_id'],session_id=entry['session_id'],frames=len(frames), 119 source_origin_host_seconds=origin,session=str(session),imu=imu_report,source_files=source_files, 120 source_references=dict(raw=entry['source_reference'],sync=entry['processing_manifest']), 121 calibration_source=str(CAL),calibration_sha256=sha(CAL/'camera_imu_calibration.json'), 122 raw_manifest_sha256=sha(raw/'manifest.json'),events_sha256=sha(session/'data.jsonl'), 123 calibration_quality=read(CAL/'quality_summary.json'), 124 camera_space='A-003 raw left OpenCV optical',world='independent metric SLAM session; not registered to XR object world') 125 write(receipt,report) 126 print(json.dumps(dict(stage='slam_session_ready',sequence=entry['sequence_id'],frames=len(frames),imu=imu_report)),flush=True) 127 return report 128 129 130 def pose_row(camera): 131 p=camera.pose;q=p.orientation;t=p.position 132 return [float(p.time),float(t.x),float(t.y),float(t.z),float(q.x),float(q.y),float(q.z),float(q.w)] 133 134 135 def run(entry,out,stop_seconds=None): 136 import spectacularAI as sai 137 start=time.monotonic();session=build(entry,out);poses={};counts={};keyframes={};final_map=False 138 def callback(vio): 139 status=str(vio.status);counts[status]=counts.get(status,0)+1 140 if vio.status==sai.TrackingStatus.TRACKING: 141 row=pose_row(vio.getCameraPose(0));poses[row[0]]=row 142 if sum(counts.values())%300==0:print(json.dumps(dict(stage='slam_tracking',sequence=entry['sequence_id'],counts=counts)),flush=True) 143 def mapped(output): 144 nonlocal final_map 145 if output.finalMap:final_map=True 146 ids=output.map.keyFrames.keys() if output.finalMap else output.updatedKeyFrames 147 for key in ids: 148 if key not in output.map.keyFrames:continue 149 keyframes[int(key)]=pose_row(output.map.keyFrames[key].frameSet.primaryFrame.cameraPose) 150 config=dict(useStereo=True,useSlam=True,blockingReplay=True,vioThread=False,parameterSets=['wrapper-base','offline-base']) 151 replay=sai.Replay(session['session'],mapperCallback=mapped,configuration=config, 152 ignoreFolderConfiguration=True,stopTime=stop_seconds) 153 replay.setPlaybackSpeed(-1);replay.setOutputCallback(callback);replay.runReplay();replay.close() 154 rows=np.array([poses[t] for t in sorted(poses)],dtype=np.float64) 155 np.savez_compressed(out/'trajectory.npz',pose_t_xyz_xyzw=rows, 156 keyframe_t_xyz_xyzw=np.array(list(keyframes.values()),dtype=np.float64).reshape(-1,8)) 157 from scipy.spatial.transform import Rotation 158 if len(rows)>1: 159 dr=Rotation.from_quat(rows[:,4:]);rot=(dr[:-1].inv()*dr[1:]).magnitude()*180/np.pi 160 step=np.linalg.norm(np.diff(rows[:,1:4],axis=0),axis=1) 161 else:step=rot=np.array([np.nan]) 162 tracking=len(rows)/max(1,sum(counts.values())) 163 report=dict(sequence_id=entry['sequence_id'],status='completed' if len(rows)>1 else 'failed_no_tracking', 164 counts=counts,tracking_fraction=tracking,poses=len(rows),keyframes=len(keyframes),final_map_received=final_map, 165 seconds=time.monotonic()-start,stop_seconds=stop_seconds,configuration=config, 166 trajectory_sha256=sha(out/'trajectory.npz'),camera_space='raw left optical', 167 world='SLAM_session_'+entry['session_id'],object_world_registered=False, 168 translation_step_quantiles_m=np.percentile(step,[50,95,99,100]).tolist(), 169 rotation_step_quantiles_deg=np.percentile(rot,[50,95,99,100]).tolist()) 170 write(out/'slam_report.json',report);print(json.dumps(report),flush=True) 171 return report 172 173 174 def batch(output): 175 entries=[x for x in read(ROOT/'0910_readiness.json')['results'] if 'raw_root' in x] 176 def launch(entry): 177 dest=output/entry['sequence_id'] 178 if (dest/'slam_report.json').exists():return read(dest/'slam_report.json') 179 dest.mkdir(parents=True,exist_ok=True) 180 with (dest/'run.log').open('w') as log: 181 completed=subprocess.run([sys.executable,'-m','v9_benchmark.hf_0910_slam','--sequence',entry['sequence_id'],'--output',str(dest)],stdout=log,stderr=subprocess.STDOUT) 182 if completed.returncode: return dict(sequence_id=entry['sequence_id'],status='failed',log=str(dest/'run.log')) 183 return read(dest/'slam_report.json') 184 results=[] 185 with ThreadPoolExecutor(max_workers=2) as pool: 186 for report in pool.map(launch,entries): 187 results.append(report);write(output/'batch_status.json',dict(results=results,total=len(entries))) 188 print(json.dumps(report),flush=True) 189 190 191 if __name__=='__main__': 192 p=argparse.ArgumentParser();p.add_argument('--sequence');p.add_argument('--all',action='store_true');p.add_argument('--output',required=True,type=Path) 193 p.add_argument('--stop-seconds',type=float);p.add_argument('--build-only',action='store_true');a=p.parse_args() 194 a.output.mkdir(parents=True,exist_ok=True) 195 if a.all:batch(a.output) 196 else: 197 entry=next(x for x in read(ROOT/'0910_readiness.json')['results'] if x['sequence_id']==a.sequence) 198 build(entry,a.output) if a.build_only else run(entry,a.output,a.stop_seconds)