Files
a7l20-hug-sim/scripts/perceive_workcell_cup.py

21 lines
1.6 KiB
Python

"""Detect dark cup from simulated RGB-D, with no object ground-truth pose input."""
import json
from pathlib import Path
import cv2,numpy as np
from scipy.optimize import least_squares
ROOT=Path(__file__).resolve().parents[1]
p=ROOT/'data/cup_sim_capture_20260928';cam=json.loads((p/'camera.json').read_text());K=np.array(cam['K']);T=np.array(cam['T_world_cam_opencv'])
im=cv2.imread(str(p/'rgb.png'));z=cv2.imread(str(p/'depth.png'),-1)/1000.;v,u=np.indices(z.shape)
xyz=np.stack([(u-K[0,2])*z/K[0,0],(v-K[1,2])*z/K[1,1],z],-1)@T[:3,:3].T+T[:3,3]
h=xyz[:,:,2]-.755
mask=((h>.02)&(h<.30)&(xyz[:,:,0]>.25)&(xyz[:,:,0]<.85)&(abs(xyz[:,:,1])<.6)&(im.max(2)<165)).astype(np.uint8)
n,labels,stats,_=cv2.connectedComponentsWithStats(mask)
if n<2:raise RuntimeError('No cup detected')
label=1+np.argmax(stats[1:,cv2.CC_STAT_AREA]);mask=labels==label
pts=xyz[mask];uv=np.c_[u[mask],v[mask]]
sel=(pts[:,2]>.87)&(pts[:,2]<.95);xy=pts[sel,:2]
f=least_squares(lambda a:np.linalg.norm(xy-a[:2],axis=1)-a[2],np.r_[xy.mean(0),.045],bounds=([.25,-.6,.02],[.85,.6,.08]),loss='soft_l1',f_scale=.001)
k=np.argmin(abs(pts[:,2]-(.755+.12))+.1*np.linalg.norm(pts[:,:2]-np.median(pts[:,:2],0),axis=1))
r={'objects':{'cup':{'uv':uv[k].tolist(),'surface_point_world':pts[k].tolist(),'estimated_axis_xy_m':f.x[:2].tolist(),'estimated_upper_radius_m':float(f.x[2]),'pixels':int(mask.sum())}},'method':'depth above known simulated support plane + dark connected component + cylinder fit','ground_truth_pose_used_for_detection':False}
(p/'perception.json').write_text(json.dumps(r,indent=2));cv2.circle(im,tuple(uv[k]),6,(0,255,0),2);cv2.imwrite(str(p/'perception.png'),im);print(r)