Files

35 lines
1.3 KiB
Python

"""Prepare a visually selected ring surface point using current calibration; no motion."""
import json
from pathlib import Path
import cv2
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
def main():
cap = ROOT / 'data/left_ring_current'
cal = json.loads((ROOT / 'data/real_calib/camera.json').read_text())
meta = json.loads((cap / 'camera.json').read_text())
for key in ('T_world_cam_opencv', 'cam_pos_world', 'extrinsic_status'):
meta[key] = cal[key]
meta['calibration_source'] = 'data/real_calib/camera.json'
# Surface of the black ring, selected after reviewing this capture.
u, v = 330, 224
depth = cv2.imread(str(cap / 'depth.png'), cv2.IMREAD_UNCHANGED)
patch = depth[v-3:v+4, u-3:u+4]
valid = patch[(patch > 200) & (patch < 1000)]
if valid.size < 25:
raise RuntimeError('Insufficient ring surface depth')
z = float(np.median(valid)) / 1000
T = np.asarray(meta['T_world_cam_opencv'])
p = T[:3, :3] @ (np.linalg.inv(meta['K']) @ [u, v, 1] * z) + T[:3, 3]
meta['objects'] = {'ring': {'uv': [u, v], 'surface_point_base': p.tolist(),
'selection': 'visual surface selection, not object center'}}
(cap / 'camera.json').write_text(json.dumps(meta, indent=2))
print('ring surface base (m):', p.tolist())
if __name__ == '__main__':
main()