from pathlib import Path
import cv2,json,numpy as np,trimesh
from scipy.spatial.transform import Rotation
B=Path(__file__).resolve().parents[1]/'output/20260915_171525_dynhamr';ROOT=B.parents[1];meta=json.loads((ROOT/'docs/20260915_171525/intrinsics.json').read_text());cam=np.load(B/'rgbd_camera.npz');old=np.load(B/'object_poses.npz');new=np.load(B/'object_aligned/object_poses.npz')
models={n:trimesh.load(B/'object_assets'/f'{n}.stl',force='mesh').vertices for n in ['upper','lower']}
c=cv2.VideoCapture(str(ROOT/'docs/20260915_171525/color.mp4'));out=B/'object_aligned';w=cv2.VideoWriter(str(out/'alignment_overlay.mp4'),cv2.VideoWriter_fourcc(*'mp4v'),30,(848,480))
for t in range(352):
 ok,im=c.read();assert ok
 for data,color in [(old,(0,190,255)),(new,(0,255,0))]:
  for n,v in models.items():
   p=Rotation.from_quat(data[n+'_quaternion_xyzw'][t]).apply(v)+data[n+'_position_world'][t];p=(p-cam['c2w'][t,:3,3])@cam['c2w'][t,:3,:3];uv=p[:,:2]/p[:,2,None]*[meta['fx'],meta['fy']]+[meta['cx'],meta['cy']]
   cv2.polylines(im,[cv2.convexHull(np.rint(uv).astype('int32'))],True,color,2)
 cv2.putText(im,'Orange: previous | Green: refitted',(10,25),0,.65,(255,255,255),2);w.write(im)
 if t in [0,83,176,351]:cv2.imwrite(str(out/f'overlay_{t:04d}.jpg'),im)
c.release();w.release()