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

27 lines
2.1 KiB
Python

import cv2,numpy as np,json
from scipy.optimize import least_squares
from pathlib import Path
p=(Path(__file__).resolve().parents[1]/'data/cup_model_20260928');im=cv2.imread(str(p/'rgb.png'));dep=cv2.imread(str(p/'depth.png'),-1)/1000.;K=np.array(json.loads((p/'camera.json').read_text())['K']);vv,uu=np.indices(dep.shape);xyz=np.stack([(uu-K[0,2])*dep/K[0,0],(vv-K[1,2])*dep/K[1,1],dep],-1)
roi=(uu>270)&(uu<655)&(vv>130)&(vv<395)&(dep>.25)&(dep<.8);pts=xyz[roi][::3];rng=np.random.default_rng(42);best=[]
for _ in range(400):
a,b,c=pts[rng.choice(len(pts),3,False)];n=np.cross(b-a,c-a);n/=max(np.linalg.norm(n),1e-10);sel=np.abs((pts-a)@n)<.003
if np.sum(sel)>len(best):best=pts[sel]
center=np.mean(best,0);_,_,vt=np.linalg.svd(best-center,full_matrices=False);n=vt[-1]
if n@center>0:n=-n
h=(xyz-center)@n
mask=np.zeros(dep.shape,np.uint8);cv2.fillPoly(mask,[np.array([[553,46],[628,52],[696,92],[704,139],[653,218],[548,300],[507,320],[467,307],[457,283],[478,212],[508,131],[531,75]])],1)
mask=(mask>0)&(dep>.15)&(dep<.8)&(h>.008)&(h<.5)
x=np.array([1.,0,0]);x-=n*(x@n);x/=np.linalg.norm(x);y=np.cross(n,x);basis=np.column_stack([x,y,n]);cup=(xyz[mask]-center)@basis
print('plane',center,n,'inliers',len(best));print('height quantiles',np.quantile(cup[:,2],[0,.01,.5,.95,.99,1]),'points',len(cup))
rows=[]
for lo in np.arange(.01,.35,.025):
pts=cup[(cup[:,2]>lo)&(cup[:,2]<lo+.025)]
if len(pts)<35:continue
xy=pts[:,:2];c=xy.mean(0)
fit=least_squares(lambda a:np.linalg.norm(xy-a[:2],axis=1)-a[2],np.r_[c,.045],bounds=([-1,-1,.015],[1,1,.08]),loss='soft_l1',f_scale=.001)
rows.append({'height_m':lo+.0125,'center':fit.x[:2].tolist(),'radius_m':fit.x[2],'rms':float(np.sqrt(np.mean(fit.fun**2))),'count':len(pts)})
print(json.dumps(rows,indent=2))
np.savez(p/'cup_points.npz',cup=cup,plane_center=center,plane_normal=n,basis=basis,xyz=xyz,mask=mask)
(p/'cup_measurement_initial.json').write_text(json.dumps({'plane_center_cam':center.tolist(),'plane_normal_cam':n.tolist(),'height_observed_max_m':float(cup[:,2].max()),'sections':rows},indent=2))
cv2.imwrite(str(p/'cup_mask.png'),np.where(mask[:,:,None],im,np.uint8(im*.25)))