Files

50 lines
2.2 KiB
Python

"""Prepare this cup capture with calibrated mid-body depth; no motion."""
import json
from pathlib import Path
import cv2
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
def main():
cap = ROOT / 'data/left_cup_current'
meta = json.loads((cap / 'camera.json').read_text())
cal = json.loads((ROOT / 'data/real_calib/camera.json').read_text())
for key in ('T_world_cam_opencv', 'cam_pos_world', 'extrinsic_status'):
meta[key] = cal[key]
depth = cv2.imread(str(cap / 'depth.png'), -1)
K = np.asarray(meta['K']); T = np.asarray(meta['T_world_cam_opencv'])
u, v = 348, 125
patch = depth[v-4:v+5, u-4:u+5]
valid = patch[(patch > 200) & (patch < 1000)]
if len(valid) < 40:
raise RuntimeError('杯身中部有效深度不足')
z = float(np.median(valid))/1000
p = T[:3,:3] @ (np.linalg.inv(K) @ [u,v,1] * z) + T[:3,3]
# Fit the adjacent clear tabletop, excluding the cup and both hands.
vv, uu = np.mgrid[265:360:3,260:540:3]
zz = depth[vv,uu]/1000
xyz = np.stack(((uu-K[0,2])*zz/K[0,0],(vv-K[1,2])*zz/K[1,1],zz),-1).reshape(-1,3)
xyz = xyz[(xyz[:,2]>.25)&(xyz[:,2]<.8)] @ T[:3,:3].T + T[:3,3]
rng = np.random.default_rng(42); best = np.empty((0,3))
for _ in range(300):
a,b,c = xyz[rng.choice(len(xyz),3,False)]
n = np.cross(b-a,c-a); n /= max(np.linalg.norm(n),1e-12)
if abs(n[2]) < .9: continue
pts = xyz[np.abs((xyz-a)@n)<.004]
if len(pts)>len(best): best=pts
if len(best)<100: raise RuntimeError('桌面拟合失败')
center=best.mean(0); _,_,vt=np.linalg.svd(best-center,full_matrices=False); n=vt[-1]
table_z=float(center[2]-(n[0]*(p[0]-center[0])+n[1]*(p[1]-center[1]))/n[2])
meta['objects']={'cup':{'uv':[u,v],'surface_point_base':p.tolist(),
'selection':'visually selected mid-body surface, not volume center'}}
meta['table_estimate']={'z_at_target':table_z,'normal':n.tolist(),'point':center.tolist(),
'inliers':len(best)}
meta['calibration_source']='data/real_calib/camera.json'
(cap/'camera.json').write_text(json.dumps(meta,indent=2))
print('cup mid-body surface',p.tolist(),'table_z',table_z,'height',p[2]-table_z)
if __name__ == '__main__': main()