21 lines
1.6 KiB
Python
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)
|