35 lines
1.3 KiB
Python
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()
|