20 lines
2.1 KiB
Python
20 lines
2.1 KiB
Python
"""Locate the same cup from a new D405 frame; preserve measured dimensions."""
|
|
from pathlib import Path
|
|
import json,shutil,datetime
|
|
import numpy as np,cv2
|
|
from scipy.optimize import least_squares
|
|
import xml.etree.ElementTree as E
|
|
ROOT=Path(__file__).resolve().parents[1];p=ROOT/'data/cup_moved_capture_20260928'
|
|
c=json.loads((p/'camera.json').read_text());K=np.array(c['K']);T=np.array(json.loads((ROOT/'data/cup_model_20260928/registered_camera.json').read_text())['T_world_cam_opencv'])
|
|
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]
|
|
mask=(u>406)&(u<547)&(v>10)&(v<198)&(xyz[:,:,2]>.795)&(xyz[:,:,2]<.935)&(z>0)
|
|
pts=xyz[mask];model=json.loads((ROOT/'data/cup_model_20260928/cup_model.json').read_text());profile=np.array(model['profile_z_radius_m']);rad=np.interp(pts[:,2]-.755,profile[:,0],profile[:,1])
|
|
f=least_squares(lambda a:np.linalg.norm(pts[:,:2]-a,axis=1)-rad,pts[:,:2].mean(0),loss='soft_l1',f_scale=.002)
|
|
new=[*f.x,.755];old=model['position_floor_m'];report={'old_position_m':old,'new_position_m':new,'shift_m':(np.array(new)-old).tolist(),'points':len(pts),'median_fit_residual_m':float(np.median(abs(f.fun))),'camera_transform':'existing tabletop-registered extrinsic assumed camera stationary','dimensions_reused':True}
|
|
backup=ROOT/'backup'/('cup-moved-'+datetime.datetime.now().strftime('%Y%m%dT%H%M%S'));backup.mkdir(parents=True)
|
|
for file in ['scene/cup_workcell.xml','data/cup_sim_capture_20260928/preview_scene.xml']:
|
|
path=ROOT/file;shutil.copy2(path,backup/path.name);tree=E.parse(path);tree.find(".//body[@name='measured_cup']").set('pos',' '.join(map(str,new)));tree.write(path,encoding='unicode')
|
|
for file in ['scene/cup_workcell.json','data/cup_model_20260928/cup_model.json']:
|
|
path=ROOT/file;shutil.copy2(path,backup/path.name);d=json.loads(path.read_text());target=d.get('cup',d);target['position_floor_m']=new;target['position_capture']=str(p);path.write_text(json.dumps(d,indent=2))
|
|
(p/'position_update.json').write_text(json.dumps(report,indent=2));print(json.dumps(report,indent=2))
|