50 lines
2.2 KiB
Python
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()
|