"""Audit RGB-D input, preserve object meshes and estimate static-background camera motion."""
import os
os.environ.setdefault('OMP_NUM_THREADS','4')
from pathlib import Path
import json
import shutil
import cv2
import numpy as np
import open3d as o3d

ROOT=Path(__file__).resolve().parents[1]
SRC=ROOT/'docs/20260915_171525';OUT=ROOT/'output/20260915_171525';OUT.mkdir(exist_ok=True)
meta=json.loads((SRC/'intrinsics.json').read_text())
times=np.asarray(meta['timestamps_ms'])/1000;times-=times[0]
assert len(times)==meta['frames'] and np.all(np.diff(times)>0)
assert len(list((SRC/'depth').glob('*.png')))==len(times)
assets=OUT/'object_assets';assets.mkdir(exist_ok=True)
meshes=[]
for source,name in [('上半.stl','upper.stl'),('下半.stl','lower.stl')]:
    shutil.copy2(ROOT/'docs'/source,assets/name)
    mesh=o3d.io.read_triangle_mesh(str(assets/name));v=np.asarray(mesh.vertices)
    meshes.append(dict(file=name,vertices=len(v),triangles=len(mesh.triangles),bounds=[v.min(0).tolist(),v.max(0).tolist()],extent_m=np.ptp(v,axis=0).tolist()))
(assets/'manifest.json').write_text(json.dumps(dict(units='assumed meters from numeric dimensions; compare observed depth before claiming registration',parts=meshes,preserve_shared_coordinates=True),indent=2))
cap=cv2.VideoCapture(str(SRC/'color.mp4'));K=o3d.camera.PinholeCameraIntrinsic(424,240,meta['fx']/2,meta['fy']/2,(meta['cx']+.5)/2-.5,(meta['cy']+.5)/2-.5)
option=o3d.pipelines.odometry.OdometryOption();option.depth_max=1.5;option.depth_min=.10;option.depth_diff_max=.025
option.iteration_number_per_pyramid_level=o3d.utility.IntVector([15,8,5])
c2w=[np.eye(4)];valid=[True];rows=[];previous=None
for i in range(len(times)):
    ok,frame=cap.read();assert ok
    depth=cv2.imread(str(SRC/'depth'/f'{i:06d}.png'),-1);assert depth.dtype==np.uint16 and depth.shape==frame.shape[:2]
    color=cv2.resize(frame,(424,240),interpolation=cv2.INTER_AREA)
    z=cv2.resize(depth,(424,240),interpolation=cv2.INTER_NEAREST).astype(np.float32)*meta['depth_scale_m']
    hsv=cv2.cvtColor(color,cv2.COLOR_BGR2HSV);ycc=cv2.cvtColor(color,cv2.COLOR_BGR2YCrCb)
    skin=cv2.inRange(ycc,np.array([0,133,77]),np.array([255,173,127]))>0
    colored=(hsv[:,:,1]>85)&(((hsv[:,:,0]<14)|(hsv[:,:,0]>165))|((hsv[:,:,0]>90)&(hsv[:,:,0]<135)))
    dynamic=skin|colored;dynamic[160:]=True
    dynamic=cv2.dilate(dynamic.astype(np.uint8),np.ones((9,9),np.uint8))>0
    z[dynamic]=0
    rgbd=o3d.geometry.RGBDImage.create_from_color_and_depth(o3d.geometry.Image(cv2.cvtColor(color,cv2.COLOR_BGR2RGB)),o3d.geometry.Image(z),depth_scale=1.,depth_trunc=1.5,convert_rgb_to_intensity=True)
    if previous is not None:
        success,T,info=o3d.pipelines.odometry.compute_rgbd_odometry(previous,rgbd,K,np.eye(4),o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(),option)
        angle=np.arccos(np.clip((np.trace(T[:3,:3])-1)/2,-1,1))
        accepted=bool(success and np.isfinite(T).all() and np.linalg.norm(T[:3,3])<.06 and angle<.12)
        valid.append(accepted)
        c2w.append(c2w[-1]@np.linalg.inv(T) if accepted else c2w[-1].copy())
        rows.append(dict(frame=i,accepted=accepted,translation_step_m=float(np.linalg.norm(T[:3,3])),rotation_step_rad=float(angle)))
    previous=rgbd
    if i%50==0:print('RGBD camera',i,len(times),flush=True)
cap.release()
np.savez_compressed(OUT/'rgbd_camera.npz',c2w=np.asarray(c2w),intrinsics=np.array([meta[k] for k in ['fx','fy','cx','cy']]),time=times,valid=np.asarray(valid),method='Open3D adjacent RGBD odometry; mask skin/red/blue/bottom third; first camera world; no loop closure')
report=dict(frames=len(times),time_span_s=float(times[-1]),color_depth_timestamp_max_difference_ms=float(np.max(np.abs(np.array(meta['timestamps_ms'])-meta['depth_timestamps_ms']))),camera_valid_steps=int(np.sum(valid)),camera_rejected_frames=np.flatnonzero(~np.asarray(valid)).tolist(),steps=rows,boundary='Odometry estimates, not external tracking truth; drift possible; failed steps held and explicitly invalid')
(OUT/'rgbd_preflight.json').write_text(json.dumps(report,indent=2))
print(json.dumps({k:v for k,v in report.items() if k!='steps'},indent=2))