27 lines
2.1 KiB
Python
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)))
|