From 7625d0bdcaa7ab18abc952bd55a802eb074d8204 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Wed, 4 Jan 2023 01:36:50 -0800 Subject: [PATCH 01/62] start generic stage interface --- parallax/camera.py | 1 - parallax/model.py | 7 ++----- parallax/scan_stage_dialog.py | 7 ------- parallax/stage.py | 17 +++++++++++++++-- 4 files changed, 17 insertions(+), 15 deletions(-) delete mode 100644 parallax/scan_stage_dialog.py diff --git a/parallax/camera.py b/parallax/camera.py index 0f68ddaa..6ae336f9 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -12,7 +12,6 @@ def list_cameras(): - global pyspin_cameras, pyspin_instance cameras = [] if PySpin is not None: cameras.extend(PySpinCamera.list_cameras()) diff --git a/parallax/model.py b/parallax/model.py index 86ef33fd..4e881662 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -17,7 +17,8 @@ def __init__(self): QObject.__init__(self) self.cameras = [] - self.init_stages() + self.stages = {} + self.cal_stage = None self.calibration = None self.lcorr, self.rcorr = False, False @@ -72,10 +73,6 @@ def register_corr_points_cal(self): else: self.msg_posted.emit('Highlight correspondence points and press C to continue') - def init_stages(self): - self.stages = {} - self.cal_stage = None - def scan_for_cameras(self): self.cameras = list_cameras() diff --git a/parallax/scan_stage_dialog.py b/parallax/scan_stage_dialog.py deleted file mode 100644 index 05005348..00000000 --- a/parallax/scan_stage_dialog.py +++ /dev/null @@ -1,7 +0,0 @@ -from PyQt5.QtWidgets import QWidget, QFileDialog, QGridLayout, QHBoxLayout, QLabel -from PyQt5.QtWidgets import QRadioButton, QDialog, QCheckBox, QLineEdit, QDialogButtonBox -from PyQt5.QtCore import Qt, QObject -from PyQt5.QtCore import QObject, QThread, pyqtSignal, Qt -from PyQt5.QtGui import QDoubleValidator, QIcon -import time, datetime, os, sys - diff --git a/parallax/stage.py b/parallax/stage.py index da639d39..6b47ff0c 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -1,10 +1,23 @@ from newscale.multistage import USBXYZStage -class Stage(): +def list_stages(): + stages = [] + # todo + return stages - def __init__(self, ip=None, serial=None): + +class NewScaleStage: + + stages = None + + @classmethod + def scan_for_stages(cls): + # todo + + def __init__(self, ip=None, serial=None): + super().__init__() if ip is not None: self.ip = ip self.name = ip From 0cca383934ee3bc64b1e64da8d3151800c125ca7 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 21:34:30 -0800 Subject: [PATCH 02/62] mock stage initial function --- parallax/control_panel.py | 4 +- parallax/main_window.py | 2 +- parallax/model.py | 20 ++----- parallax/stage.py | 117 +++++++++++++++++++++++++++++++++++-- parallax/stage_dropdown.py | 9 ++- parallax/stage_manager.py | 4 +- 6 files changed, 130 insertions(+), 26 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index e99a8b09..d080dde1 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -115,8 +115,8 @@ def update_relative_origin(self): self.zero_button.setText('Set Relative Origin: (%d %d %d)' % (x, y, z)) def handle_stage_selection(self, index): - stage_name = self.dropdown.currentText() - self.set_stage(self.model.stages[stage_name]) + stage = self.dropdown.current_stage() + self.set_stage(stage) self.update_coordinates() def set_stage(self, stage): diff --git a/parallax/main_window.py b/parallax/main_window.py index 8ea747ba..84c015a3 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -66,7 +66,7 @@ def __init__(self, model): self.console = None self.refresh_cameras() - self.model.scan_for_usb_stages() + self.model.scan_for_stages() self.refresh_focus_controllers() def launch_stage_manager(self): diff --git a/parallax/model.py b/parallax/model.py index 8c60ef9d..46a4bdfc 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -4,10 +4,8 @@ import serial.tools.list_ports from mis_focus_controller import FocusController -from newscale.interfaces import NewScaleSerial - from .camera import list_cameras, close_cameras -from .stage import Stage +from .stage import list_stages, close_stages class Model(QObject): @@ -18,8 +16,7 @@ def __init__(self): self.cameras = [] self.focos = [] - self.stages = {} - self.cal_stage = None + self.stages = [] self.calibration = None self.calibrations = {} @@ -58,12 +55,8 @@ def clear_rcorr(self): def scan_for_cameras(self): self.cameras = list_cameras() - def scan_for_usb_stages(self): - instances = NewScaleSerial.get_instances() - self.init_stages() - for instance in instances: - stage = Stage(serial=instance) - self.add_stage(stage) + def scan_for_stages(self): + self.stages = list_stages() def scan_for_focus_controllers(self): self.focos = [] @@ -80,10 +73,7 @@ def add_stage(self, stage): def clean(self): close_cameras() - self.clean_stages() - - def clean_stages(self): - pass + close_stages() def halt_all_stages(self): for stage in self.stages.values(): diff --git a/parallax/stage.py b/parallax/stage.py index fe097988..cdebd0ee 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -1,21 +1,40 @@ +import time, queue, threading +import numpy as np from newscale.multistage import USBXYZStage, PoEXYZStage -from newscale.interfaces import USBInterface +from newscale.interfaces import USBInterface, NewScaleSerial def list_stages(): stages = [] - # todo + stages.extend(NewScaleStage.scan_for_stages()) + if len(stages) == 0: + stages.extend([MockStage(), MockStage()]) return stages +def close_stages(): + NewScaleStage.close_stages() + MockStage.close_stages() + class NewScaleStage: - stages = None + stages = {} @classmethod def scan_for_stages(cls): - # todo + instances = NewScaleSerial.get_instances() + stages = [] + for serial in instances: + if serial not in cls.stages: + cls.stages[serial] = NewScaleStage(serial=serial) + stages.append(cls.stages[serial]) + return stages + + @classmethod + def close_stages(cls): + for stage in cls.stages.values(): + stage.close() def __init__(self, ip=None, serial=None): super().__init__() @@ -30,6 +49,9 @@ def __init__(self, ip=None, serial=None): self.initialize() + def close(self): + pass + def calibrate_frequency(self): self.device.calibrate_all() @@ -98,3 +120,90 @@ def get_accel(self): def halt(self): pass + + +class MockStage: + + n_mock_stages = 0 + + def __init__(self): + self.speed = 1000 # um/s + self.accel = 5000 # um/s^2 + self.pos = np.array([0, 0, 0]) + self.name = f"mock_stage_{MockStage.n_mock_stages}" + MockStage.n_mock_stages += 1 + + self.move_queue = queue.Queue() + self.move_thread = threading.Thread(target=self.thread_loop, daemon=True) + self.move_thread.start() + + def get_origin(self): + return [0, 0, 0] + + @classmethod + def close_stages(self): + pass + + def get_name(self): + return self.name + + def get_speed(self): + return self.speed + + def set_speed(self, speed): + self.speed = speed + + def get_accel(self): + return self.accel + + def get_position(self): + return self.pos.copy() + + def move_to_target_3d(self, x, y, z): + move_cmd = {'pos': np.array([x, y, z]), 'speed': self.speed, 'accel': self.accel, 'finished': False, 'interrupted': False} + self.move_queue.put(move_cmd) + + def move_distance_1d(self, axis, distance): + ax_ind = 'xyz'.index(axis) + pos = self.get_position() + pos[ax_ind] += distance + return self.move_to_target_3d(*pos) + + def thread_loop(self): + current_move = None + while True: + try: + next_move = self.move_queue.get(block=False) + except queue.Empty: + next_move = None + + if next_move is not None: + if current_move is not None: + current_move['interrupted'] = True + current_move['finished'] = True + current_move = next_move + last_update = time.perf_counter() + + if current_move is not None: + now = time.perf_counter() + dt = now = last_update + last_update = now + + pos = self.get_position() + target = current_move['pos'] + dx = target - pos + dist_to_go = np.linalg.norm(dx) + max_dist_per_step = current_move['speed'] * dt + if dist_to_go > max_dist_per_step: + # take a step + direction = dx / dist_to_go + dist_this_step = min(dist_to_go, max_dist_per_step) + step = direction * dist_this_step + self.pos = pos + step + else: + self.pos = target.copy() + current_move['interrupted'] = False + current_move['finished'] = True + current_move = None + + time.sleep(10e-3) diff --git a/parallax/stage_dropdown.py b/parallax/stage_dropdown.py index b6a7518d..06de23ea 100644 --- a/parallax/stage_dropdown.py +++ b/parallax/stage_dropdown.py @@ -7,6 +7,7 @@ class StageDropdown(QComboBox): def __init__(self, model): QComboBox.__init__(self) self.model = model + self.stages = [] self.selected = False self.setFocusPolicy(Qt.NoFocus) @@ -18,13 +19,17 @@ def set_selected(self): def is_selected(self): return self.selected + def current_stage(self): + return self.stages[self.currentIndex()] + def showPopup(self): self.populate() QComboBox.showPopup(self) def populate(self): self.clear() - for ip in self.model.stages.keys(): - self.addItem(ip) + self.stages = self.model.stages[:] + for stage in self.stages: + self.addItem(stage.get_name()) diff --git a/parallax/stage_manager.py b/parallax/stage_manager.py index 3de771e7..23c60217 100755 --- a/parallax/stage_manager.py +++ b/parallax/stage_manager.py @@ -7,7 +7,7 @@ from serial.tools.list_ports import comports as list_comports from serial.serialutil import SerialException -from .stage import Stage +from .stage import NewScaleStage from .helper import PORT_NEWSCALE @@ -69,7 +69,7 @@ def run(self): else: print('ip = ', ip) s.close() - self.stages.append((ip, Stage(ip=ip))) + self.stages.append((ip, NewScaleStage(ip=ip))) self.progress_made.emit(i) self.finished.emit() From ecbcd952043b04a18a762ce20eb36f9754f6448c Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 03:02:31 -0800 Subject: [PATCH 03/62] add mock camera prototype --- mock_camera.py | 366 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 366 insertions(+) create mode 100644 mock_camera.py diff --git a/mock_camera.py b/mock_camera.py new file mode 100644 index 00000000..94fbe961 --- /dev/null +++ b/mock_camera.py @@ -0,0 +1,366 @@ +import pyqtgraph as pg +import numpy as np +import coorx +from parallax.calibration import CameraTransform, StereoCameraTransform + + +class PerspectiveTransform(coorx.BaseTransform): + """3D perspective or orthographic matrix transform using homogeneous coordinates. + + Assumes a camera at the origin, looking toward the -Z axis. + The camera's top points toward +Y, and right points toward +X. + + Points inside the perspective frustum are mapped to the range [-1, +1] along all three axes. + """ + def __init__(self): + super().__init__(dims=(3, 3)) + self.affine = coorx.AffineTransform(dims=(4, 4)) + + def _map(self, arr): + arr4 = np.empty((arr.shape[0], 4), dtype=arr.dtype) + arr4[:, :3] = arr + arr4[:, 3] = 1 + out = self.affine._map(arr4) + return out[:, :3] / out[:, 3:4] + + def set_ortho(self, left, right, bottom, top, znear, zfar): + """Set orthographic transform. + """ + assert(right != left) + assert(bottom != top) + assert(znear != zfar) + + M = np.zeros((4, 4), dtype=np.float32) + M[0, 0] = +2.0 / (right - left) + M[3, 0] = -(right + left) / float(right - left) + M[1, 1] = +2.0 / (top - bottom) + M[3, 1] = -(top + bottom) / float(top - bottom) + M[2, 2] = -2.0 / (zfar - znear) + M[3, 2] = -(zfar + znear) / float(zfar - znear) + M[3, 3] = 1.0 + self.affine.matrix = M.T + + def set_perspective(self, fovy, aspect, znear, zfar): + """Set the perspective + + Parameters + ---------- + fov : float + Field of view. + aspect : float + Aspect ratio. + near : float + Near location. + far : float + Far location. + """ + assert(znear != zfar) + h = np.tan(fovy * np.pi / 360.0) * znear + w = h * aspect + self.set_frustum(-w, w, -h, h, znear, zfar) + + def set_frustum(self, left, right, bottom, top, near, far): # noqa + """Set the frustum + """ + M = frustum(left, right, bottom, top, near, far) + self.affine.matrix = M.T + + +def frustum(left, right, bottom, top, znear, zfar): + """Create view frustum + + Parameters + ---------- + left : float + Left coordinate of the field of view. + right : float + Right coordinate of the field of view. + bottom : float + Bottom coordinate of the field of view. + top : float + Top coordinate of the field of view. + znear : float + Near coordinate of the field of view. + zfar : float + Far coordinate of the field of view. + + Returns + ------- + M : ndarray + View frustum matrix (4x4). + """ + assert(right != left) + assert(bottom != top) + assert(znear != zfar) + + M = np.zeros((4, 4)) + M[0, 0] = +2.0 * znear / float(right - left) + M[2, 0] = (right + left) / float(right - left) + M[1, 1] = +2.0 * znear / float(top - bottom) + M[2, 1] = (top + bottom) / float(top - bottom) + M[2, 2] = -(zfar + znear) / float(zfar - znear) + M[3, 2] = -2.0 * znear * zfar / float(zfar - znear) + M[2, 3] = -1.0 + return M + + +class CameraTransform(coorx.CompositeTransform): + def __init__(self): + self.view = coorx.SRT3DTransform() + self.proj = PerspectiveTransform() + self.screen = coorx.STTransform(dims=(3, 3)) + super().__init__([self.view, self.proj, self.screen]) + + def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1)): + center = np.asarray(center) + look = np.asarray(look) + up = np.asarray(up) + + aspect_ratio = screen_size[0] / screen_size[1] + look_dist = np.linalg.norm(look - center) + forward = look - center + forward /= np.linalg.norm(forward) + right = np.cross(forward, up) + right /= np.linalg.norm(right) + up = np.cross(right, forward) + up /= np.linalg.norm(up) + + pts1 = np.array([center, center + forward, center + right, center + up]) + pts2 = np.array([[0, 0, 0], [0, 0, -1], [1, 0, 0], [0, 1, 0]]) + self._view_mapping_err = self.view.set_mapping(pts1, pts2) + + self.proj.set_perspective(fov, aspect_ratio, look_dist / 100, look_dist * 100) + + self.screen.set_mapping( + [[-1, -1, -1], [1, 1, 1]], + [[0, screen_size[1], 0], [screen_size[0], 0, 1]] + ) + + + +class GraphicsItem: + def __init__(self, view): + self.view = view + self.full_transform = coorx.CompositeTransform([]) + self.transform = coorx.SRT3DTransform() + self.items = [] + self.scene = view.scene() + self.full_transform.add_change_callback(self.transform_changed) + + @property + def transform(self): + return self._transform + + @transform.setter + def transform(self, tr): + self._transform = tr + self.full_transform.transforms = [tr, self.view.camera_tr] + + def render(self): + self.clear_graphics_items() + for item in self.items: + pts = None + if 'points' in item: + pts = self.full_transform.map(item['points']) + pts_xy = pts[:, :2] + pts_z = pts[:, 2] + + if item['type'] == 'poly': + polygon = pg.QtGui.QPolygonF([pg.QtCore.QPointF(*pt) for pt in pts_xy]) + polygon_item = pg.QtWidgets.QGraphicsPolygonItem(polygon) + item['graphicsItem'] = polygon_item + elif item['type'] == 'line': + line_item = pg.QtWidgets.QGraphicsLineItem(*pts_xy.flatten()) + item['graphicsItem'] = line_item + elif item['type'] == 'plot': + line_item = pg.PlotCurveItem(x=pts_xy[:,0], y=pts_xy[:,1]) + item['graphicsItem'] = line_item + else: + raise TypeError(item['type']) + + if 'pen' in item: + pen = pg.mkPen(item['pen']) + item['graphicsItem'].setPen(pen) + + if 'brush' in item: + brush = pg.mkBrush(item['brush']) + item['graphicsItem'].setBrush(brush) + + item['graphicsItem'].setZValue(-pts_z.mean()) + + self.scene.addItem(item['graphicsItem']) + + def clear_graphics_items(self): + for item in self.items: + gfxitem = item.pop('graphicsItem', None) + if gfxitem is not None: + self.scene.removeItem(gfxitem) + + def add_items(self, items): + self.items.extend(items) + self.render() + + def transform_changed(self, event): + self.render() + + +class CheckerBoard(GraphicsItem): + def __init__(self, view, size, colors): + super().__init__(view) + + for i in range(size): + for j in range(size): + self.items.append({ + 'type': 'poly', + 'points': [[i, j, 0], [i+1, j, 0], [i+1, j+1, 0], [i, j+1, 0], [i, j, 0]], + 'pen': None, + 'brush': colors[(i + j) % 2], + }) + self.render() + + +class Axis(GraphicsItem): + def __init__(self, view): + super().__init__(view) + + self.items = [ + {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': 'r'}, + {'type': 'line', 'points': [[0, 0, 0], [0, 1, 0]], 'pen': 'g'}, + {'type': 'line', 'points': [[0, 0, 0], [0, 0, 1]], 'pen': 'b'}, + ] + self.render() + + +class Electrode(GraphicsItem): + def __init__(self, view): + super().__init__(view) + + self.items = [ + {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ + [0, 0, 0], [1, 0, 1], [1, 0, 100], [-1, 0, 100], [-1, 0, 1], [0, 0, 0] + ]}, + ] + self.render() + + +class GraphicsView3D(pg.GraphicsView): + def __init__(self, **kwds): + self.camera_tr = CameraTransform() + self.press_event = None + self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'dist': 10, 'fov': 45} + super().__init__(**kwds) + self.setRenderHint(pg.QtGui.QPainter.Antialiasing) + self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, dist=10, fov=45) + + def set_camera(self, **kwds): + for k,v in kwds.items(): + assert k in self.camera_params + self.camera_params[k] = v + self.update_camera() + + def update_camera(self): + p = self.camera_params + look = np.asarray(p['look']) + pitch = p['pitch'] * np.pi/180 + hdist = p['dist'] * np.cos(pitch) + yaw = p['yaw'] * np.pi/180 + cam_pos = look + np.array([ + hdist * np.cos(yaw), + hdist * np.sin(yaw), + p['dist'] * np.sin(pitch) + ]) + self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()]) + + def mousePressEvent(self, ev): + self.press_event = ev + self.last_mouse_pos = ev.pos() + ev.accept() + + def mouseMoveEvent(self, ev): + if self.press_event is None: + return + dif = ev.pos() - self.last_mouse_pos + self.last_mouse_pos = ev.pos() + + self.camera_params['pitch'] += dif.y() + self.camera_params['yaw'] -= dif.x() + self.update_camera() + + def mouseReleaseEvent(self, ev): + self.press_event = None + + def resizeEvent(self, ev): + super().resizeEvent(ev) + self.update_camera() + + def get_array(self): + return pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True) + + +def find_checker_corners(img, board_shape, show=False): + """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html + """ + import cv2 as cv + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + objp = np.zeros((board_shape[0] * board_shape[1], 3)) + objp[:, :2] = np.mgrid[0:board_shape[0], 0:board_shape[1]].T.reshape(-1, 2) + + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + + # Find the chess board corners + ret, corners = cv.findChessboardCorners(gray, board_shape, None) + if not ret: + print(ret, corners) + return None + + # If found, add object points, image points (after refining them) + imgp = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) + + if show: + v = pg.image(img) + plt = pg.ScatterPlotItem(x=imgp[:, 0, 1], y=imgp[:, 0, 0]) + v.view.addItem(plt) + + return objp, imgp + + + +if __name__ == '__main__': + # pg.dbg() + + screen_size = (800, 600) + + app = pg.mkQApp() + win = GraphicsView3D() + win.setBackground(pg.mkColor(128, 128, 128)) + win.resize(*screen_size) + win.show() + + checkers = CheckerBoard(win, size=5, colors=[0.1, 0.9]) + checkers.transform.set_params(offset=[-2.5, -2.5, -1]) + axis = Axis(win) + + # s = 0.1 + # electrodes = [] + # for i in range(4): + # e = Electrode(win) + # electrodes.append(e) + # e.transform.scale([s, s, s]) + # e.transform.translate([0, 0, 5]) + # e.transform.rotate(45, [1, 0, 0]) + # e.transform.rotate(15 * i, [0, 0, 1]) + + + tr = coorx.AffineTransform(dims=(3, 3)) + tr.translate([-2.5, -2.5, 0]) + tr.scale(0.5) + tr.rotate(30, [1, 0, 0]) + tr.rotate(45, [0, 0, 1]) + + # checkers.transform.set_from_affine(tr) + + + # checkers2 = CheckerBoard(win, size=5, colors=[(0, 0, 0, 40), (255, 255, 255, 40)]) + # checkers2.transform = tr From 1bc6fa1ab1e68a7f2607fba698fdd5d0bedceb3b Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 03:02:50 -0800 Subject: [PATCH 04/62] add radial distortion --- mock_camera.py | 98 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 83 insertions(+), 15 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index 94fbe961..a0517c5c 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -1,4 +1,5 @@ import pyqtgraph as pg +import cv2 as cv import numpy as np import coorx from parallax.calibration import CameraTransform, StereoCameraTransform @@ -104,14 +105,35 @@ def frustum(left, right, bottom, top, znear, zfar): return M +class RadialDistortionTransform(coorx.BaseTransform): + def __init__(self, k=(0, 0, 0)): + super().__init__(dims=(3, 3)) + self.k = k + + def set_k(self, k): + self.k = k + self._update() + + def _map(self, arr): + r = np.linalg.norm(arr, axis=1) + dist = (1 + self.k[0] * r**2 + self.k[1] * r**4 + self.k[2] * r**6) + out = np.empty_like(arr) + # distort x,y + out[:, :2] = arr[:, :2] * dist[:, None] + # leave other axes unchanged + out[:, 2:] = arr[:, 2:] + return out + + class CameraTransform(coorx.CompositeTransform): def __init__(self): self.view = coorx.SRT3DTransform() self.proj = PerspectiveTransform() + self.dist = RadialDistortionTransform() self.screen = coorx.STTransform(dims=(3, 3)) - super().__init__([self.view, self.proj, self.screen]) + super().__init__([self.view, self.proj, self.dist, self.screen]) - def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1)): + def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0, 0, 0)): center = np.asarray(center) look = np.asarray(look) up = np.asarray(up) @@ -136,6 +158,7 @@ def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1)): [[0, screen_size[1], 0], [screen_size[0], 0, 1]] ) + self.dist.set_k(distortion) class GraphicsItem: @@ -247,7 +270,7 @@ class GraphicsView3D(pg.GraphicsView): def __init__(self, **kwds): self.camera_tr = CameraTransform() self.press_event = None - self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'dist': 10, 'fov': 45} + self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'dist': 10, 'fov': 45, 'distortion': (0, 0, 0)} super().__init__(**kwds) self.setRenderHint(pg.QtGui.QPainter.Antialiasing) self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, dist=10, fov=45) @@ -269,7 +292,7 @@ def update_camera(self): hdist * np.sin(yaw), p['dist'] * np.sin(pitch) ]) - self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()]) + self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()], distortion=p['distortion']) def mousePressEvent(self, ev): self.press_event = ev @@ -289,6 +312,10 @@ def mouseMoveEvent(self, ev): def mouseReleaseEvent(self, ev): self.press_event = None + def wheelEvent(self, event): + self.camera_params['dist'] *= 1.01**event.angleDelta().y() + self.update_camera() + def resizeEvent(self, ev): super().resizeEvent(ev) self.update_camera() @@ -300,11 +327,13 @@ def get_array(self): def find_checker_corners(img, board_shape, show=False): """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html """ - import cv2 as cv + if show: + view = pg.image(img) + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) - objp = np.zeros((board_shape[0] * board_shape[1], 3)) + objp = np.zeros((board_shape[0] * board_shape[1], 3), dtype='float32') objp[:, :2] = np.mgrid[0:board_shape[0], 0:board_shape[1]].T.reshape(-1, 2) gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) @@ -312,20 +341,58 @@ def find_checker_corners(img, board_shape, show=False): # Find the chess board corners ret, corners = cv.findChessboardCorners(gray, board_shape, None) if not ret: - print(ret, corners) - return None + return None, None # If found, add object points, image points (after refining them) imgp = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) if show: - v = pg.image(img) plt = pg.ScatterPlotItem(x=imgp[:, 0, 1], y=imgp[:, 0, 0]) - v.view.addItem(plt) + view.view.addItem(plt) return objp, imgp +def generate_calibration_data(view, size): + imgp = [] + objp = [] + cam = view.camera_params.copy() + for i in range(size*4): + pitch = np.random.uniform(45, 89) + yaw = np.random.uniform(0, 360) + distance = np.random.uniform(5, 15) + view.set_camera(pitch=pitch, yaw=yaw, dist=distance) + op,ip = find_checker_corners(view.get_array(), (4, 4)) + if op is None: + continue + objp.append(op) + imgp.append(ip) + if len(imgp) >= size: + break + app.processEvents() + view.set_camera(**cam) + return objp, imgp + + +def calibrate_camera(view, size=40): + objp, imgp = generate_calibration_data(view, size=size) + ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objp, imgp, (view.width(), view.height()), None, None) + return ret, mtx, dist, rvecs, tvecs + + +def undistort_image(img, mtx, dist): + h, w = img.shape[:2] + new_camera_mtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h)) + + # undistort + mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, new_camera_mtx, (w,h), 5) + dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR) + + # crop the image + x, y, w, h = roi + dst = dst[y:y+h, x:x+w] + return dst + if __name__ == '__main__': # pg.dbg() @@ -338,6 +405,8 @@ def find_checker_corners(img, board_shape, show=False): win.resize(*screen_size) win.show() + win.set_camera(distortion=(-0.03, 0, 0)) + checkers = CheckerBoard(win, size=5, colors=[0.1, 0.9]) checkers.transform.set_params(offset=[-2.5, -2.5, -1]) axis = Axis(win) @@ -359,8 +428,7 @@ def find_checker_corners(img, board_shape, show=False): tr.rotate(30, [1, 0, 0]) tr.rotate(45, [0, 0, 1]) - # checkers.transform.set_from_affine(tr) - - - # checkers2 = CheckerBoard(win, size=5, colors=[(0, 0, 0, 40), (255, 255, 255, 40)]) - # checkers2.transform = tr + def test(): + ret, mtx, dist, rvecs, tvecs = calibrate_camera(win) + print(dist) + pg.image(undistort_image(win.get_array(), mtx, dist)) \ No newline at end of file From c173639eb8d99267d10cf434dcf01c24a980c77e Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 11:08:27 -0800 Subject: [PATCH 05/62] fix distortion --- mock_camera.py | 50 ++++++++++++++++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index a0517c5c..d2f54382 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -151,10 +151,17 @@ def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0 pts2 = np.array([[0, 0, 0], [0, 0, -1], [1, 0, 0], [0, 1, 0]]) self._view_mapping_err = self.view.set_mapping(pts1, pts2) - self.proj.set_perspective(fov, aspect_ratio, look_dist / 100, look_dist * 100) + near = look_dist / 100 + far = look_dist * 100 + + # set perspective with aspect=1 so that distortion is performed on + # isotropic coordinates + self.proj.set_perspective(fov, 1.0, near, far) + + # correct for aspect in the screen transform instead self.screen.set_mapping( - [[-1, -1, -1], [1, 1, 1]], + [[-1, -1 / aspect_ratio, -1], [1, 1 / aspect_ratio, 1]], [[0, screen_size[1], 0], [screen_size[0], 0, 1]] ) @@ -353,29 +360,29 @@ def find_checker_corners(img, board_shape, show=False): return objp, imgp -def generate_calibration_data(view, size): +def generate_calibration_data(view, n_images, cb_size): imgp = [] objp = [] cam = view.camera_params.copy() - for i in range(size*4): + for i in range(n_images*4): pitch = np.random.uniform(45, 89) yaw = np.random.uniform(0, 360) distance = np.random.uniform(5, 15) view.set_camera(pitch=pitch, yaw=yaw, dist=distance) - op,ip = find_checker_corners(view.get_array(), (4, 4)) + op,ip = find_checker_corners(view.get_array(), cb_size) if op is None: continue objp.append(op) imgp.append(ip) - if len(imgp) >= size: + if len(imgp) >= n_images: break app.processEvents() view.set_camera(**cam) return objp, imgp -def calibrate_camera(view, size=40): - objp, imgp = generate_calibration_data(view, size=size) +def calibrate_camera(view, cb_size, n_images=40): + objp, imgp = generate_calibration_data(view, n_images=n_images, cb_size=cb_size) ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objp, imgp, (view.width(), view.height()), None, None) return ret, mtx, dist, rvecs, tvecs @@ -405,10 +412,11 @@ def undistort_image(img, mtx, dist): win.resize(*screen_size) win.show() - win.set_camera(distortion=(-0.03, 0, 0)) + win.set_camera(distortion=(-0.05, 0, 0)) - checkers = CheckerBoard(win, size=5, colors=[0.1, 0.9]) - checkers.transform.set_params(offset=[-2.5, -2.5, -1]) + cb_size = 8 + checkers = CheckerBoard(win, size=cb_size, colors=[0.1, 0.9]) + checkers.transform.set_params(offset=[-cb_size/2, -cb_size/2, 0]) axis = Axis(win) # s = 0.1 @@ -422,13 +430,15 @@ def undistort_image(img, mtx, dist): # e.transform.rotate(15 * i, [0, 0, 1]) - tr = coorx.AffineTransform(dims=(3, 3)) - tr.translate([-2.5, -2.5, 0]) - tr.scale(0.5) - tr.rotate(30, [1, 0, 0]) - tr.rotate(45, [0, 0, 1]) + # tr = coorx.AffineTransform(dims=(3, 3)) + # tr.translate([-2.5, -2.5, 0]) + # tr.scale(0.5) + # tr.rotate(30, [1, 0, 0]) + # tr.rotate(45, [0, 0, 1]) - def test(): - ret, mtx, dist, rvecs, tvecs = calibrate_camera(win) - print(dist) - pg.image(undistort_image(win.get_array(), mtx, dist)) \ No newline at end of file + def test(n_images=10): + ret, mtx, dist, rvecs, tvecs = calibrate_camera(win, n_images=n_images, cb_size=(cb_size-1, cb_size-1)) + # print(f"Distortion coefficients: {dist}") + # print(f"Intrinsic matrix: {mtx}") + pg.image(undistort_image(win.get_array(), mtx, dist)) + return mtx, dist From 4900969c8d9b64a0a22afe6bd28c5fc6ffcbdd08 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 12:08:24 -0800 Subject: [PATCH 06/62] add stereo rendering --- mock_camera.py | 111 ++++++++++++++++++++++++++++++------------------- 1 file changed, 69 insertions(+), 42 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index d2f54382..338cd22e 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -169,13 +169,10 @@ def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0 class GraphicsItem: - def __init__(self, view): - self.view = view - self.full_transform = coorx.CompositeTransform([]) - self.transform = coorx.SRT3DTransform() + def __init__(self, views): + self._transform = coorx.SRT3DTransform() self.items = [] - self.scene = view.scene() - self.full_transform.add_change_callback(self.transform_changed) + self.views = [GraphicsItemView(self, view) for view in views] @property def transform(self): @@ -184,11 +181,36 @@ def transform(self): @transform.setter def transform(self, tr): self._transform = tr - self.full_transform.transforms = [tr, self.view.camera_tr] + for view in self.views: + view.update_transform() + + def add_items(self, items): + self.items.extend(items) + self.render() + + def render(self): + for view in self.views: + view.render() + + +class GraphicsItemView: + def __init__(self, item, view): + self.item = item + self.view = view + self.full_transform = coorx.CompositeTransform([]) + self.update_transform() + self.scene = view.scene() + self.full_transform.add_change_callback(self.transform_changed) + + def update_transform(self): + self.full_transform.transforms = [self.item.transform, self.view.camera_tr] + + def transform_changed(self, event): + self.render() def render(self): self.clear_graphics_items() - for item in self.items: + for item in self.item.items: pts = None if 'points' in item: pts = self.full_transform.map(item['points']) @@ -197,46 +219,38 @@ def render(self): if item['type'] == 'poly': polygon = pg.QtGui.QPolygonF([pg.QtCore.QPointF(*pt) for pt in pts_xy]) - polygon_item = pg.QtWidgets.QGraphicsPolygonItem(polygon) - item['graphicsItem'] = polygon_item + gfx_item = pg.QtWidgets.QGraphicsPolygonItem(polygon) elif item['type'] == 'line': - line_item = pg.QtWidgets.QGraphicsLineItem(*pts_xy.flatten()) - item['graphicsItem'] = line_item + gfx_item = pg.QtWidgets.QGraphicsLineItem(*pts_xy.flatten()) elif item['type'] == 'plot': - line_item = pg.PlotCurveItem(x=pts_xy[:,0], y=pts_xy[:,1]) - item['graphicsItem'] = line_item + gfx_item = pg.PlotCurveItem(x=pts_xy[:,0], y=pts_xy[:,1]) else: raise TypeError(item['type']) if 'pen' in item: pen = pg.mkPen(item['pen']) - item['graphicsItem'].setPen(pen) + gfx_item.setPen(pen) if 'brush' in item: brush = pg.mkBrush(item['brush']) - item['graphicsItem'].setBrush(brush) - - item['graphicsItem'].setZValue(-pts_z.mean()) + gfx_item.setBrush(brush) - self.scene.addItem(item['graphicsItem']) + gfx_item.setZValue(-pts_z.mean()) + item.setdefault('graphicsItems', {}) + item['graphicsItems'][self] = gfx_item + self.scene.addItem(gfx_item) def clear_graphics_items(self): - for item in self.items: - gfxitem = item.pop('graphicsItem', None) + for item in self.item.items: + gfxitems = item.get('graphicsItems', {}) + gfxitem = gfxitems.pop(self, None) if gfxitem is not None: self.scene.removeItem(gfxitem) - def add_items(self, items): - self.items.extend(items) - self.render() - - def transform_changed(self, event): - self.render() - class CheckerBoard(GraphicsItem): - def __init__(self, view, size, colors): - super().__init__(view) + def __init__(self, views, size, colors): + super().__init__(views) for i in range(size): for j in range(size): @@ -250,8 +264,8 @@ def __init__(self, view, size, colors): class Axis(GraphicsItem): - def __init__(self, view): - super().__init__(view) + def __init__(self, views): + super().__init__(views) self.items = [ {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': 'r'}, @@ -262,8 +276,8 @@ def __init__(self, view): class Electrode(GraphicsItem): - def __init__(self, view): - super().__init__(view) + def __init__(self, views): + super().__init__(views) self.items = [ {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ @@ -401,23 +415,36 @@ def undistort_image(img, mtx, dist): return dst +class StereoView(pg.QtWidgets.QWidget): + def __init__(self, parent=None, background=(128, 128, 128)): + pg.QtWidgets.QWidget.__init__(self, parent) + self.layout = pg.QtWidgets.QHBoxLayout() + self.setLayout(self.layout) + self.views = [GraphicsView3D(parent=self), GraphicsView3D(parent=self)] + for v in self.views: + self.layout.addWidget(v) + v.setBackground(pg.mkColor(background)) + + def set_camera(self, cam, **kwds): + self.views[cam].set_camera(**kwds) + + + if __name__ == '__main__': # pg.dbg() - screen_size = (800, 600) - app = pg.mkQApp() - win = GraphicsView3D() - win.setBackground(pg.mkColor(128, 128, 128)) - win.resize(*screen_size) + win = StereoView() + win.resize(1600, 600) win.show() - win.set_camera(distortion=(-0.05, 0, 0)) + win.set_camera(0, yaw=-5, distortion=(-0.05, 0, 0)) + win.set_camera(1, yaw=5, distortion=(-0.05, 0, 0)) cb_size = 8 - checkers = CheckerBoard(win, size=cb_size, colors=[0.1, 0.9]) + checkers = CheckerBoard(views=win.views, size=cb_size, colors=[0.1, 0.9]) checkers.transform.set_params(offset=[-cb_size/2, -cb_size/2, 0]) - axis = Axis(win) + axis = Axis(views=win.views) # s = 0.1 # electrodes = [] From 3be68e2432764f80c070b1118ad07859f621f4ee Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 13:25:31 -0800 Subject: [PATCH 07/62] tangential distortion + invert attempt --- mock_camera.py | 114 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 87 insertions(+), 27 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index 338cd22e..13a579fa 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -105,23 +105,50 @@ def frustum(left, right, bottom, top, znear, zfar): return M -class RadialDistortionTransform(coorx.BaseTransform): - def __init__(self, k=(0, 0, 0)): - super().__init__(dims=(3, 3)) - self.k = k +class LensDistortionTransform(coorx.BaseTransform): + """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html + """ + def __init__(self, coeff=(0, 0, 0, 0, 0)): + super().__init__(dims=(2, 2)) + self.coeff = coeff - def set_k(self, k): - self.k = k + def set_coeff(self, coeff): + self.coeff = coeff self._update() def _map(self, arr): + k1, k2, p1, p2, k3 = self.coeff + + # radial distortion r = np.linalg.norm(arr, axis=1) - dist = (1 + self.k[0] * r**2 + self.k[1] * r**4 + self.k[2] * r**6) - out = np.empty_like(arr) - # distort x,y - out[:, :2] = arr[:, :2] * dist[:, None] - # leave other axes unchanged - out[:, 2:] = arr[:, 2:] + dist = (1 + k1 * r**2 + k2 * r**4 + k3 * r**6) + out = arr * dist[:, None] + + # tangential distortion + x = out[:, 0] + y = out[:, 1] + xy = x * y + r2 = r**2 + out[:, 0] += 2 * p1 * xy + p2 * (r2 + 2 * x**2) + out[:, 1] += 2 * p2 * xy + p1 * (r2 + 2 * y**2) + + return out + + +class AxisSelectionEmbeddedTransform(coorx.BaseTransform): + def __init__(self, axes, transform, dims): + super().__init__(dims=dims) + self.axes = axes + self.subtr = transform + + def _map(self, arr): + out = arr.copy() + out[:, self.axes] = self.subtr.map(arr[:, self.axes]) + return out + + def _imap(self, arr): + out = arr.copy() + out[:, axes] = self.subtr.imap(arr[:, axes]) return out @@ -129,11 +156,12 @@ class CameraTransform(coorx.CompositeTransform): def __init__(self): self.view = coorx.SRT3DTransform() self.proj = PerspectiveTransform() - self.dist = RadialDistortionTransform() + self.dist = LensDistortionTransform() + self.dist_embed = AxisSelectionEmbeddedTransform(axes=[0, 1], transform=self.dist, dims=(3, 3)) self.screen = coorx.STTransform(dims=(3, 3)) - super().__init__([self.view, self.proj, self.dist, self.screen]) + super().__init__([self.view, self.proj, self.dist_embed, self.screen]) - def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0, 0, 0)): + def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0, 0, 0, 0, 0)): center = np.asarray(center) look = np.asarray(look) up = np.asarray(up) @@ -165,7 +193,7 @@ def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0 [[0, screen_size[1], 0], [screen_size[0], 0, 1]] ) - self.dist.set_k(distortion) + self.dist.set_coeff(distortion) class GraphicsItem: @@ -291,10 +319,10 @@ class GraphicsView3D(pg.GraphicsView): def __init__(self, **kwds): self.camera_tr = CameraTransform() self.press_event = None - self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'dist': 10, 'fov': 45, 'distortion': (0, 0, 0)} + self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'distance': 10, 'fov': 45, 'distortion': (0, 0, 0, 0, 0)} super().__init__(**kwds) self.setRenderHint(pg.QtGui.QPainter.Antialiasing) - self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, dist=10, fov=45) + self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, distance=10, fov=45) def set_camera(self, **kwds): for k,v in kwds.items(): @@ -306,12 +334,12 @@ def update_camera(self): p = self.camera_params look = np.asarray(p['look']) pitch = p['pitch'] * np.pi/180 - hdist = p['dist'] * np.cos(pitch) + hdist = p['distance'] * np.cos(pitch) yaw = p['yaw'] * np.pi/180 cam_pos = look + np.array([ hdist * np.cos(yaw), hdist * np.sin(yaw), - p['dist'] * np.sin(pitch) + p['distance'] * np.sin(pitch) ]) self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()], distortion=p['distortion']) @@ -382,7 +410,7 @@ def generate_calibration_data(view, n_images, cb_size): pitch = np.random.uniform(45, 89) yaw = np.random.uniform(0, 360) distance = np.random.uniform(5, 15) - view.set_camera(pitch=pitch, yaw=yaw, dist=distance) + view.set_camera(pitch=pitch, yaw=yaw, distance=distance) op,ip = find_checker_corners(view.get_array(), cb_size) if op is None: continue @@ -401,7 +429,7 @@ def calibrate_camera(view, cb_size, n_images=40): return ret, mtx, dist, rvecs, tvecs -def undistort_image(img, mtx, dist): +def undistort_image(img, mtx, dist, crop=False): h, w = img.shape[:2] new_camera_mtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h)) @@ -410,8 +438,9 @@ def undistort_image(img, mtx, dist): dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR) # crop the image - x, y, w, h = roi - dst = dst[y:y+h, x:x+w] + if crop: + x, y, w, h = roi + dst = dst[y:y+h, x:x+w] return dst @@ -431,15 +460,20 @@ def set_camera(self, cam, **kwds): if __name__ == '__main__': - # pg.dbg() + pg.dbg() app = pg.mkQApp() win = StereoView() win.resize(1600, 600) win.show() - win.set_camera(0, yaw=-5, distortion=(-0.05, 0, 0)) - win.set_camera(1, yaw=5, distortion=(-0.05, 0, 0)) + camera_params = dict( + pitch=30, + distance=15, + distortion=(-0.05, 0, 0, 0, 0), + ) + win.set_camera(0, yaw=-5, **camera_params) + win.set_camera(1, yaw=5, **camera_params) cb_size = 8 checkers = CheckerBoard(views=win.views, size=cb_size, colors=[0.1, 0.9]) @@ -469,3 +503,29 @@ def test(n_images=10): # print(f"Intrinsic matrix: {mtx}") pg.image(undistort_image(win.get_array(), mtx, dist)) return mtx, dist + + + def test2(): + """Can we invert opencv's undistortion? + """ + ret, mtx, dist, rvecs, tvecs = calibrate_camera(win.views[0], n_images=10, cb_size=(cb_size-1, cb_size-1)) + print(mtx) + print(dist) + img = win.views[0].get_array() + uimg = undistort_image(img, mtx, dist) + v1 = pg.image(img) + v2 = pg.image(uimg) + tr = coorx.AffineTransform(matrix=mtx[:2, :2], offset=mtx[:2, 2]) + ltr = LensDistortionTransform(dist[0]) + ttr = coorx.CompositeTransform([tr.inverse, ltr, tr]) + + objp, imgp = find_checker_corners(uimg, board_shape=(cb_size-1, cb_size-1)) + undistorted_pts = imgp[:, 0, :] + + distorted_pts = ttr.map(undistorted_pts) + + s1 = pg.ScatterPlotItem(x=undistorted_pts[:,1], y=undistorted_pts[:,0]) + v2.view.addItem(s1) + + s2 = pg.ScatterPlotItem(x=distorted_pts[:,1], y=distorted_pts[:,0]) + v1.view.addItem(s2) From 17617388124fb3e2fe9711df9c4bf789d3c15d64 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 17:01:02 -0800 Subject: [PATCH 08/62] minor edits --- mock_camera.py | 30 +++++++++++++++++----------- parallax/calibration.py | 44 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+), 12 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index 13a579fa..01e87571 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -354,7 +354,7 @@ def mouseMoveEvent(self, ev): dif = ev.pos() - self.last_mouse_pos self.last_mouse_pos = ev.pos() - self.camera_params['pitch'] += dif.y() + self.camera_params['pitch'] = np.clip(self.camera_params['pitch'] + dif.y(), -90, 90) self.camera_params['yaw'] -= dif.x() self.update_camera() @@ -362,7 +362,7 @@ def mouseReleaseEvent(self, ev): self.press_event = None def wheelEvent(self, event): - self.camera_params['dist'] *= 1.01**event.angleDelta().y() + self.camera_params['distance'] *= 1.01**event.angleDelta().y() self.update_camera() def resizeEvent(self, ev): @@ -470,7 +470,7 @@ def set_camera(self, cam, **kwds): camera_params = dict( pitch=30, distance=15, - distortion=(-0.05, 0, 0, 0, 0), + distortion=(0, 0, 0, 0, 0), ) win.set_camera(0, yaw=-5, **camera_params) win.set_camera(1, yaw=5, **camera_params) @@ -480,15 +480,21 @@ def set_camera(self, cam, **kwds): checkers.transform.set_params(offset=[-cb_size/2, -cb_size/2, 0]) axis = Axis(views=win.views) - # s = 0.1 - # electrodes = [] - # for i in range(4): - # e = Electrode(win) - # electrodes.append(e) - # e.transform.scale([s, s, s]) - # e.transform.translate([0, 0, 5]) - # e.transform.rotate(45, [1, 0, 0]) - # e.transform.rotate(15 * i, [0, 0, 1]) + s = 0.1 + electrodes = [] + for i in range(4): + e = Electrode(win.views) + electrodes.append(e) + # e.transform.set_scale([s, s, s]) + # e.transform.set_offset([-5, 0, 5]) + # e.transform.rotate(45, [1, 0, 0]) + # e.transform.rotate(15 * i, [0, 0, 1]) + + e.transform = coorx.AffineTransform(dims=(3, 3)) + e.transform.scale([s, s, s]) + e.transform.rotate(60, [1, 0, 0]) + e.transform.translate([0, 1, 1]) + e.transform.rotate(15*i, [0, 0, 1]) # tr = coorx.AffineTransform(dims=(3, 3)) diff --git a/parallax/calibration.py b/parallax/calibration.py index 98eb9b9a..aed5f91d 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -70,6 +70,50 @@ def calibrate(self, img_points1, img_points2, obj_points, origin): self.set_origin(origin) + +class CameraTransform(coorx.BaseTransform): + """Maps from camera sensor pixels to undistorted UV. + """ + def __init__(self, mtx=None, dist=None, **kwds): + super().__init__(dims=(2, 2), **kwds) + self.mtx = mtx + self.dist = dist + + def set_coeff(self, mtx, dist): + self.mtx = mtx + self.dist = dist + + def _map(self, pts): + return lib.undistort_image_points(pts, self.mtx, self.dist) + + +class StereoCameraTransform(coorx.BaseTransform): + """Maps from dual camera sensor pixels to 3D object space. + """ + imtx1 = np.array([ + [1.81982227e+04, 0.00000000e+00, 2.59310865e+03], + [0.00000000e+00, 1.89774632e+04, 1.48105977e+03], + [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] + ]) + + imtx2 = np.array([ + [1.55104298e+04, 0.00000000e+00, 1.95422363e+03], + [0.00000000e+00, 1.54250418e+04, 1.64814750e+03], + [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] + ]) + + idist1 = np.array([[ 1.70600649e+00, -9.85797706e+01, 4.53808433e-03, -2.13200143e-02, 1.79088477e+03]]) + idist2 = np.array([[-4.94883798e-01, 1.65465770e+02, -1.61013572e-03, 5.22601960e-03, -8.73875986e+03]]) + + + def __init__(self, **kwds): + super().__init__(dims=(4, 3), **kwds) + self.camera_tr1 = CameraTransform() + self.camera_tr2 = CameraTransform() + self.proj1 = None + self.proj2 = None + + def set_mapping(self, img_points1, img_points2, obj_points): # undistort calibration points img_points1 = lib.undistort_image_points(img_points1, self.imtx1, self.idist1) img_points2 = lib.undistort_image_points(img_points2, self.imtx2, self.idist2) From c661925cc4155e01ca63c2d06414588bccaede92 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 22:21:09 -0800 Subject: [PATCH 09/62] moved mock code into package, mock camera test script is working again --- mock_camera.py | 475 ++-------------------------------------- parallax/calibration.py | 1 + parallax/lib.py | 29 +++ parallax/mock_sim.py | 274 +++++++++++++++++++++++ 4 files changed, 319 insertions(+), 460 deletions(-) create mode 100644 parallax/mock_sim.py diff --git a/mock_camera.py b/mock_camera.py index 01e87571..0c4da34a 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -1,448 +1,7 @@ import pyqtgraph as pg -import cv2 as cv -import numpy as np import coorx -from parallax.calibration import CameraTransform, StereoCameraTransform - - -class PerspectiveTransform(coorx.BaseTransform): - """3D perspective or orthographic matrix transform using homogeneous coordinates. - - Assumes a camera at the origin, looking toward the -Z axis. - The camera's top points toward +Y, and right points toward +X. - - Points inside the perspective frustum are mapped to the range [-1, +1] along all three axes. - """ - def __init__(self): - super().__init__(dims=(3, 3)) - self.affine = coorx.AffineTransform(dims=(4, 4)) - - def _map(self, arr): - arr4 = np.empty((arr.shape[0], 4), dtype=arr.dtype) - arr4[:, :3] = arr - arr4[:, 3] = 1 - out = self.affine._map(arr4) - return out[:, :3] / out[:, 3:4] - - def set_ortho(self, left, right, bottom, top, znear, zfar): - """Set orthographic transform. - """ - assert(right != left) - assert(bottom != top) - assert(znear != zfar) - - M = np.zeros((4, 4), dtype=np.float32) - M[0, 0] = +2.0 / (right - left) - M[3, 0] = -(right + left) / float(right - left) - M[1, 1] = +2.0 / (top - bottom) - M[3, 1] = -(top + bottom) / float(top - bottom) - M[2, 2] = -2.0 / (zfar - znear) - M[3, 2] = -(zfar + znear) / float(zfar - znear) - M[3, 3] = 1.0 - self.affine.matrix = M.T - - def set_perspective(self, fovy, aspect, znear, zfar): - """Set the perspective - - Parameters - ---------- - fov : float - Field of view. - aspect : float - Aspect ratio. - near : float - Near location. - far : float - Far location. - """ - assert(znear != zfar) - h = np.tan(fovy * np.pi / 360.0) * znear - w = h * aspect - self.set_frustum(-w, w, -h, h, znear, zfar) - - def set_frustum(self, left, right, bottom, top, near, far): # noqa - """Set the frustum - """ - M = frustum(left, right, bottom, top, near, far) - self.affine.matrix = M.T - - -def frustum(left, right, bottom, top, znear, zfar): - """Create view frustum - - Parameters - ---------- - left : float - Left coordinate of the field of view. - right : float - Right coordinate of the field of view. - bottom : float - Bottom coordinate of the field of view. - top : float - Top coordinate of the field of view. - znear : float - Near coordinate of the field of view. - zfar : float - Far coordinate of the field of view. - - Returns - ------- - M : ndarray - View frustum matrix (4x4). - """ - assert(right != left) - assert(bottom != top) - assert(znear != zfar) - - M = np.zeros((4, 4)) - M[0, 0] = +2.0 * znear / float(right - left) - M[2, 0] = (right + left) / float(right - left) - M[1, 1] = +2.0 * znear / float(top - bottom) - M[2, 1] = (top + bottom) / float(top - bottom) - M[2, 2] = -(zfar + znear) / float(zfar - znear) - M[3, 2] = -2.0 * znear * zfar / float(zfar - znear) - M[2, 3] = -1.0 - return M - - -class LensDistortionTransform(coorx.BaseTransform): - """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html - """ - def __init__(self, coeff=(0, 0, 0, 0, 0)): - super().__init__(dims=(2, 2)) - self.coeff = coeff - - def set_coeff(self, coeff): - self.coeff = coeff - self._update() - - def _map(self, arr): - k1, k2, p1, p2, k3 = self.coeff - - # radial distortion - r = np.linalg.norm(arr, axis=1) - dist = (1 + k1 * r**2 + k2 * r**4 + k3 * r**6) - out = arr * dist[:, None] - - # tangential distortion - x = out[:, 0] - y = out[:, 1] - xy = x * y - r2 = r**2 - out[:, 0] += 2 * p1 * xy + p2 * (r2 + 2 * x**2) - out[:, 1] += 2 * p2 * xy + p1 * (r2 + 2 * y**2) - - return out - - -class AxisSelectionEmbeddedTransform(coorx.BaseTransform): - def __init__(self, axes, transform, dims): - super().__init__(dims=dims) - self.axes = axes - self.subtr = transform - - def _map(self, arr): - out = arr.copy() - out[:, self.axes] = self.subtr.map(arr[:, self.axes]) - return out - - def _imap(self, arr): - out = arr.copy() - out[:, axes] = self.subtr.imap(arr[:, axes]) - return out - - -class CameraTransform(coorx.CompositeTransform): - def __init__(self): - self.view = coorx.SRT3DTransform() - self.proj = PerspectiveTransform() - self.dist = LensDistortionTransform() - self.dist_embed = AxisSelectionEmbeddedTransform(axes=[0, 1], transform=self.dist, dims=(3, 3)) - self.screen = coorx.STTransform(dims=(3, 3)) - super().__init__([self.view, self.proj, self.dist_embed, self.screen]) - - def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0, 0, 0, 0, 0)): - center = np.asarray(center) - look = np.asarray(look) - up = np.asarray(up) - - aspect_ratio = screen_size[0] / screen_size[1] - look_dist = np.linalg.norm(look - center) - forward = look - center - forward /= np.linalg.norm(forward) - right = np.cross(forward, up) - right /= np.linalg.norm(right) - up = np.cross(right, forward) - up /= np.linalg.norm(up) - - pts1 = np.array([center, center + forward, center + right, center + up]) - pts2 = np.array([[0, 0, 0], [0, 0, -1], [1, 0, 0], [0, 1, 0]]) - self._view_mapping_err = self.view.set_mapping(pts1, pts2) - - - near = look_dist / 100 - far = look_dist * 100 - - # set perspective with aspect=1 so that distortion is performed on - # isotropic coordinates - self.proj.set_perspective(fov, 1.0, near, far) - - # correct for aspect in the screen transform instead - self.screen.set_mapping( - [[-1, -1 / aspect_ratio, -1], [1, 1 / aspect_ratio, 1]], - [[0, screen_size[1], 0], [screen_size[0], 0, 1]] - ) - - self.dist.set_coeff(distortion) - - -class GraphicsItem: - def __init__(self, views): - self._transform = coorx.SRT3DTransform() - self.items = [] - self.views = [GraphicsItemView(self, view) for view in views] - - @property - def transform(self): - return self._transform - - @transform.setter - def transform(self, tr): - self._transform = tr - for view in self.views: - view.update_transform() - - def add_items(self, items): - self.items.extend(items) - self.render() - - def render(self): - for view in self.views: - view.render() - - -class GraphicsItemView: - def __init__(self, item, view): - self.item = item - self.view = view - self.full_transform = coorx.CompositeTransform([]) - self.update_transform() - self.scene = view.scene() - self.full_transform.add_change_callback(self.transform_changed) - - def update_transform(self): - self.full_transform.transforms = [self.item.transform, self.view.camera_tr] - - def transform_changed(self, event): - self.render() - - def render(self): - self.clear_graphics_items() - for item in self.item.items: - pts = None - if 'points' in item: - pts = self.full_transform.map(item['points']) - pts_xy = pts[:, :2] - pts_z = pts[:, 2] - - if item['type'] == 'poly': - polygon = pg.QtGui.QPolygonF([pg.QtCore.QPointF(*pt) for pt in pts_xy]) - gfx_item = pg.QtWidgets.QGraphicsPolygonItem(polygon) - elif item['type'] == 'line': - gfx_item = pg.QtWidgets.QGraphicsLineItem(*pts_xy.flatten()) - elif item['type'] == 'plot': - gfx_item = pg.PlotCurveItem(x=pts_xy[:,0], y=pts_xy[:,1]) - else: - raise TypeError(item['type']) - - if 'pen' in item: - pen = pg.mkPen(item['pen']) - gfx_item.setPen(pen) - - if 'brush' in item: - brush = pg.mkBrush(item['brush']) - gfx_item.setBrush(brush) - - gfx_item.setZValue(-pts_z.mean()) - item.setdefault('graphicsItems', {}) - item['graphicsItems'][self] = gfx_item - self.scene.addItem(gfx_item) - - def clear_graphics_items(self): - for item in self.item.items: - gfxitems = item.get('graphicsItems', {}) - gfxitem = gfxitems.pop(self, None) - if gfxitem is not None: - self.scene.removeItem(gfxitem) - - -class CheckerBoard(GraphicsItem): - def __init__(self, views, size, colors): - super().__init__(views) - - for i in range(size): - for j in range(size): - self.items.append({ - 'type': 'poly', - 'points': [[i, j, 0], [i+1, j, 0], [i+1, j+1, 0], [i, j+1, 0], [i, j, 0]], - 'pen': None, - 'brush': colors[(i + j) % 2], - }) - self.render() - - -class Axis(GraphicsItem): - def __init__(self, views): - super().__init__(views) - - self.items = [ - {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': 'r'}, - {'type': 'line', 'points': [[0, 0, 0], [0, 1, 0]], 'pen': 'g'}, - {'type': 'line', 'points': [[0, 0, 0], [0, 0, 1]], 'pen': 'b'}, - ] - self.render() - - -class Electrode(GraphicsItem): - def __init__(self, views): - super().__init__(views) - - self.items = [ - {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ - [0, 0, 0], [1, 0, 1], [1, 0, 100], [-1, 0, 100], [-1, 0, 1], [0, 0, 0] - ]}, - ] - self.render() - - -class GraphicsView3D(pg.GraphicsView): - def __init__(self, **kwds): - self.camera_tr = CameraTransform() - self.press_event = None - self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'distance': 10, 'fov': 45, 'distortion': (0, 0, 0, 0, 0)} - super().__init__(**kwds) - self.setRenderHint(pg.QtGui.QPainter.Antialiasing) - self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, distance=10, fov=45) - - def set_camera(self, **kwds): - for k,v in kwds.items(): - assert k in self.camera_params - self.camera_params[k] = v - self.update_camera() - - def update_camera(self): - p = self.camera_params - look = np.asarray(p['look']) - pitch = p['pitch'] * np.pi/180 - hdist = p['distance'] * np.cos(pitch) - yaw = p['yaw'] * np.pi/180 - cam_pos = look + np.array([ - hdist * np.cos(yaw), - hdist * np.sin(yaw), - p['distance'] * np.sin(pitch) - ]) - self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()], distortion=p['distortion']) - - def mousePressEvent(self, ev): - self.press_event = ev - self.last_mouse_pos = ev.pos() - ev.accept() - - def mouseMoveEvent(self, ev): - if self.press_event is None: - return - dif = ev.pos() - self.last_mouse_pos - self.last_mouse_pos = ev.pos() - - self.camera_params['pitch'] = np.clip(self.camera_params['pitch'] + dif.y(), -90, 90) - self.camera_params['yaw'] -= dif.x() - self.update_camera() - - def mouseReleaseEvent(self, ev): - self.press_event = None - - def wheelEvent(self, event): - self.camera_params['distance'] *= 1.01**event.angleDelta().y() - self.update_camera() - - def resizeEvent(self, ev): - super().resizeEvent(ev) - self.update_camera() - - def get_array(self): - return pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True) - - -def find_checker_corners(img, board_shape, show=False): - """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html - """ - if show: - view = pg.image(img) - - criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) - - # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) - objp = np.zeros((board_shape[0] * board_shape[1], 3), dtype='float32') - objp[:, :2] = np.mgrid[0:board_shape[0], 0:board_shape[1]].T.reshape(-1, 2) - - gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) - - # Find the chess board corners - ret, corners = cv.findChessboardCorners(gray, board_shape, None) - if not ret: - return None, None - - # If found, add object points, image points (after refining them) - imgp = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) - - if show: - plt = pg.ScatterPlotItem(x=imgp[:, 0, 1], y=imgp[:, 0, 0]) - view.view.addItem(plt) - - return objp, imgp - - -def generate_calibration_data(view, n_images, cb_size): - imgp = [] - objp = [] - cam = view.camera_params.copy() - for i in range(n_images*4): - pitch = np.random.uniform(45, 89) - yaw = np.random.uniform(0, 360) - distance = np.random.uniform(5, 15) - view.set_camera(pitch=pitch, yaw=yaw, distance=distance) - op,ip = find_checker_corners(view.get_array(), cb_size) - if op is None: - continue - objp.append(op) - imgp.append(ip) - if len(imgp) >= n_images: - break - app.processEvents() - view.set_camera(**cam) - return objp, imgp - - -def calibrate_camera(view, cb_size, n_images=40): - objp, imgp = generate_calibration_data(view, n_images=n_images, cb_size=cb_size) - ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objp, imgp, (view.width(), view.height()), None, None) - return ret, mtx, dist, rvecs, tvecs - - -def undistort_image(img, mtx, dist, crop=False): - h, w = img.shape[:2] - new_camera_mtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h)) - - # undistort - mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, new_camera_mtx, (w,h), 5) - dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR) - - # crop the image - if crop: - x, y, w, h = roi - dst = dst[y:y+h, x:x+w] - return dst - +from parallax.mock_sim import GraphicsView3D, CheckerBoard, Axis, Electrode, calibrate_camera, undistort_image +from parallax.lib import find_checker_corners class StereoView(pg.QtWidgets.QWidget): def __init__(self, parent=None, background=(128, 128, 128)): @@ -470,7 +29,7 @@ def set_camera(self, cam, **kwds): camera_params = dict( pitch=30, distance=15, - distortion=(0, 0, 0, 0, 0), + distortion=(-0.1, 0, 0, 0, 0), ) win.set_camera(0, yaw=-5, **camera_params) win.set_camera(1, yaw=5, **camera_params) @@ -481,20 +40,16 @@ def set_camera(self, cam, **kwds): axis = Axis(views=win.views) s = 0.1 - electrodes = [] - for i in range(4): - e = Electrode(win.views) - electrodes.append(e) - # e.transform.set_scale([s, s, s]) - # e.transform.set_offset([-5, 0, 5]) - # e.transform.rotate(45, [1, 0, 0]) - # e.transform.rotate(15 * i, [0, 0, 1]) + # electrodes = [] + # for i in range(4): + # e = Electrode(win.views) + # electrodes.append(e) - e.transform = coorx.AffineTransform(dims=(3, 3)) - e.transform.scale([s, s, s]) - e.transform.rotate(60, [1, 0, 0]) - e.transform.translate([0, 1, 1]) - e.transform.rotate(15*i, [0, 0, 1]) + # e.transform = coorx.AffineTransform(dims=(3, 3)) + # e.transform.scale([s, s, s]) + # e.transform.rotate(60, [1, 0, 0]) + # e.transform.translate([0, 1, 1]) + # e.transform.rotate(15*i, [0, 0, 1]) # tr = coorx.AffineTransform(dims=(3, 3)) @@ -522,7 +77,7 @@ def test2(): v1 = pg.image(img) v2 = pg.image(uimg) tr = coorx.AffineTransform(matrix=mtx[:2, :2], offset=mtx[:2, 2]) - ltr = LensDistortionTransform(dist[0]) + ltr = coorx.nonlinear.LensDistortionTransform(dist[0]) ttr = coorx.CompositeTransform([tr.inverse, ltr, tr]) objp, imgp = find_checker_corners(uimg, board_shape=(cb_size-1, cb_size-1)) @@ -530,8 +85,8 @@ def test2(): distorted_pts = ttr.map(undistorted_pts) - s1 = pg.ScatterPlotItem(x=undistorted_pts[:,1], y=undistorted_pts[:,0]) + s1 = pg.ScatterPlotItem(x=undistorted_pts[:,1], y=undistorted_pts[:,0], brush='r', pen=None) v2.view.addItem(s1) - s2 = pg.ScatterPlotItem(x=distorted_pts[:,1], y=distorted_pts[:,0]) + s2 = pg.ScatterPlotItem(x=distorted_pts[:,1], y=distorted_pts[:,0], brush='r', pen=None) v1.view.addItem(s2) diff --git a/parallax/calibration.py b/parallax/calibration.py index aed5f91d..a3a42a38 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -2,6 +2,7 @@ import numpy as np import cv2 as cv +import coorx from . import lib from .helper import WF, HF diff --git a/parallax/lib.py b/parallax/lib.py index 9e1d150a..ebdef8b9 100644 --- a/parallax/lib.py +++ b/parallax/lib.py @@ -62,3 +62,32 @@ def DLT(P1, P2, point1, point2): def triangulate_from_image_points(img_point1, img_point2, proj1, proj2): x,y,z = DLT(proj1, proj2, img_point1, img_point2) return np.array([x,y,z], dtype=np.float32) + + +def find_checker_corners(img, board_shape, show=False): + """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html + """ + if show: + view = pg.image(img) + + criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) + objp = np.zeros((board_shape[0] * board_shape[1], 3), dtype='float32') + objp[:, :2] = np.mgrid[0:board_shape[0], 0:board_shape[1]].T.reshape(-1, 2) + + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) + + # Find the chess board corners + ret, corners = cv.findChessboardCorners(gray, board_shape, None) + if not ret: + return None, None + + # If found, add object points, image points (after refining them) + imgp = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) + + if show: + plt = pg.ScatterPlotItem(x=imgp[:, 0, 1], y=imgp[:, 0, 0]) + view.view.addItem(plt) + + return objp, imgp diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py new file mode 100644 index 00000000..84c2cb26 --- /dev/null +++ b/parallax/mock_sim.py @@ -0,0 +1,274 @@ +import pyqtgraph as pg +import cv2 as cv +import numpy as np +import coorx +from parallax.calibration import CameraTransform, StereoCameraTransform +from parallax.lib import find_checker_corners + + +class CameraTransform(coorx.CompositeTransform): + def __init__(self): + self.view = coorx.SRT3DTransform() + self.proj = coorx.linear.PerspectiveTransform() + self.dist = coorx.nonlinear.LensDistortionTransform() + self.dist_embed = coorx.util.AxisSelectionEmbeddedTransform(axes=[0, 1], transform=self.dist, dims=(3, 3)) + self.screen = coorx.STTransform(dims=(3, 3)) + super().__init__([self.view, self.proj, self.dist_embed, self.screen]) + + def set_camera(self, center, look, fov, screen_size, up=(0, 0, 1), distortion=(0, 0, 0, 0, 0)): + center = np.asarray(center) + look = np.asarray(look) + up = np.asarray(up) + + aspect_ratio = screen_size[0] / screen_size[1] + look_dist = np.linalg.norm(look - center) + forward = look - center + forward /= np.linalg.norm(forward) + right = np.cross(forward, up) + right /= np.linalg.norm(right) + up = np.cross(right, forward) + up /= np.linalg.norm(up) + + pts1 = np.array([center, center + forward, center + right, center + up]) + pts2 = np.array([[0, 0, 0], [0, 0, -1], [1, 0, 0], [0, 1, 0]]) + self._view_mapping_err = self.view.set_mapping(pts1, pts2) + + + near = look_dist / 100 + far = look_dist * 100 + + # set perspective with aspect=1 so that distortion is performed on + # isotropic coordinates + self.proj.set_perspective(fov, 1.0, near, far) + + # correct for aspect in the screen transform instead + self.screen.set_mapping( + [[-1, -1 / aspect_ratio, -1], [1, 1 / aspect_ratio, 1]], + [[0, screen_size[1], 0], [screen_size[0], 0, 1]] + ) + + self.dist.set_coeff(distortion) + + +class GraphicsItem: + def __init__(self, views): + self._transform = coorx.SRT3DTransform() + self.items = [] + self.views = [GraphicsItemView(self, view) for view in views] + + @property + def transform(self): + return self._transform + + @transform.setter + def transform(self, tr): + self._transform = tr + for view in self.views: + view.update_transform() + + def add_items(self, items): + self.items.extend(items) + self.render() + + def render(self): + for view in self.views: + view.render() + + +class GraphicsItemView: + def __init__(self, item, view): + self.item = item + self.view = view + self.full_transform = coorx.CompositeTransform([]) + self.update_transform() + self.scene = view.scene() + self.full_transform.add_change_callback(self.transform_changed) + + def update_transform(self): + self.full_transform.transforms = [self.item.transform, self.view.camera_tr] + + def transform_changed(self, event): + self.render() + + def render(self): + self.clear_graphics_items() + for item in self.item.items: + pts = None + if 'points' in item: + pts = self.full_transform.map(item['points']) + pts_xy = pts[:, :2] + pts_z = pts[:, 2] + + if item['type'] == 'poly': + polygon = pg.QtGui.QPolygonF([pg.QtCore.QPointF(*pt) for pt in pts_xy]) + gfx_item = pg.QtWidgets.QGraphicsPolygonItem(polygon) + elif item['type'] == 'line': + gfx_item = pg.QtWidgets.QGraphicsLineItem(*pts_xy.flatten()) + elif item['type'] == 'plot': + gfx_item = pg.PlotCurveItem(x=pts_xy[:,0], y=pts_xy[:,1]) + else: + raise TypeError(item['type']) + + if 'pen' in item: + pen = pg.mkPen(item['pen']) + gfx_item.setPen(pen) + + if 'brush' in item: + brush = pg.mkBrush(item['brush']) + gfx_item.setBrush(brush) + + gfx_item.setZValue(-pts_z.mean()) + item.setdefault('graphicsItems', {}) + item['graphicsItems'][self] = gfx_item + self.scene.addItem(gfx_item) + + def clear_graphics_items(self): + for item in self.item.items: + gfxitems = item.get('graphicsItems', {}) + gfxitem = gfxitems.pop(self, None) + if gfxitem is not None: + self.scene.removeItem(gfxitem) + + +class CheckerBoard(GraphicsItem): + def __init__(self, views, size, colors): + super().__init__(views) + + for i in range(size): + for j in range(size): + self.items.append({ + 'type': 'poly', + 'points': [[i, j, 0], [i+1, j, 0], [i+1, j+1, 0], [i, j+1, 0], [i, j, 0]], + 'pen': None, + 'brush': colors[(i + j) % 2], + }) + self.render() + + +class Axis(GraphicsItem): + def __init__(self, views): + super().__init__(views) + + self.items = [ + {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': 'r'}, + {'type': 'line', 'points': [[0, 0, 0], [0, 1, 0]], 'pen': 'g'}, + {'type': 'line', 'points': [[0, 0, 0], [0, 0, 1]], 'pen': 'b'}, + ] + self.render() + + +class Electrode(GraphicsItem): + def __init__(self, views): + super().__init__(views) + + self.items = [ + {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ + [0, 0, 0], [1, 0, 1], [1, 0, 100], [-1, 0, 100], [-1, 0, 1], [0, 0, 0] + ]}, + ] + self.render() + + +class GraphicsView3D(pg.GraphicsView): + def __init__(self, **kwds): + self.camera_tr = CameraTransform() + self.press_event = None + self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'distance': 10, 'fov': 45, 'distortion': (0, 0, 0, 0, 0)} + super().__init__(**kwds) + self.setRenderHint(pg.QtGui.QPainter.Antialiasing) + self.set_camera(look=[0, 0, 0], pitch=30, yaw=0, distance=10, fov=45) + + def set_camera(self, **kwds): + for k,v in kwds.items(): + assert k in self.camera_params + self.camera_params[k] = v + self.update_camera() + + def update_camera(self): + p = self.camera_params + look = np.asarray(p['look']) + pitch = p['pitch'] * np.pi/180 + hdist = p['distance'] * np.cos(pitch) + yaw = p['yaw'] * np.pi/180 + cam_pos = look + np.array([ + hdist * np.cos(yaw), + hdist * np.sin(yaw), + p['distance'] * np.sin(pitch) + ]) + self.camera_tr.set_camera(center=cam_pos, look=look, fov=p['fov'], screen_size=[self.width(), self.height()], distortion=p['distortion']) + + def mousePressEvent(self, ev): + self.press_event = ev + self.last_mouse_pos = ev.pos() + ev.accept() + + def mouseMoveEvent(self, ev): + if self.press_event is None: + return + dif = ev.pos() - self.last_mouse_pos + self.last_mouse_pos = ev.pos() + + self.camera_params['pitch'] = np.clip(self.camera_params['pitch'] + dif.y(), -90, 90) + self.camera_params['yaw'] -= dif.x() + self.update_camera() + + def mouseReleaseEvent(self, ev): + self.press_event = None + + def wheelEvent(self, event): + self.camera_params['distance'] *= 1.01**event.angleDelta().y() + self.update_camera() + + def resizeEvent(self, ev): + super().resizeEvent(ev) + self.update_camera() + + def get_array(self): + return pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True) + + +def generate_calibration_data(view, n_images, cb_size): + app = pg.QtWidgets.QApplication.instance() + imgp = [] + objp = [] + cam = view.camera_params.copy() + for i in range(n_images*4): + pitch = np.random.uniform(45, 89) + yaw = np.random.uniform(0, 360) + distance = np.random.uniform(5, 15) + view.set_camera(pitch=pitch, yaw=yaw, distance=distance) + op,ip = find_checker_corners(view.get_array(), cb_size) + if op is None: + continue + objp.append(op) + imgp.append(ip) + if len(imgp) >= n_images: + break + app.processEvents() + view.set_camera(**cam) + return objp, imgp + + +def calibrate_camera(view, cb_size, n_images=40): + objp, imgp = generate_calibration_data(view, n_images=n_images, cb_size=cb_size) + ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objp, imgp, (view.width(), view.height()), None, None) + return ret, mtx, dist, rvecs, tvecs + + +def undistort_image(img, mtx, dist, optimize=False, crop=False): + h, w = img.shape[:2] + if optimize: + new_camera_mtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h)) + else: + assert crop is False + new_camera_mtx = mtx + + # undistort + mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, new_camera_mtx, (w,h), 5) + dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR) + + # crop the image + if crop: + x, y, w, h = roi + dst = dst[y:y+h, x:x+w] + return dst From 8e3541909aa011f799dce604f1e0377b75123572 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 9 Feb 2023 23:34:54 -0800 Subject: [PATCH 10/62] mock camera is rendering --- mock_camera.py | 12 ++++----- parallax/camera.py | 21 ++++++++++++--- parallax/mock_sim.py | 61 +++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 80 insertions(+), 14 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index 0c4da34a..50c2ad2b 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -19,7 +19,7 @@ def set_camera(self, cam, **kwds): if __name__ == '__main__': - pg.dbg() + # pg.dbg() app = pg.mkQApp() win = StereoView() @@ -62,7 +62,7 @@ def test(n_images=10): ret, mtx, dist, rvecs, tvecs = calibrate_camera(win, n_images=n_images, cb_size=(cb_size-1, cb_size-1)) # print(f"Distortion coefficients: {dist}") # print(f"Intrinsic matrix: {mtx}") - pg.image(undistort_image(win.get_array(), mtx, dist)) + pg.image(undistort_image(win.get_array().transpose(1, 0, 2), mtx, dist)) return mtx, dist @@ -74,8 +74,8 @@ def test2(): print(dist) img = win.views[0].get_array() uimg = undistort_image(img, mtx, dist) - v1 = pg.image(img) - v2 = pg.image(uimg) + v1 = pg.image(img.transpose(1, 0, 2)) + v2 = pg.image(uimg.transpose(1, 0, 2)) tr = coorx.AffineTransform(matrix=mtx[:2, :2], offset=mtx[:2, 2]) ltr = coorx.nonlinear.LensDistortionTransform(dist[0]) ttr = coorx.CompositeTransform([tr.inverse, ltr, tr]) @@ -85,8 +85,8 @@ def test2(): distorted_pts = ttr.map(undistorted_pts) - s1 = pg.ScatterPlotItem(x=undistorted_pts[:,1], y=undistorted_pts[:,0], brush='r', pen=None) + s1 = pg.ScatterPlotItem(x=undistorted_pts[:,0], y=undistorted_pts[:,1], brush='r', pen=None) v2.view.addItem(s1) - s2 = pg.ScatterPlotItem(x=distorted_pts[:,1], y=distorted_pts[:,0], brush='r', pen=None) + s2 = pg.ScatterPlotItem(x=distorted_pts[:,0], y=distorted_pts[:,1], brush='r', pen=None) v1.view.addItem(s2) diff --git a/parallax/camera.py b/parallax/camera.py index b32fe0a2..0b02bc7b 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -3,6 +3,7 @@ import threading import numpy as np import logging +from .mock_sim import MockSim logger = logging.getLogger(__name__) @@ -162,9 +163,19 @@ class MockCamera: def __init__(self): self._name = f"MockCamera{MockCamera.n_cameras}" MockCamera.n_cameras += 1 - self.data = np.random.randint(0, 255, size=(5, 3000, 4000), dtype='ubyte') + self.noise = np.random.randint(0, 10, size=(5, 3000, 4000, 3), dtype='ubyte') self._next_frame = 0 + self.camera_params = dict( + pitch=30, + distance=15, + distortion=(-0.1, 0, 0, 0, 0), + ) + + + self.sim = MockSim.instance() + self.sim.add_camera(self, size=(4000, 3000)) + def name(self): return self._name @@ -172,6 +183,8 @@ def get_last_image_data(self): """ Return last image as numpy array with shape (height, width, 3) for RGB or (height, width) for mono. """ - frame = self.data[self._next_frame] - self._next_frame = (self._next_frame + 1) % self.data.shape[0] - return frame + noise = self.noise[self._next_frame] + self._next_frame = (self._next_frame + 1) % self.noise.shape[0] + + image = self.sim.get_camera_frame(self) + noise + return image diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 84c2cb26..896f162a 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -54,7 +54,13 @@ class GraphicsItem: def __init__(self, views): self._transform = coorx.SRT3DTransform() self.items = [] - self.views = [GraphicsItemView(self, view) for view in views] + self.item_views = [] + for view in views: + self.add_view(view) + + def add_view(self, view): + self.item_views.append(GraphicsItemView(self, view)) + self.render() @property def transform(self): @@ -63,7 +69,7 @@ def transform(self): @transform.setter def transform(self, tr): self._transform = tr - for view in self.views: + for view in self.item_views: view.update_transform() def add_items(self, items): @@ -71,7 +77,7 @@ def add_items(self, items): self.render() def render(self): - for view in self.views: + for view in self.item_views: view.render() @@ -224,7 +230,8 @@ def resizeEvent(self, ev): self.update_camera() def get_array(self): - return pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True) + arr = pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True, transpose=False)[..., :3] + return arr def generate_calibration_data(view, n_images, cb_size): @@ -272,3 +279,49 @@ def undistort_image(img, mtx, dist, optimize=False, crop=False): x, y, w, h = roi dst = dst[y:y+h, x:x+w] return dst + + +class MockSim: + + _instance = None + + @classmethod + def instance(cls): + if cls._instance is None: + cls._instance = MockSim() + return cls._instance + + def __init__(self): + self.cameras = {} + self.stages = [] + + cb_size = 8 + checkers = CheckerBoard(views=[], size=cb_size, colors=[0.1, 0.9]) + checkers.transform.set_params(offset=[-cb_size/2, -cb_size/2, 0]) + axis = Axis(views=[]) + self.items = [checkers, axis] + + def add_camera(self, cam, size): + view = GraphicsView3D() + view.resize(*size) + view.set_camera(**cam.camera_params) + self.cameras[cam] = {'view': view, 'frame': None} + + for item in self.items: + item.add_view(view) + + def clear_frames(self): + for v in self.cameras.values(): + v['frame'] = None + + def get_camera_frame(self, cam): + if self.cameras[cam]['frame'] is None: + view = self.cameras[cam]['view'] + self.cameras[cam]['frame'] = view.get_array() + return self.cameras[cam]['frame'] + + def add_stage(self, stage): + self.stages.append(stage) + + + From 1f7caed506c051b59ae8fe21613ae337fa6ed3e5 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Fri, 10 Feb 2023 13:16:58 -0800 Subject: [PATCH 11/62] mock sim working --- parallax/camera.py | 23 ++++++++++++-------- parallax/mock_sim.py | 50 +++++++++++++++++++++++++++++++++----------- parallax/stage.py | 39 ++++++++++++++++++++++++++++++++-- 3 files changed, 89 insertions(+), 23 deletions(-) diff --git a/parallax/camera.py b/parallax/camera.py index 0b02bc7b..f9070211 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -19,8 +19,11 @@ def list_cameras(): cameras = [] if PySpin is not None: cameras.extend(PySpinCamera.list_cameras()) - while len(cameras) < 2: - cameras.append(MockCamera()) + else: + cameras.extend([ + MockCamera(camera_params={'pitch': 60, 'yaw': 0}), + MockCamera(camera_params={'pitch': 60, 'yaw': 30}), + ]) return cameras @@ -160,21 +163,23 @@ def capture_loop(self): class MockCamera: n_cameras = 0 - def __init__(self): - self._name = f"MockCamera{MockCamera.n_cameras}" + def __init__(self, camera_params): + self._name = f"mock_camera_{MockCamera.n_cameras}" MockCamera.n_cameras += 1 - self.noise = np.random.randint(0, 10, size=(5, 3000, 4000, 3), dtype='ubyte') + self.sensor_size = (4000, 3000) + self.noise = np.random.randint(0, 15, size=(5, self.sensor_size[1], self.sensor_size[0], 3), dtype='ubyte') self._next_frame = 0 self.camera_params = dict( pitch=30, - distance=15, + yaw=0, + fov=5, + distance=200e3, distortion=(-0.1, 0, 0, 0, 0), ) - - + self.camera_params.update(camera_params) self.sim = MockSim.instance() - self.sim.add_camera(self, size=(4000, 3000)) + self.sim.add_camera(self) def name(self): return self._name diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 896f162a..05882acb 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -166,10 +166,11 @@ def __init__(self, views): class Electrode(GraphicsItem): def __init__(self, views): super().__init__(views) - + w = 70 + l = 10e3 self.items = [ {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ - [0, 0, 0], [1, 0, 1], [1, 0, 100], [-1, 0, 100], [-1, 0, 1], [0, 0, 0] + [0, 0, 0], [w, 0, w], [w, 0, l], [-w, 0, l], [-w, 0, l], [0, 0, 0] ]}, ] self.render() @@ -281,10 +282,12 @@ def undistort_image(img, mtx, dist, optimize=False, crop=False): return dst -class MockSim: +class MockSim(pg.QtCore.QObject): _instance = None + stage_moved = pg.QtCore.Signal(object) + @classmethod def instance(cls): if cls._instance is None: @@ -292,19 +295,24 @@ def instance(cls): return cls._instance def __init__(self): + pg.QtCore.QObject.__init__(self) self.cameras = {} - self.stages = [] + self.stages = {} cb_size = 8 - checkers = CheckerBoard(views=[], size=cb_size, colors=[0.1, 0.9]) - checkers.transform.set_params(offset=[-cb_size/2, -cb_size/2, 0]) + checkers = CheckerBoard(views=[], size=cb_size, colors=[0.3, 0.7]) + s = 1e3 + checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, 0], scale=[s, s, s]) axis = Axis(views=[]) + axis.transform.set_params(scale=[s, s, s]) self.items = [checkers, axis] + self.stage_moved.connect(self.update_stage) - def add_camera(self, cam, size): - view = GraphicsView3D() - view.resize(*size) + def add_camera(self, cam): + view = GraphicsView3D(background=(128, 128, 128)) + view.resize(*cam.sensor_size) view.set_camera(**cam.camera_params) + view.scene().changed.connect(self.clear_frames) self.cameras[cam] = {'view': view, 'frame': None} for item in self.items: @@ -321,7 +329,25 @@ def get_camera_frame(self, cam): return self.cameras[cam]['frame'] def add_stage(self, stage): - self.stages.append(stage) + views = [c['view'] for c in self.cameras.values()] + item = Electrode(views=views) + + tr = coorx.AffineTransform(dims=(3, 3)) + theta, phi = stage.orientation() + # s = 1e3 + # tr.scale([s, s, s]) + tr.rotate(phi, [1, 0, 0]) + tr.rotate(theta, [0, 0, 1]) + item.transform = tr + + self.stages[stage] = {'item': item} + stage.add_move_callback(self._stage_moved_cb) - - + def _stage_moved_cb(self, stage): + # callback is invoked in thread; send to gui thread by signal + self.stage_moved.emit(stage) + + def update_stage(self, stage): + pos = stage.get_tip_position() + item = self.stages[stage]['item'] + item.transform.offset = pos diff --git a/parallax/stage.py b/parallax/stage.py index cdebd0ee..0478fd90 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -1,14 +1,25 @@ import time, queue, threading import numpy as np +import coorx from newscale.multistage import USBXYZStage, PoEXYZStage from newscale.interfaces import USBInterface, NewScaleSerial +from .mock_sim import MockSim def list_stages(): stages = [] stages.extend(NewScaleStage.scan_for_stages()) if len(stages) == 0: - stages.extend([MockStage(), MockStage()]) + tr1 = coorx.AffineTransform(dims=(3, 3)) + tr1.rotate(130, (1, 0, 0)) + tr1.rotate(30, (0, 0, 1)) + tr2 = tr1.copy() + tr2.rotate(60, (0, 0, 1)) + + stages.extend([ + MockStage(transform=tr1), + MockStage(transform=tr2), + ]) return stages @@ -126,17 +137,23 @@ class MockStage: n_mock_stages = 0 - def __init__(self): + def __init__(self, transform): + self.transform = transform self.speed = 1000 # um/s self.accel = 5000 # um/s^2 self.pos = np.array([0, 0, 0]) self.name = f"mock_stage_{MockStage.n_mock_stages}" MockStage.n_mock_stages += 1 + self.move_callbacks = [] + self.move_queue = queue.Queue() self.move_thread = threading.Thread(target=self.thread_loop, daemon=True) self.move_thread.start() + self.sim = MockSim.instance() + self.sim.add_stage(self) + def get_origin(self): return [0, 0, 0] @@ -205,5 +222,23 @@ def thread_loop(self): current_move['interrupted'] = False current_move['finished'] = True current_move = None + + for cb in self.move_callbacks[:]: + cb(self) time.sleep(10e-3) + + def add_move_callback(self, cb): + self.move_callbacks.append(cb) + + def orientation(self): + p1 = self.transform.map([0, 0, 0]) + p2 = self.transform.map([0, 0, 1]) + axis = p1 - p2 + r = np.linalg.norm(axis[:2]) + phi = np.arctan2(r, axis[2]) * 180 / np.pi + theta = np.arctan2(axis[1], axis[0]) * 180 / np.pi + return theta, phi + + def get_tip_position(self): + return self.transform.map(self.get_position()) From f14092a674e1779358b17488aa55d9c5494a2451 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 11:44:14 -0800 Subject: [PATCH 12/62] Update .gitignore --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index fa98d919..06636ba9 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,8 @@ parallax/*.log *.npy *.pkl *.csv +*.json +.idea +*.egg-info +*-workspace +*.log \ No newline at end of file From 3eae984a0ee409d5fa375d3db73b648832bf95e8 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 12:12:40 -0800 Subject: [PATCH 13/62] Don't change CWD if possible -- this is global --- parallax/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/__init__.py b/parallax/__init__.py index 038da10e..9d5bb0d9 100644 --- a/parallax/__init__.py +++ b/parallax/__init__.py @@ -7,5 +7,5 @@ # allow multiple OpenMP instances os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' -# change workdir to package root -os.chdir(os.path.dirname(os.path.realpath(__file__))) +# # change workdir to package root +# os.chdir(os.path.dirname(os.path.realpath(__file__))) From 903a987b297fa833a7134950f692c13dd90ec603 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 12:18:33 -0800 Subject: [PATCH 14/62] Add config system with calibration path + mock sim options --- parallax/config.py | 39 ++++++++++++++++++++++++++++++++++++++ parallax/geometry_panel.py | 6 +++--- parallax/mock_sim.py | 21 +++++++++++++------- run-parallax.py | 6 +++++- 4 files changed, 61 insertions(+), 11 deletions(-) create mode 100644 parallax/config.py diff --git a/parallax/config.py b/parallax/config.py new file mode 100644 index 00000000..9c555a7c --- /dev/null +++ b/parallax/config.py @@ -0,0 +1,39 @@ +import json, argparse + +# global configuration +config = { + 'views': [{}, {}], + 'cameras': [], + 'stages': [], + 'mock_sim': { + 'show_checkers': True, + 'show_axes': True, + }, + 'calibration_path': './calibrations', +} + + + + + +def parse_cli_args(): + parser = argparse.ArgumentParser(prog='parallax') + parser.add_argument('--config', type=str, default=None, help='configuration file to load at startup') + args = parser.parse_args() + return args + + +def init_config(args, model, main_window): + global config + if args.config is not None: + loaded_config = json.load(open(args.config, 'r')) + for k,v in loaded_config.items(): + if k not in config: + raise KeyError(f"Invalid config key {k}") + config[k] = v + + for i,view in enumerate(config['views']): + screen_widget_ctrl = main_window.widget.add_screen() + if 'default_camera' in view: + camera = model.get_camera(view['default_camera']) + screen_widget_ctrl.screen_widget.set_camera(camera) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 6a7b6ac0..02e21a1e 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -11,7 +11,7 @@ from .rigid_body_transform_tool import RigidBodyTransformTool, PointTransformWidget from .calibration import Calibration from .calibration_worker import CalibrationWorker - +from .config import config class GeometryPanel(QFrame): msg_posted = pyqtSignal(str) @@ -147,7 +147,7 @@ def handle_cal_finished(self): self.model.cal_in_progress = False def load_cal(self): - filename = QFileDialog.getOpenFileName(self, 'Load calibration file', '.', + filename = QFileDialog.getOpenFileName(self, 'Load calibration file', config['calibration_path'], 'Pickle files (*.pkl)')[0] if filename: with open(filename, 'rb') as f: @@ -163,7 +163,7 @@ def save_cal(self): else: cal_selected = self.model.calibrations[self.cal_combo.currentText()] - suggested_filename = os.path.join(os.getcwd(), cal_selected.name + '.pkl') + suggested_filename = os.path.join(config['calibration_path'], cal_selected.name + '.pkl') filename = QFileDialog.getSaveFileName(self, 'Save calibration file', suggested_filename, 'Pickle files (*.pkl)')[0] diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 05882acb..aba09c72 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -4,6 +4,7 @@ import coorx from parallax.calibration import CameraTransform, StereoCameraTransform from parallax.lib import find_checker_corners +from parallax.config import config class CameraTransform(coorx.CompositeTransform): @@ -299,13 +300,19 @@ def __init__(self): self.cameras = {} self.stages = {} - cb_size = 8 - checkers = CheckerBoard(views=[], size=cb_size, colors=[0.3, 0.7]) - s = 1e3 - checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, 0], scale=[s, s, s]) - axis = Axis(views=[]) - axis.transform.set_params(scale=[s, s, s]) - self.items = [checkers, axis] + self.items = [] + if config['mock_sim']['show_checkers']: + cb_size = 8 + checkers = CheckerBoard(views=[], size=cb_size, colors=[0.4, 0.6]) + s = 1e3 + checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, -2000], scale=[s, s, s]) + self.items.append(checkers) + + if config['mock_sim']['show_axes']: + axis = Axis(views=[]) + axis.transform.set_params(scale=[s, s, s]) + self.items.append(axis) + self.stage_moved.connect(self.update_stage) def add_camera(self, cam): diff --git a/run-parallax.py b/run-parallax.py index 8337b39b..3a47b764 100755 --- a/run-parallax.py +++ b/run-parallax.py @@ -1,8 +1,9 @@ #!/usr/bin/env python +import atexit from PyQt5.QtWidgets import QApplication from parallax.model import Model from parallax.main_window import MainWindow -import atexit +import parallax.config # set up logging to file import logging @@ -22,4 +23,7 @@ main_window = MainWindow(model) main_window.show() +args = parallax.config.parse_cli_args() +parallax.config.init_config(args, model, main_window) + app.exec() From 3f41e1e7943d91c900fb55eb5bdcd04f5c0e3a2a Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 12:26:33 -0800 Subject: [PATCH 15/62] GUI updates: - added main splitter - adjusted layout margins, default window size - support for more cameras (simplified code) --- parallax/dialogs.py | 4 +-- parallax/geometry_panel.py | 4 +-- parallax/main_window.py | 71 ++++++++++++++++++++------------------ parallax/message_log.py | 1 + parallax/model.py | 22 ++++++------ parallax/screen_widget.py | 1 + 6 files changed, 52 insertions(+), 51 deletions(-) diff --git a/parallax/dialogs.py b/parallax/dialogs.py index 176fcba8..94cdc661 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -144,9 +144,7 @@ def __init__(self, model, parent=None): self.setMinimumWidth(300) def get_stage(self): - ip = self.stage_dropdown.currentText() - stage = self.model.stages[ip] - return stage + return self.stage_dropdown.current_stage() def get_resolution(self): return self.resolution_box.value() diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 02e21a1e..c43bc864 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -81,7 +81,7 @@ def triangulate(self): else: cal_selected = self.model.calibrations[self.cal_combo.currentText()] - if not (self.model.lcorr and self.model.rcorr): + if None in (self.model.lcorr, self.model.rcorr): self.msg_posted.emit('No correspondence points selected.') return else: @@ -126,7 +126,7 @@ def handle_cal_point_reached(self, n, num_cal, x,y,z): def register_corr_points_cal(self): lcorr, rcorr = self.model.lcorr, self.model.rcorr - if (lcorr and rcorr): + if None not in (lcorr, rcorr): self.cal_worker.register_corr_points(lcorr, rcorr) self.msg_posted.emit('Correspondence points registered: (%d,%d) and (%d,%d)' % \ (lcorr[0],lcorr[1], rcorr[0],rcorr[1])) diff --git a/parallax/main_window.py b/parallax/main_window.py index 84c015a3..14f2c7cf 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -1,4 +1,4 @@ -from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QHBoxLayout, QMainWindow, QAction +from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QIcon import pyqtgraph.console @@ -20,6 +20,7 @@ def __init__(self, model): self.widget = MainWidget(model) self.setCentralWidget(self.widget) + self.resize(1280, 900) # menubar actions self.save_frames_action = QAction("Save Camera Frames") @@ -85,7 +86,7 @@ def new_transform(self, name, tr): self.model.add_transform(name, tr) def screens(self): - return self.widget.lscreen, self.widget.rscreen + return self.widget.screens[:] def refresh_cameras(self): self.model.scan_for_cameras() @@ -107,36 +108,37 @@ def refresh_focus_controllers(self): screen.update_focus_control_menu() -class MainWidget(QWidget): - +class MainWidget(QSplitter): def __init__(self, model): - QWidget.__init__(self) + QSplitter.__init__(self, Qt.Vertical) self.model = model - self.screens = QWidget() - hlayout = QHBoxLayout() - self.lscreen = ScreenWidgetControl(model=self.model) - self.rscreen = ScreenWidgetControl(model=self.model) - hlayout.addWidget(self.lscreen) - hlayout.addWidget(self.rscreen) - self.screens.setLayout(hlayout) + self.screens_widget = QWidget() + self.screens_layout = QHBoxLayout() + self.screens_widget.setLayout(self.screens_layout) + self.screens = [] # screens are added by init config + self.addWidget(self.screens_widget) self.controls = QWidget() self.control_panel1 = ControlPanel(self.model) self.control_panel2 = ControlPanel(self.model) self.geo_panel = GeometryPanel(self.model) - hlayout = QHBoxLayout() - hlayout.addWidget(self.control_panel1) - hlayout.addWidget(self.geo_panel) - hlayout.addWidget(self.control_panel2) - self.controls.setLayout(hlayout) + self.msg_log = MessageLog() + self.controls_layout = QGridLayout() + self.controls_layout.addWidget(self.control_panel1, 0, 0) + self.controls_layout.addWidget(self.geo_panel, 0, 1) + self.controls_layout.addWidget(self.control_panel2, 0, 2) + self.controls_layout.addWidget(self.msg_log, 1, 0, 1, 3) + self.controls.setLayout(self.controls_layout) + self.addWidget(self.controls) + + self.setSizes([550, 350]) self.refresh_timer = QTimer() self.refresh_timer.timeout.connect(self.refresh) self.refresh_timer.start(125) # connections - self.msg_log = MessageLog() self.control_panel1.msg_posted.connect(self.msg_log.post) self.control_panel2.msg_posted.connect(self.msg_log.post) self.control_panel1.target_reached.connect(self.zoom_out) @@ -145,16 +147,14 @@ def __init__(self, model): self.geo_panel.cal_point_reached.connect(self.clear_selected) self.geo_panel.cal_point_reached.connect(self.zoom_out) self.model.msg_posted.connect(self.msg_log.post) - self.lscreen.selected.connect(self.model.set_lcorr) - self.lscreen.cleared.connect(self.model.clear_lcorr) - self.rscreen.selected.connect(self.model.set_rcorr) - self.rscreen.cleared.connect(self.model.clear_rcorr) - main_layout = QVBoxLayout() - main_layout.addWidget(self.screens) - main_layout.addWidget(self.controls) - main_layout.addWidget(self.msg_log) - self.setLayout(main_layout) + def add_screen(self): + screen = ScreenWidgetControl(model=self.model) + self.screens_layout.addWidget(screen) + self.screens.append(screen) + screen.selected.connect(self.update_corr) + screen.cleared.connect(self.update_corr) + return screen def keyPressEvent(self, e): if e.key() == Qt.Key_R: @@ -169,16 +169,16 @@ def keyPressEvent(self, e): self.model.halt_all_stages() def refresh(self): - self.lscreen.refresh() - self.rscreen.refresh() + for screen in self.screens: + screen.refresh() def clear_selected(self): - self.lscreen.clear_selected() - self.rscreen.clear_selected() + for screen in self.screens: + screen.clear_selected() def zoom_out(self): - self.lscreen.zoom_out() - self.rscreen.zoom_out() + for screen in self.screens: + screen.zoom_out() def save_camera_frames(self): for i,camera in enumerate(self.model.cameras): @@ -187,4 +187,7 @@ def save_camera_frames(self): camera.save_last_image(filename) self.msg_log.post('Saved camera frame: %s' % filename) - + def update_corr(self): + # send correspondence points to model + pts = [ctrl.screen_widget.get_selected() for ctrl in self.screens] + self.model.set_correspondence_points(pts) diff --git a/parallax/message_log.py b/parallax/message_log.py index 1e1b9e32..19ace843 100644 --- a/parallax/message_log.py +++ b/parallax/message_log.py @@ -15,6 +15,7 @@ def __init__(self, parent=None): main_layout = QVBoxLayout() main_layout.addWidget(self.message_log) + main_layout.setContentsMargins(0, 0, 0, 0) self.setLayout(main_layout) def post(self, message, **kwargs): diff --git a/parallax/model.py b/parallax/model.py index 46a4bdfc..a968dc99 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -22,7 +22,7 @@ def __init__(self): self.calibrations = {} self.cal_in_progress = False - self.lcorr, self.rcorr = False, False + self.lcorr, self.rcorr = None, None self.obj_point_last = None self.transforms = {} @@ -31,6 +31,12 @@ def __init__(self): def ncameras(self): return len(self.cameras) + def get_camera(self, camera_name): + for cam in self.cameras: + if cam.name() == camera_name: + return cam + raise NameError(f"No camera named {camera_name}") + def set_last_object_point(self, obj_point): self.obj_point_last = obj_point @@ -40,17 +46,9 @@ def add_calibration(self, cal): def set_calibration(self, calibration): self.calibration = calibration - def set_lcorr(self, xc, yc): - self.lcorr = [xc, yc] - - def clear_lcorr(self): - self.lcorr = False - - def set_rcorr(self, xc, yc): - self.rcorr = [xc, yc] - - def clear_rcorr(self): - self.rcorr = False + def set_correspondence_points(self, pts): + self.lcorr = pts[0] + self.rcorr = pts[1] def scan_for_cameras(self): self.cameras = list_cameras() diff --git a/parallax/screen_widget.py b/parallax/screen_widget.py index e0d6e494..2503302b 100755 --- a/parallax/screen_widget.py +++ b/parallax/screen_widget.py @@ -28,6 +28,7 @@ def __init__(self, filename=None, model=None, parent=None): self.layout.addWidget(self.contrast_slider) self.layout.addWidget(self.brightness_slider) self.setLayout(self.layout) + self.layout.setContentsMargins(0, 0, 0, 0) # connections self.screen_widget.selected.connect(self.selected) From 964965410723b0c3ccc2f5715c45425b7169309d Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 12:27:17 -0800 Subject: [PATCH 16/62] Get calibration working again --- parallax/calibration.py | 63 ++++++++++++++------------------------ parallax/geometry_panel.py | 2 +- parallax/lib.py | 4 --- parallax/stage.py | 8 ++++- 4 files changed, 31 insertions(+), 46 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index a3a42a38..017d5627 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -21,10 +21,8 @@ class Calibration: - def __init__(self, name): self.name = name - self.set_initial_intrinsics_default() def set_name(self, name): self.name = name @@ -35,41 +33,17 @@ def set_origin(self, origin): def get_origin(self): return self.origin - def set_initial_intrinsics(self, mtx1, mtx2, dist1, dist2): - - self.imtx1 = mtx1 - self.imtx2 = mtx2 - self.idist1 = dist1 - self.idist2 = dist2 - - def set_initial_intrinsics_default(self): - - self.imtx1 = np.array(imtx1, dtype=np.float32) - self.imtx2 = np.array(imtx2, dtype=np.float32) - self.idist1 = np.array(idist1, dtype=np.float32) - self.idist2 = np.array(idist2, dtype=np.float32) - def triangulate(self, lcorr, rcorr): """ l/rcorr = [xc, yc] """ - - img_points1_cv = np.array([[lcorr]], dtype=np.float32) - img_points2_cv = np.array([[rcorr]], dtype=np.float32) - - # undistort - img_points1_cv = lib.undistort_image_points(img_points1_cv, self.mtx1, self.dist1) - img_points2_cv = lib.undistort_image_points(img_points2_cv, self.mtx2, self.dist2) - - img_point1 = img_points1_cv[0,0] - img_point2 = img_points2_cv[0,0] - obj_point_reconstructed = lib.triangulate_from_image_points(img_point1, img_point2, self.proj1, self.proj2) - - return obj_point_reconstructed # [x,y,z] + return self.transform.map(np.concatenate([lcorr, rcorr])) def calibrate(self, img_points1, img_points2, obj_points, origin): - self.set_origin(origin) + self.transform = StereoCameraTransform() + self.transform.set_mapping(img_points1, img_points2, obj_points) + class CameraTransform(coorx.BaseTransform): @@ -85,7 +59,7 @@ def set_coeff(self, mtx, dist): self.dist = dist def _map(self, pts): - return lib.undistort_image_points(pts, self.mtx, self.dist) + return lib.undistort_image_points(pts.astype('float32'), self.mtx, self.dist)[0] class StereoCameraTransform(coorx.BaseTransform): @@ -128,17 +102,26 @@ def set_mapping(self, img_points1, img_points2, obj_points): (WF, HF), self.imtx2, self.idist2, flags=my_flags) + self.camera_tr1.set_coeff(mtx1, dist1) + self.camera_tr2.set_coeff(mtx2, dist2) + # calculate projection matrices - proj1 = lib.get_projection_matrix(mtx1, rvecs1[0], tvecs1[0]) - proj2 = lib.get_projection_matrix(mtx2, rvecs2[0], tvecs2[0]) - - self.mtx1 = mtx1 - self.mtx2 = mtx2 - self.dist1 = dist1 - self.dist2 = dist2 - self.proj1 = proj1 - self.proj2 = proj2 + self.proj1 = lib.get_projection_matrix(mtx1, rvecs1[0], tvecs1[0]) + self.proj2 = lib.get_projection_matrix(mtx2, rvecs2[0], tvecs2[0]) + self.rmse1 = rmse1 self.rmse2 = rmse2 + def triangulate(self, img_point1, img_point2): + x,y,z = lib.DLT(self.proj1, self.proj2, img_point1, img_point2) + return np.array([x,y,z]) + + def _map(self, arr2d): + # undistort + img_pts1 = self.camera_tr1.map(arr2d[:, 0:2]) + img_pts2 = self.camera_tr2.map(arr2d[:, 2:4]) + # triangulate + n_pts = arr2d.shape[0] + obj_points = [self.triangulate(*img_pts) for img_pts in zip(img_pts1, img_pts2)] + return np.vstack(obj_points) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index c43bc864..d6e04e44 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -141,7 +141,7 @@ def handle_cal_finished(self): origin = self.cal_worker.stage.get_origin() cal.calibrate(img_points1, img_points2, obj_points, origin) self.msg_posted.emit('Calibration finished. RMSE1 = %f, RMSE2 = %f' % \ - (cal.rmse1, cal.rmse2)) + (cal.transform.rmse1, cal.transform.rmse2)) self.model.add_calibration(cal) self.update_cals() self.model.cal_in_progress = False diff --git a/parallax/lib.py b/parallax/lib.py index ebdef8b9..65f4dcbf 100644 --- a/parallax/lib.py +++ b/parallax/lib.py @@ -59,10 +59,6 @@ def DLT(P1, P2, point1, point2): U, s, vh = linalg.svd(B, full_matrices = False) return vh[3,0:3]/vh[3,3] -def triangulate_from_image_points(img_point1, img_point2, proj1, proj2): - x,y,z = DLT(proj1, proj2, img_point1, img_point2) - return np.array([x,y,z], dtype=np.float32) - def find_checker_corners(img, board_shape, show=False): """https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html diff --git a/parallax/stage.py b/parallax/stage.py index 0478fd90..5d9c78b4 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -176,7 +176,13 @@ def get_accel(self): def get_position(self): return self.pos.copy() - def move_to_target_3d(self, x, y, z): + def move_to_target_3d(self, x, y, z, relative=False, safe=False): + if relative: + xo,yo,zo = self.get_origin() + x += xo + y += yo + z += zo + move_cmd = {'pos': np.array([x, y, z]), 'speed': self.speed, 'accel': self.accel, 'finished': False, 'interrupted': False} self.move_queue.put(move_cmd) From 01457efad4c5bf794ab81f812d3a1cb8e672646d Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sat, 11 Feb 2023 12:27:31 -0800 Subject: [PATCH 17/62] mock sim tweaks --- parallax/camera.py | 9 ++++++++- parallax/mock_sim.py | 8 ++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/parallax/camera.py b/parallax/camera.py index f9070211..e8967e3e 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -191,5 +191,12 @@ def get_last_image_data(self): noise = self.noise[self._next_frame] self._next_frame = (self._next_frame + 1) % self.noise.shape[0] - image = self.sim.get_camera_frame(self) + noise + image = self.sim.get_camera_frame(self) + + # # add noise + # image = image.copy() + # # squeeze to leave room for noise (and prevent overflows) + # np.multiply(image, 224/255, out=image, casting='unsafe') + # image += noise + return image diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index aba09c72..7b05f0de 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -157,9 +157,9 @@ def __init__(self, views): super().__init__(views) self.items = [ - {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': 'r'}, - {'type': 'line', 'points': [[0, 0, 0], [0, 1, 0]], 'pen': 'g'}, - {'type': 'line', 'points': [[0, 0, 0], [0, 0, 1]], 'pen': 'b'}, + {'type': 'line', 'points': [[0, 0, 0], [1, 0, 0]], 'pen': {'color': 'r', 'width': 5}}, + {'type': 'line', 'points': [[0, 0, 0], [0, 1, 0]], 'pen': {'color': 'g', 'width': 5}}, + {'type': 'line', 'points': [[0, 0, 0], [0, 0, 1]], 'pen': {'color': 'b', 'width': 5}}, ] self.render() @@ -171,7 +171,7 @@ def __init__(self, views): l = 10e3 self.items = [ {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ - [0, 0, 0], [w, 0, w], [w, 0, l], [-w, 0, l], [-w, 0, l], [0, 0, 0] + [0, 0, 0], [w, 0, w], [w, 0, l], [-w, 0, l], [-w, 0, -w], [0, 0, 0] ]}, ] self.render() From 3f5e0d5aac81b7d20b58bc713569ad7e57f671d7 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 01:40:04 -0800 Subject: [PATCH 18/62] Calibration working correctly on mock stage calibrations now know from/to coordinate system names --- parallax/calibration.py | 55 +++++++++++++++------------- parallax/calibration_worker.py | 36 +++++++++---------- parallax/camera.py | 6 ++++ parallax/geometry_panel.py | 7 +--- parallax/lib.py | 6 ++-- parallax/main_window.py | 17 ++++++++- parallax/screen_widget.py | 8 ++++- parallax/stage.py | 50 ++++++++++++++++++-------- zz_notes | 66 ++++++++++++++++++++++++++++++++++ 9 files changed, 181 insertions(+), 70 deletions(-) create mode 100644 zz_notes diff --git a/parallax/calibration.py b/parallax/calibration.py index 017d5627..eb97c272 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -4,7 +4,6 @@ import cv2 as cv import coorx from . import lib -from .helper import WF, HF imtx1 = [[1.81982227e+04, 0.00000000e+00, 2.59310865e+03], @@ -21,17 +20,17 @@ class Calibration: - def __init__(self, name): + def __init__(self, name, img_size): self.name = name + self.img_size = img_size + self.img_points1 = [] + self.img_points2 = [] + self.obj_points = [] # units are mm - def set_name(self, name): - self.name = name - - def set_origin(self, origin): - self.origin = origin - - def get_origin(self): - return self.origin + def add_points(self, img_pt1, img_pt2, obj_pt): + self.img_points1.append(img_pt1) + self.img_points2.append(img_pt2) + self.obj_points.append(obj_pt) def triangulate(self, lcorr, rcorr): """ @@ -39,11 +38,16 @@ def triangulate(self, lcorr, rcorr): """ return self.transform.map(np.concatenate([lcorr, rcorr])) - def calibrate(self, img_points1, img_points2, obj_points, origin): - self.set_origin(origin) - self.transform = StereoCameraTransform() - self.transform.set_mapping(img_points1, img_points2, obj_points) - + def calibrate(self): + from_cs = f"{self.img_points1[0].system.name}+{self.img_points2[0].system.name}" + to_cs = self.obj_points[0].system.name + self.transform = StereoCameraTransform(from_cs=from_cs, to_cs=to_cs) + self.transform.set_mapping( + np.array(self.img_points1), + np.array(self.img_points2), + np.array(self.obj_points), + self.img_size + ) class CameraTransform(coorx.BaseTransform): @@ -59,7 +63,7 @@ def set_coeff(self, mtx, dist): self.dist = dist def _map(self, pts): - return lib.undistort_image_points(pts.astype('float32'), self.mtx, self.dist)[0] + return lib.undistort_image_points(pts, self.mtx, self.dist) class StereoCameraTransform(coorx.BaseTransform): @@ -88,19 +92,20 @@ def __init__(self, **kwds): self.proj1 = None self.proj2 = None - def set_mapping(self, img_points1, img_points2, obj_points): + def set_mapping(self, img_points1, img_points2, obj_points, img_size): # undistort calibration points - img_points1 = lib.undistort_image_points(img_points1, self.imtx1, self.idist1) - img_points2 = lib.undistort_image_points(img_points2, self.imtx2, self.idist2) + img_points1_undist = lib.undistort_image_points(img_points1, self.imtx1, self.idist1) + img_points2_undist = lib.undistort_image_points(img_points2, self.imtx2, self.idist2) # calibrate each camera against these points + obj_points = obj_points.astype('float32') my_flags = cv.CALIB_USE_INTRINSIC_GUESS + cv.CALIB_FIX_PRINCIPAL_POINT - rmse1, mtx1, dist1, rvecs1, tvecs1 = cv.calibrateCamera(obj_points, img_points1, - (WF, HF), self.imtx1, self.idist1, - flags=my_flags) - rmse2, mtx2, dist2, rvecs2, tvecs2 = cv.calibrateCamera(obj_points, img_points2, - (WF, HF), self.imtx2, self.idist2, - flags=my_flags) + rmse1, mtx1, dist1, rvecs1, tvecs1 = cv.calibrateCamera(obj_points[np.newaxis, ...], img_points1_undist[np.newaxis, ...], + img_size, self.imtx1, self.idist1, + flags=my_flags) + rmse2, mtx2, dist2, rvecs2, tvecs2 = cv.calibrateCamera(obj_points[np.newaxis, ...], img_points2_undist[np.newaxis, ...], + img_size, self.imtx2, self.idist2, + flags=my_flags) self.camera_tr1.set_coeff(mtx1, dist1) self.camera_tr2.set_coeff(mtx2, dist2) diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 8a0f1355..9f8d25b1 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -1,6 +1,9 @@ from PyQt5.QtCore import QObject, pyqtSignal import numpy as np import time +import queue +from .calibration import Calibration +from .helper import WF, HF class CalibrationWorker(QObject): @@ -16,24 +19,17 @@ def __init__(self, name, stage, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_ # (so default value of 3 will yield 3^3 = 27 calibration points) # extent_um is the extent in microns for each dimension, centered on zero QObject.__init__(self) - self.name = name self.stage = stage self.resolution = resolution self.extent_um = extent_um self.ready_to_go = False - self.object_points = [] # units are mm self.num_cal = self.resolution**3 - - self.img_points1 = [] - self.img_points2 = [] + self.calibration = Calibration(name, img_size=(WF, HF)) + self.corr_point_queue = queue.Queue() def register_corr_points(self, lcorr, rcorr): - self.img_points1.append(lcorr) - self.img_points2.append(rcorr) - - def carry_on(self): - self.ready_to_go = True + self.corr_point_queue.put((lcorr, rcorr)) def run(self): mx = self.extent_um / 2. @@ -43,18 +39,18 @@ def run(self): for y in np.linspace(mn, mx, self.resolution): for z in np.linspace(mn, mx, self.resolution): self.stage.move_to_target_3d(x,y,z, relative=True, safe=False) - self.calibration_point_reached.emit(n,self.num_cal, x,y,z) - self.ready_to_go = False - while not self.ready_to_go: - time.sleep(0.1) - self.object_points.append([x,y,z]) + self.calibration_point_reached.emit(n, self.num_cal, x, y, z) + + # wait for correspondence points to arrive + lcorr, rcorr = self.corr_point_queue.get() + + # add to calibration + self.calibration.add_points(lcorr, rcorr, self.stage.get_position()) n += 1 self.finished.emit() - def get_image_points(self): - return np.array([self.img_points1], dtype=np.float32), \ - np.array([self.img_points2], dtype=np.float32) + def get_calibration(self): + self.calibration.calibrate() + return self.calibration - def get_object_points(self): - return np.array([self.object_points], dtype=np.float32) diff --git a/parallax/camera.py b/parallax/camera.py index e8967e3e..1716d8a4 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -200,3 +200,9 @@ def get_last_image_data(self): # image += noise return image + + @property + def camera_tr(self): + """Transform mapping from simulated global coordinate system to camera image pixels. + """ + return self.sim.cameras[self]['view'].camera_tr \ No newline at end of file diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index d6e04e44..962ac32b 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -130,16 +130,11 @@ def register_corr_points_cal(self): self.cal_worker.register_corr_points(lcorr, rcorr) self.msg_posted.emit('Correspondence points registered: (%d,%d) and (%d,%d)' % \ (lcorr[0],lcorr[1], rcorr[0],rcorr[1])) - self.cal_worker.carry_on() else: self.msg_posted.emit('Highlight correspondence points and press C to continue') def handle_cal_finished(self): - cal = Calibration(self.cal_worker.name) - img_points1, img_points2 = self.cal_worker.get_image_points() - obj_points = self.cal_worker.get_object_points() - origin = self.cal_worker.stage.get_origin() - cal.calibrate(img_points1, img_points2, obj_points, origin) + cal = self.cal_worker.get_calibration() self.msg_posted.emit('Calibration finished. RMSE1 = %f, RMSE2 = %f' % \ (cal.transform.rmse1, cal.transform.rmse2)) self.model.add_calibration(cal) diff --git a/parallax/lib.py b/parallax/lib.py index 65f4dcbf..41bb80b8 100644 --- a/parallax/lib.py +++ b/parallax/lib.py @@ -25,7 +25,7 @@ def undistort_image_points(img_points, mtx, dist): - img_points_corrected_normalized = cv.undistortPoints(img_points, mtx, dist) + img_points_corrected_normalized = cv.undistortPoints(img_points.astype('float32'), mtx, dist) fx = mtx[0,0] fy = mtx[1,1] cx = mtx[0,2] @@ -35,8 +35,8 @@ def undistort_image_points(img_points, mtx, dist): x,y = img_point[0] x = x * fx + cx y = y * fy + cy - img_points_corrected.append(np.array([x,y])) - return np.array([img_points_corrected], dtype=np.float32) + img_points_corrected.append([x, y]) + return np.array(img_points_corrected, dtype=np.float32) def get_projection_matrix(mtx, r, t): R, jacobian = cv.Rodrigues(r) diff --git a/parallax/main_window.py b/parallax/main_window.py index 14f2c7cf..d5a90fb5 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -18,6 +18,9 @@ def __init__(self, model): QMainWindow.__init__(self) self.model = model + # allow main window to be accessed globally + model.main_window = self + self.widget = MainWidget(model) self.setCentralWidget(self.widget) self.resize(1280, 900) @@ -95,7 +98,8 @@ def refresh_cameras(self): def show_console(self): if self.console is None: - self.console = pyqtgraph.console.ConsoleWidget() + self.console = pyqtgraph.console.ConsoleWidget(namespace={'model': self.model, 'win': self}) + self.console.catchNextException() self.console.show() def closeEvent(self, ev): @@ -146,6 +150,7 @@ def __init__(self, model): self.geo_panel.msg_posted.connect(self.msg_log.post) self.geo_panel.cal_point_reached.connect(self.clear_selected) self.geo_panel.cal_point_reached.connect(self.zoom_out) + self.geo_panel.cal_point_reached.connect(self.auto_select_cal_point) self.model.msg_posted.connect(self.msg_log.post) def add_screen(self): @@ -191,3 +196,13 @@ def update_corr(self): # send correspondence points to model pts = [ctrl.screen_widget.get_selected() for ctrl in self.screens] self.model.set_correspondence_points(pts) + + def auto_select_cal_point(self): + # auto-calibrate mock stage + stage = self.geo_panel.cal_worker.stage + if hasattr(stage, 'get_tip_position'): + tip_pos = stage.get_tip_position() + for ctrl in self.model.main_window.screens(): + screen = ctrl.screen_widget + pos = screen.camera.camera_tr.map(tip_pos.coordinates) + screen.set_selected(pos[:2]) diff --git a/parallax/screen_widget.py b/parallax/screen_widget.py index 2503302b..48aef8a8 100755 --- a/parallax/screen_widget.py +++ b/parallax/screen_widget.py @@ -4,6 +4,7 @@ from PyQt5.QtCore import pyqtSignal, Qt from PyQt5 import QtCore import pyqtgraph as pg +import coorx class ScreenWidgetControl(QWidget): @@ -155,10 +156,15 @@ def set_focochan(self, foco, chan): def get_selected(self): if self.click_target.isVisible(): pos = self.click_target.pos() - return pos.x(), pos.y() + return coorx.Point([pos.x(), pos.y()], self.camera.name()) else: return None + def set_selected(self, pos): + self.click_target.setPos(pos) + self.click_target.setVisible(True) + self.selected.emit(*self.get_selected()) + def wheelEvent(self, e): forward = bool(e.angleDelta().y() > 0) control = bool(e.modifiers() & Qt.ControlModifier) diff --git a/parallax/stage.py b/parallax/stage.py index 5d9c78b4..5642c369 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -1,6 +1,7 @@ import time, queue, threading import numpy as np import coorx +from coorx import Point from newscale.multistage import USBXYZStage, PoEXYZStage from newscale.interfaces import USBInterface, NewScaleSerial from .mock_sim import MockSim @@ -85,7 +86,9 @@ def get_position(self, relative=False): x -= self.origin[0] y -= self.origin[1] z -= self.origin[2] - return x,y,z + return Point([x,y,z], self.get_name()+'_rel') + else: + return Point([x,y,z], self.get_name()) def move_to_target_1d(self, axis, position, relative=False): if axis == 'x': @@ -143,6 +146,7 @@ def __init__(self, transform): self.accel = 5000 # um/s^2 self.pos = np.array([0, 0, 0]) self.name = f"mock_stage_{MockStage.n_mock_stages}" + transform.set_systems(self.name, "global") MockStage.n_mock_stages += 1 self.move_callbacks = [] @@ -174,21 +178,24 @@ def get_accel(self): return self.accel def get_position(self): - return self.pos.copy() + return Point(self.pos.copy(), self.get_name()) - def move_to_target_3d(self, x, y, z, relative=False, safe=False): + def move_to_target_3d(self, x, y, z, relative=False, safe=False, block=True): if relative: xo,yo,zo = self.get_origin() x += xo y += yo z += zo - move_cmd = {'pos': np.array([x, y, z]), 'speed': self.speed, 'accel': self.accel, 'finished': False, 'interrupted': False} - self.move_queue.put(move_cmd) + move_cmd = MoveFuture(self, pos=np.array([x, y, z]), speed=self.speed, accel=self.accel) + self.move_queue.put(move_cmd) + if block: + move_cmd.wait() + return move_cmd def move_distance_1d(self, axis, distance): ax_ind = 'xyz'.index(axis) - pos = self.get_position() + pos = self.get_position().coordinates pos[ax_ind] += distance return self.move_to_target_3d(*pos) @@ -202,8 +209,7 @@ def thread_loop(self): if next_move is not None: if current_move is not None: - current_move['interrupted'] = True - current_move['finished'] = True + current_move.finish(interrupted=True) current_move = next_move last_update = time.perf_counter() @@ -213,10 +219,10 @@ def thread_loop(self): last_update = now pos = self.get_position() - target = current_move['pos'] + target = current_move.pos dx = target - pos dist_to_go = np.linalg.norm(dx) - max_dist_per_step = current_move['speed'] * dt + max_dist_per_step = current_move.speed * dt if dist_to_go > max_dist_per_step: # take a step direction = dx / dist_to_go @@ -225,8 +231,7 @@ def thread_loop(self): self.pos = pos + step else: self.pos = target.copy() - current_move['interrupted'] = False - current_move['finished'] = True + current_move.finish(interrupted=False) current_move = None for cb in self.move_callbacks[:]: @@ -238,8 +243,8 @@ def add_move_callback(self, cb): self.move_callbacks.append(cb) def orientation(self): - p1 = self.transform.map([0, 0, 0]) - p2 = self.transform.map([0, 0, 1]) + p1 = self.transform.map(Point([0, 0, 0], self.name)) + p2 = self.transform.map(Point([0, 0, 1], self.name)) axis = p1 - p2 r = np.linalg.norm(axis[:2]) phi = np.arctan2(r, axis[2]) * 180 / np.pi @@ -248,3 +253,20 @@ def orientation(self): def get_tip_position(self): return self.transform.map(self.get_position()) + + +class MoveFuture: + def __init__(self, stage, pos, speed, accel): + self.stage = stage + self.pos = pos + self.speed = speed + self.accel = accel + self.finished = threading.Event() + self.interrupted = False + + def finish(self, interrupted): + self.interrupted = interrupted + self.finished.set() + + def wait(self, timeout=None): + self.finished.wait(timeout=timeout) diff --git a/zz_notes b/zz_notes new file mode 100644 index 00000000..3d33f83b --- /dev/null +++ b/zz_notes @@ -0,0 +1,66 @@ +-Package maintenance / code quality: + - Standardize package structure + root code folder named parallax + setup.py + relative imports + - camelCase vs snake_case? + I recommend snake_case (regret using camelCase for acq4) + - Unit tests? + don't put tests in __main__ + - Continuous integration + - Avoid `from X import *` + - lib and Helper have some overlap (getIntrinsicsFromChecherboard, DLT) + - rename / reorg lib and Helper + +- Use QtPy? + This wraps multiple python-Qt libraries so that we can change versions more easily + Otherwise we will need to transition to Qt6 in the near future + +- Add error display / logging facilities + Users will appreciate this + We want good records kept in case the software destroys an electrode / ruins an experiment + +- Add console for realtime debugging + +- Simple 3D rendering of electrodes in MockCamera + so we can test triangulation / calibration + +- Put camera polling in a background thread + I assume it takes some time to request a frame from the camera, + and that the GIL is unlocked during this time + +- Do we want to depend on pyspin for camera access, or write a thin layer to allow adapting other sources later? + Possibly acq4 camera devices? + +- Device configuration system + specify what cameras / stages to look for + any relevant physical details that could be modified in the future and require user config + +- Coorx for transforms + np arrays as standard coordinate object? + coordinate system-aware points? + +- parallax : project name collisions (python packages, neural dsp, .. ) + +- What features may eventually be added? + - Other device models (cameras, stages) + - Other device types (focal / widefield photostim? indicator imaging? behavioral? others?) + - Automatic electrode calibration + - Animal data tracking (pull MRI and desired electrode coordinates from server) + - Automatic electrode delivery + +- Things we might import from acq4: + stage device infrastructure + camera device infrastructure + device management + imaging pipeline + error logging + benefits: + device abstractions to allow extensibility later (as opposed to pyspin) + shared codebase with other allen institute projects + device transform graph + pre-optimized imaging pipeline + device configuration system + drawbacks: + lots of baggage, but perhaps this can be well hidden + From cb0268b6e4618fd4d5fca16113b3cd341cf9f0e5 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 15:01:53 -0800 Subject: [PATCH 19/62] add console command history and editor support --- parallax/config.py | 2 ++ parallax/main_window.py | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/parallax/config.py b/parallax/config.py index 9c555a7c..0a06b2c9 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -10,6 +10,8 @@ 'show_axes': True, }, 'calibration_path': './calibrations', + 'console_history_file': './console_history', + 'console_edit_command': 'code -g {fileName}:{lineNum}', } diff --git a/parallax/main_window.py b/parallax/main_window.py index d5a90fb5..409e505b 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -10,6 +10,7 @@ from .dialogs import AboutDialog from .rigid_body_transform_tool import RigidBodyTransformTool from .stage_manager import StageManager +from .config import config class MainWindow(QMainWindow): @@ -98,7 +99,11 @@ def refresh_cameras(self): def show_console(self): if self.console is None: - self.console = pyqtgraph.console.ConsoleWidget(namespace={'model': self.model, 'win': self}) + self.console = pyqtgraph.console.ConsoleWidget( + historyFile=config['console_history_file'], + editor=config['console_edit_command'], + namespace={'model': self.model, 'win': self} + ) self.console.catchNextException() self.console.show() From d089a403a3b618116a7f4e70bd5fe9ef344cc592 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 15:02:23 -0800 Subject: [PATCH 20/62] move more calibration code to individual camera transforms --- parallax/calibration.py | 88 +++++++++++++++++++++++------------------ 1 file changed, 49 insertions(+), 39 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index eb97c272..70e8bf0f 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -53,38 +53,56 @@ def calibrate(self): class CameraTransform(coorx.BaseTransform): """Maps from camera sensor pixels to undistorted UV. """ + # initial intrinsic / distortion coefficients + imtx = np.array([ + [1.81982227e+04, 0.00000000e+00, 2.59310865e+03], + [0.00000000e+00, 1.89774632e+04, 1.48105977e+03], + [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] + ]) + idist = np.array([[ 1.70600649e+00, -9.85797706e+01, 4.53808433e-03, -2.13200143e-02, 1.79088477e+03]]) + def __init__(self, mtx=None, dist=None, **kwds): super().__init__(dims=(2, 2), **kwds) - self.mtx = mtx - self.dist = dist + self.set_coeff(mtx, dist) def set_coeff(self, mtx, dist): self.mtx = mtx self.dist = dist + self._inverse_transform = None def _map(self, pts): return lib.undistort_image_points(pts, self.mtx, self.dist) + def _imap(self, pts): + if self._inverse_transform is None: + atr1 = coorx.AffineTransform(matrix=self.mtx[:2, :2], offset=self.mtx[:2, 2]) + ltr1 = coorx.nonlinear.LensDistortionTransform(self.dist[0]) + self._inverse_transform = coorx.CompositeTransform([atr.inverse, ltr, atr]) + return self._inverse_transform.map(pts) -class StereoCameraTransform(coorx.BaseTransform): - """Maps from dual camera sensor pixels to 3D object space. - """ - imtx1 = np.array([ - [1.81982227e+04, 0.00000000e+00, 2.59310865e+03], - [0.00000000e+00, 1.89774632e+04, 1.48105977e+03], - [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] - ]) - - imtx2 = np.array([ - [1.55104298e+04, 0.00000000e+00, 1.95422363e+03], - [0.00000000e+00, 1.54250418e+04, 1.64814750e+03], - [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] - ]) + def set_mapping(self, img_pts, obj_pts, img_size): + # undistort calibration points + img_pts_undist = lib.undistort_image_points(img_pts, self.imtx, self.idist) + + # calibrate against correspondence points + rmse, mtx, dist, rvecs, tvecs = cv.calibrateCamera( + obj_pts.astype('float32')[np.newaxis, ...], + img_pts_undist[np.newaxis, ...], + img_size, self.imtx, self.idist, + flags=cv.CALIB_USE_INTRINSIC_GUESS + cv.CALIB_FIX_PRINCIPAL_POINT, + ) - idist1 = np.array([[ 1.70600649e+00, -9.85797706e+01, 4.53808433e-03, -2.13200143e-02, 1.79088477e+03]]) - idist2 = np.array([[-4.94883798e-01, 1.65465770e+02, -1.61013572e-03, 5.22601960e-03, -8.73875986e+03]]) + # calculate projection matrix + self.proj_matrix = lib.get_projection_matrix(mtx, rvecs[0], tvecs[0]) + # record results + self.set_coeff(mtx, dist) + self.calibration_result = {'rmse': rmse, 'mtx': mtx, 'dist': dist, 'rvecs': rvecs, 'tvecs': tvecs} + +class StereoCameraTransform(coorx.BaseTransform): + """Maps from dual camera sensor pixels to 3D object space. + """ def __init__(self, **kwds): super().__init__(dims=(4, 3), **kwds) self.camera_tr1 = CameraTransform() @@ -93,29 +111,14 @@ def __init__(self, **kwds): self.proj2 = None def set_mapping(self, img_points1, img_points2, obj_points, img_size): - # undistort calibration points - img_points1_undist = lib.undistort_image_points(img_points1, self.imtx1, self.idist1) - img_points2_undist = lib.undistort_image_points(img_points2, self.imtx2, self.idist2) + self.camera_tr1.set_mapping(img_points1, obj_points, img_size) + self.camera_tr2.set_mapping(img_points2, obj_points, img_size) - # calibrate each camera against these points - obj_points = obj_points.astype('float32') - my_flags = cv.CALIB_USE_INTRINSIC_GUESS + cv.CALIB_FIX_PRINCIPAL_POINT - rmse1, mtx1, dist1, rvecs1, tvecs1 = cv.calibrateCamera(obj_points[np.newaxis, ...], img_points1_undist[np.newaxis, ...], - img_size, self.imtx1, self.idist1, - flags=my_flags) - rmse2, mtx2, dist2, rvecs2, tvecs2 = cv.calibrateCamera(obj_points[np.newaxis, ...], img_points2_undist[np.newaxis, ...], - img_size, self.imtx2, self.idist2, - flags=my_flags) + self.proj1 = self.camera_tr1.proj_matrix + self.proj2 = self.camera_tr2.proj_matrix - self.camera_tr1.set_coeff(mtx1, dist1) - self.camera_tr2.set_coeff(mtx2, dist2) - - # calculate projection matrices - self.proj1 = lib.get_projection_matrix(mtx1, rvecs1[0], tvecs1[0]) - self.proj2 = lib.get_projection_matrix(mtx2, rvecs2[0], tvecs2[0]) - - self.rmse1 = rmse1 - self.rmse2 = rmse2 + self.rmse1 = self.camera_tr1.calibration_result['rmse'] + self.rmse2 = self.camera_tr2.calibration_result['rmse'] def triangulate(self, img_point1, img_point2): x,y,z = lib.DLT(self.proj1, self.proj2, img_point1, img_point2) @@ -130,3 +133,10 @@ def _map(self, arr2d): n_pts = arr2d.shape[0] obj_points = [self.triangulate(*img_pts) for img_pts in zip(img_pts1, img_pts2)] return np.vstack(obj_points) + + def _imap(self, arr2d): + itr1, itr2 = self._inverse_transforms + ret = np.empty((len(arr2d), 4)) + ret[:, 0:2] = itr1.map(arr2d) + ret[:, 2:4] = itr2.map(arr2d) + return ret From e84a6c51bee26a160eaf9953d15b58c3ca1c4f9d Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 15:02:38 -0800 Subject: [PATCH 21/62] make it easier to access selected calibration --- parallax/geometry_panel.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 962ac32b..89e99055 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -75,11 +75,10 @@ def __init__(self, model): def triangulate(self): - if (self.cal_combo.currentIndex() < 0): + cal_selected = self.selected_calibration() + if cal_selected is None: self.msg_posted.emit('No calibration selected.') return - else: - cal_selected = self.model.calibrations[self.cal_combo.currentText()] if None in (self.model.lcorr, self.model.rcorr): self.msg_posted.emit('No correspondence points selected.') @@ -94,6 +93,12 @@ def triangulate(self): self.msg_posted.emit('Reconstructed object point: ' '[{0:.2f}, {1:.2f}, {2:.2f}]'.format(x, y, z)) + def selected_calibration(self): + if (self.cal_combo.currentIndex() < 0): + return None + else: + return self.model.calibrations[self.cal_combo.currentText()] + def launch_cal_dialog(self): dlg = CalibrationDialog(self.model) From a44d57023246732e42f0fc9295869ee0d3b81dc3 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 15:51:23 -0800 Subject: [PATCH 22/62] fix esc key --- parallax/model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallax/model.py b/parallax/model.py index a968dc99..453f7855 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -74,7 +74,7 @@ def clean(self): close_stages() def halt_all_stages(self): - for stage in self.stages.values(): + for stage in self.stages: stage.halt() self.msg_posted.emit('Halting all stages.') From c29c569ff02f684cc4301495a2c718961459d3ab Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 19:56:38 -0800 Subject: [PATCH 23/62] store CS names with calibration --- parallax/calibration.py | 27 ++++++++------------------- 1 file changed, 8 insertions(+), 19 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index 70e8bf0f..6a51bee4 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -1,31 +1,16 @@ -#!/usr/bin/python3 - import numpy as np import cv2 as cv import coorx from . import lib -imtx1 = [[1.81982227e+04, 0.00000000e+00, 2.59310865e+03], - [0.00000000e+00, 1.89774632e+04, 1.48105977e+03], - [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]] - -imtx2 = [[1.55104298e+04, 0.00000000e+00, 1.95422363e+03], - [0.00000000e+00, 1.54250418e+04, 1.64814750e+03], - [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]] - -idist1 = [[ 1.70600649e+00, -9.85797706e+01, 4.53808433e-03, -2.13200143e-02, 1.79088477e+03]] - -idist2 = [[-4.94883798e-01, 1.65465770e+02, -1.61013572e-03, 5.22601960e-03, -8.73875986e+03]] - - class Calibration: def __init__(self, name, img_size): self.name = name self.img_size = img_size self.img_points1 = [] self.img_points2 = [] - self.obj_points = [] # units are mm + self.obj_points = [] def add_points(self, img_pt1, img_pt2, obj_pt): self.img_points1.append(img_pt1) @@ -39,9 +24,13 @@ def triangulate(self, lcorr, rcorr): return self.transform.map(np.concatenate([lcorr, rcorr])) def calibrate(self): - from_cs = f"{self.img_points1[0].system.name}+{self.img_points2[0].system.name}" - to_cs = self.obj_points[0].system.name - self.transform = StereoCameraTransform(from_cs=from_cs, to_cs=to_cs) + cam1 = self.img_points1[0].system.name + cam2 = self.img_points2[0].system.name + stage = self.obj_points[0].system.name + self.camera_names = (cam1, cam2) + self.stage_name = stage + + self.transform = StereoCameraTransform(from_cs=f"{cam1}+{cam2}", to_cs=stage) self.transform.set_mapping( np.array(self.img_points1), np.array(self.img_points2), From 7a86da3bfa4a66881a27fcf816e6ac76c395e0ec Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 19:57:52 -0800 Subject: [PATCH 24/62] more efficient mock graphics --- parallax/camera.py | 8 +++--- parallax/mock_sim.py | 64 +++++++++++++++++++++++--------------------- 2 files changed, 38 insertions(+), 34 deletions(-) diff --git a/parallax/camera.py b/parallax/camera.py index 1716d8a4..54a16267 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -21,8 +21,8 @@ def list_cameras(): cameras.extend(PySpinCamera.list_cameras()) else: cameras.extend([ - MockCamera(camera_params={'pitch': 60, 'yaw': 0}), - MockCamera(camera_params={'pitch': 60, 'yaw': 30}), + MockCamera(camera_params={'pitch': 70, 'yaw': 120}), + MockCamera(camera_params={'pitch': 70, 'yaw': 150}), ]) return cameras @@ -188,12 +188,12 @@ def get_last_image_data(self): """ Return last image as numpy array with shape (height, width, 3) for RGB or (height, width) for mono. """ - noise = self.noise[self._next_frame] - self._next_frame = (self._next_frame + 1) % self.noise.shape[0] image = self.sim.get_camera_frame(self) # # add noise + # noise = self.noise[self._next_frame] + # self._next_frame = (self._next_frame + 1) % self.noise.shape[0] # image = image.copy() # # squeeze to leave room for noise (and prevent overflows) # np.multiply(image, 224/255, out=image, casting='unsafe') diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 7b05f0de..244536b7 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -86,16 +86,19 @@ class GraphicsItemView: def __init__(self, item, view): self.item = item self.view = view + self.rendered = False self.full_transform = coorx.CompositeTransform([]) self.update_transform() self.scene = view.scene() + view.prepare_for_paint.connect(self.render_if_needed) self.full_transform.add_change_callback(self.transform_changed) def update_transform(self): self.full_transform.transforms = [self.item.transform, self.view.camera_tr] def transform_changed(self, event): - self.render() + self.rendered = False + self.view.update() def render(self): self.clear_graphics_items() @@ -128,6 +131,11 @@ def render(self): item.setdefault('graphicsItems', {}) item['graphicsItems'][self] = gfx_item self.scene.addItem(gfx_item) + self.rendered = True + + def render_if_needed(self): + if not self.rendered: + self.render() def clear_graphics_items(self): for item in self.item.items: @@ -171,14 +179,18 @@ def __init__(self, views): l = 10e3 self.items = [ {'type': 'poly', 'pen': None, 'brush': 0.2, 'points': [ - [0, 0, 0], [w, 0, w], [w, 0, l], [-w, 0, l], [-w, 0, -w], [0, 0, 0] + [0, 0, 0], [w, 0, 2*w], [w, 0, l], [-w, 0, l], [-w, 0, 2*w], [0, 0, 0] ]}, ] self.render() class GraphicsView3D(pg.GraphicsView): + + prepare_for_paint = pg.QtCore.Signal() + def __init__(self, **kwds): + self.cached_frame = None self.camera_tr = CameraTransform() self.press_event = None self.camera_params = {'look': [0, 0, 0], 'pitch': 30, 'yaw': 0, 'distance': 10, 'fov': 45, 'distortion': (0, 0, 0, 0, 0)} @@ -231,9 +243,23 @@ def resizeEvent(self, ev): super().resizeEvent(ev) self.update_camera() + def paintEvent(self, ev): + self.prepare_for_paint.emit() + return super().paintEvent(ev) + + def item_changed(self, item): + self.clear_frames() + self.update() + def get_array(self): - arr = pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True, transpose=False)[..., :3] - return arr + if self.cached_frame is None: + self.prepare_for_paint.emit() + self.cached_frame = pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True, transpose=False)[..., :3] + return self.cached_frame + + def update(self): + self.cached_frame = None + super().update() def generate_calibration_data(view, n_images, cb_size): @@ -312,15 +338,13 @@ def __init__(self): axis = Axis(views=[]) axis.transform.set_params(scale=[s, s, s]) self.items.append(axis) - - self.stage_moved.connect(self.update_stage) def add_camera(self, cam): view = GraphicsView3D(background=(128, 128, 128)) view.resize(*cam.sensor_size) view.set_camera(**cam.camera_params) view.scene().changed.connect(self.clear_frames) - self.cameras[cam] = {'view': view, 'frame': None} + self.cameras[cam] = {'view': view} for item in self.items: item.add_view(view) @@ -330,31 +354,11 @@ def clear_frames(self): v['frame'] = None def get_camera_frame(self, cam): - if self.cameras[cam]['frame'] is None: - view = self.cameras[cam]['view'] - self.cameras[cam]['frame'] = view.get_array() - return self.cameras[cam]['frame'] + view = self.cameras[cam]['view'] + return view.get_array() def add_stage(self, stage): views = [c['view'] for c in self.cameras.values()] item = Electrode(views=views) - - tr = coorx.AffineTransform(dims=(3, 3)) - theta, phi = stage.orientation() - # s = 1e3 - # tr.scale([s, s, s]) - tr.rotate(phi, [1, 0, 0]) - tr.rotate(theta, [0, 0, 1]) - item.transform = tr - + item.transform = stage.transform self.stages[stage] = {'item': item} - stage.add_move_callback(self._stage_moved_cb) - - def _stage_moved_cb(self, stage): - # callback is invoked in thread; send to gui thread by signal - self.stage_moved.emit(stage) - - def update_stage(self, stage): - pos = stage.get_tip_position() - item = self.stages[stage]['item'] - item.transform.offset = pos From c5fe533e36c95a68bf9d033844a141c7da91db09 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 19:58:22 -0800 Subject: [PATCH 25/62] show stage position as it moves fix mock stage orientation bug --- parallax/control_panel.py | 6 ++++- parallax/stage.py | 48 +++++++++++++++++++++++++++++---------- 2 files changed, 41 insertions(+), 13 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index d080dde1..2348f1f7 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -120,7 +120,10 @@ def handle_stage_selection(self, index): self.update_coordinates() def set_stage(self, stage): + if self.stage is not None: + self.stage.position_changed.disconnect(self.stage_position_changed) self.stage = stage + self.stage.position_changed.connect(self.stage_position_changed) self.update_relative_origin() def move_to_target(self, *args): @@ -182,4 +185,5 @@ def halt(self): # doesn't actually work now because we need threading self.stage.halt() - + def stage_position_changed(self, stage, pos): + self.update_coordinates() diff --git a/parallax/stage.py b/parallax/stage.py index 5642c369..504da2e3 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -1,5 +1,6 @@ import time, queue, threading import numpy as np +from PyQt5 import QtCore import coorx from coorx import Point from newscale.multistage import USBXYZStage, PoEXYZStage @@ -12,10 +13,10 @@ def list_stages(): stages.extend(NewScaleStage.scan_for_stages()) if len(stages) == 0: tr1 = coorx.AffineTransform(dims=(3, 3)) - tr1.rotate(130, (1, 0, 0)) - tr1.rotate(30, (0, 0, 1)) + tr1.rotate(90, (1, 0, 0)) + tr1.rotate(90, (0, 0, 1)) tr2 = tr1.copy() - tr2.rotate(60, (0, 0, 1)) + tr2.rotate(90, (0, 0, 1)) stages.extend([ MockStage(transform=tr1), @@ -136,19 +137,25 @@ def halt(self): pass -class MockStage: +class MockStage(QtCore.QObject): n_mock_stages = 0 + position_changed = QtCore.pyqtSignal(object, object) # self, pos + def __init__(self, transform): - self.transform = transform + super().__init__() + self.base_transform = transform + self.transform = coorx.AffineTransform(dims=(3, 3)) self.speed = 1000 # um/s self.accel = 5000 # um/s^2 self.pos = np.array([0, 0, 0]) self.name = f"mock_stage_{MockStage.n_mock_stages}" - transform.set_systems(self.name, "global") + self.transform.set_systems(self.name, "global") MockStage.n_mock_stages += 1 + self.update_pos(self.pos) + self.move_callbacks = [] self.move_queue = queue.Queue() @@ -199,6 +206,18 @@ def move_distance_1d(self, axis, distance): pos[ax_ind] += distance return self.move_to_target_3d(*pos) + def update_pos(self, pos): + # stage has moved; update and emit + self.pos = pos + + # update transform used by mock graphics + tr = coorx.AffineTransform(dims=(3, 3)) + tr.translate(pos) + tr2 = self.base_transform * tr + self.transform.set_params(matrix=tr2.matrix, offset=tr2.offset) + + self.position_changed.emit(self, pos) + def thread_loop(self): current_move = None while True: @@ -209,7 +228,7 @@ def thread_loop(self): if next_move is not None: if current_move is not None: - current_move.finish(interrupted=True) + current_move.finish(interrupted=True, message="interrupted by another move request") current_move = next_move last_update = time.perf_counter() @@ -228,16 +247,16 @@ def thread_loop(self): direction = dx / dist_to_go dist_this_step = min(dist_to_go, max_dist_per_step) step = direction * dist_this_step - self.pos = pos + step + self.update_pos(pos + step) else: - self.pos = target.copy() + self.update_pos(target.copy()) current_move.finish(interrupted=False) current_move = None for cb in self.move_callbacks[:]: cb(self) - time.sleep(10e-3) + time.sleep(100e-3) def add_move_callback(self, cb): self.move_callbacks.append(cb) @@ -252,7 +271,7 @@ def orientation(self): return theta, phi def get_tip_position(self): - return self.transform.map(self.get_position()) + return self.transform.map(Point([0, 0, 0], self.name)) class MoveFuture: @@ -264,9 +283,14 @@ def __init__(self, stage, pos, speed, accel): self.finished = threading.Event() self.interrupted = False - def finish(self, interrupted): + def finish(self, interrupted, message=None): self.interrupted = interrupted + self.message = message self.finished.set() + @property + def succeeded(self): + return self.finished and not self.interrupted + def wait(self, timeout=None): self.finished.wait(timeout=timeout) From 6e8c8dc7d6a36de48b55ee28bff1de11d947abf6 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 21:53:31 -0800 Subject: [PATCH 26/62] use system names in calibration file name carry system names through calibration transforms, check when moving to target --- parallax/calibration.py | 14 +++++++-- parallax/calibration_worker.py | 4 +-- parallax/control_panel.py | 8 +++-- parallax/dialogs.py | 54 +++++++++++++++++++++------------- parallax/geometry_panel.py | 17 +++++------ 5 files changed, 59 insertions(+), 38 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index 6a51bee4..247b51bb 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -1,3 +1,4 @@ +import time import numpy as np import cv2 as cv import coorx @@ -5,13 +6,17 @@ class Calibration: - def __init__(self, name, img_size): - self.name = name + def __init__(self, img_size): self.img_size = img_size self.img_points1 = [] self.img_points2 = [] self.obj_points = [] + @property + def name(self): + date = time.strftime("%Y-%m-%d-%H-%M-%S", self.timestamp) + return f"{self.camera_names[0]}-{self.camera_names[1]}-{self.stage_name}-{date}" + def add_points(self, img_pt1, img_pt2, obj_pt): self.img_points1.append(img_pt1) self.img_points2.append(img_pt2) @@ -21,7 +26,9 @@ def triangulate(self, lcorr, rcorr): """ l/rcorr = [xc, yc] """ - return self.transform.map(np.concatenate([lcorr, rcorr])) + concat = np.hstack([lcorr, rcorr]) + cpt = coorx.Point(concat, f'{lcorr.system.name}+{rcorr.system.name}') + return self.transform.map(cpt) def calibrate(self): cam1 = self.img_points1[0].system.name @@ -29,6 +36,7 @@ def calibrate(self): stage = self.obj_points[0].system.name self.camera_names = (cam1, cam2) self.stage_name = stage + self.timestamp = time.localtime() self.transform = StereoCameraTransform(from_cs=f"{cam1}+{cam2}", to_cs=stage) self.transform.set_mapping( diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 9f8d25b1..2bfe57fe 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -13,7 +13,7 @@ class CalibrationWorker(QObject): RESOLUTION_DEFAULT = 3 EXTENT_UM_DEFAULT = 2000 - def __init__(self, name, stage, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_UM_DEFAULT, + def __init__(self, stage, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_UM_DEFAULT, parent=None): # resolution is number of steps per dimension, for 3 dimensions # (so default value of 3 will yield 3^3 = 27 calibration points) @@ -25,7 +25,7 @@ def __init__(self, name, stage, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_ self.ready_to_go = False self.num_cal = self.resolution**3 - self.calibration = Calibration(name, img_size=(WF, HF)) + self.calibration = Calibration(img_size=(WF, HF)) self.corr_point_queue = queue.Queue() def register_corr_points(self, lcorr, rcorr): diff --git a/parallax/control_panel.py b/parallax/control_panel.py index 2348f1f7..87d3ff7e 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -2,6 +2,7 @@ from PyQt5.QtWidgets import QVBoxLayout, QGridLayout from PyQt5.QtCore import pyqtSignal, Qt from PyQt5.QtGui import QIcon +import coorx from .helper import FONT_BOLD from .dialogs import StageSettingsDialog, TargetDialog @@ -130,10 +131,11 @@ def move_to_target(self, *args): dlg = TargetDialog(self.model) if dlg.exec_(): params = dlg.get_params() - x = params['x'] - y = params['y'] - z = params['z'] + pt = params['point'] if self.stage: + if isinstance(pt, coorx.Point) and pt.system.name != self.stage.get_name(): + raise Exception(f"Not moving stage {self.stage.get_name()} to coordinate in system {pt.system.name}") + x, y, z = pt self.stage.move_to_target_3d(x, y, z, relative=params['relative'], safe=True) if params['relative']: self.msg_posted.emit('Moved to relative position: ' diff --git a/parallax/dialogs.py b/parallax/dialogs.py index 94cdc661..b76a2644 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -117,13 +117,6 @@ def __init__(self, model, parent=None): self.extent_label.setAlignment(Qt.AlignCenter) self.extent_edit = QLineEdit(str(cw.EXTENT_UM_DEFAULT)) - self.name_label = QLabel('Name') - ts = time.time() - dt = datetime.datetime.fromtimestamp(ts) - cal_default_name = 'cal_%04d%02d%02d-%02d%02d%02d' % (dt.year, - dt.month, dt.day, dt.hour, dt.minute, dt.second) - self.name_edit = QLineEdit(cal_default_name) - self.go_button = QPushButton('Start Calibration Routine') self.go_button.setEnabled(False) self.go_button.clicked.connect(self.go) @@ -135,8 +128,6 @@ def __init__(self, model, parent=None): layout.addWidget(self.resolution_box, 1,1, 1,1) layout.addWidget(self.extent_label, 2,0, 1,1) layout.addWidget(self.extent_edit, 2,1, 1,1) - layout.addWidget(self.name_label, 3,0, 1,1) - layout.addWidget(self.name_edit, 3,1, 1,1) layout.addWidget(self.go_button, 4,0, 1,2) self.setLayout(layout) @@ -152,9 +143,6 @@ def get_resolution(self): def get_extent(self): return float(self.extent_edit.text()) - def get_name(self): - return self.name_edit.text() - def go(self): self.accept() @@ -172,6 +160,7 @@ def __init__(self, model): QDialog.__init__(self) self.model = model + self.obj_point = None self.last_button = QPushButton('Last Reconstructed Point') self.last_button.clicked.connect(self.populate_last) if self.model.obj_point_last is None: @@ -199,10 +188,15 @@ def __init__(self, model): self.zedit = QLineEdit() self.zedit.setValidator(validator) - self.info_label = QLabel('(units are microns)') + self.xedit.textEdited.connect(self.input_changed) + self.yedit.textEdited.connect(self.input_changed) + self.zedit.textEdited.connect(self.input_changed) + self.abs_rel_toggle.toggled.connect(self.input_changed) + + self.info_label = QLabel('') self.info_label.setAlignment(Qt.AlignCenter) self.info_label.setFont(FONT_BOLD) - + self.update_info() self.dialog_buttons = QDialogButtonBox( QDialogButtonBox.Ok | QDialogButtonBox.Cancel, Qt.Horizontal, self) @@ -228,23 +222,41 @@ def __init__(self, model): self.setWindowTitle('Set Target Coordinates') def populate_last(self): - self.xedit.setText('{0:.2f}'.format(self.model.obj_point_last[0])) - self.yedit.setText('{0:.2f}'.format(self.model.obj_point_last[1])) - self.zedit.setText('{0:.2f}'.format(self.model.obj_point_last[2])) + op = self.model.obj_point_last + self.xedit.setText('{0:.2f}'.format(op[0])) + self.yedit.setText('{0:.2f}'.format(op[1])) + self.zedit.setText('{0:.2f}'.format(op[2])) + self.abs_rel_toggle.setChecked(False) + self.obj_point = op + self.update_info() def populate_random(self): + self.obj_point = None self.xedit.setText('{0:.2f}'.format(np.random.uniform(-2000, 2000))) self.yedit.setText('{0:.2f}'.format(np.random.uniform(-2000, 2000))) self.zedit.setText('{0:.2f}'.format(np.random.uniform(-2000, 2000))) + self.update_info() def get_params(self): params = {} - params['x'] = float(self.xedit.text()) - params['y'] = float(self.yedit.text()) - params['z'] = float(self.zedit.text()) - params['relative'] = self.abs_rel_toggle.isChecked() + if self.obj_point is None: + params['point'] = np.array([self.xedit.text(), self.yedit.text(), self.zedit.text()]) + params['relative'] = self.abs_rel_toggle.isChecked() + else: + params['point'] = self.obj_point + params['relative'] = False return params + def update_info(self): + info = "(units are μm)" + if self.obj_point is not None: + info = f'coord sys: {self.obj_point.system.name}\n{info}' + self.info_label.setText(info) + + def input_changed(self): + self.obj_point = None + self.update_info() + class CsvDialog(QDialog): diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 89e99055..4c1c219e 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -90,8 +90,9 @@ def triangulate(self): self.model.set_last_object_point(obj_point) x,y,z = obj_point - self.msg_posted.emit('Reconstructed object point: ' - '[{0:.2f}, {1:.2f}, {2:.2f}]'.format(x, y, z)) + self.msg_posted.emit( + f'Reconstructed object point: [{x:.2f}, {y:.2f}, {z:.2f}] in {obj_point.system.name}' + ) def selected_calibration(self): if (self.cal_combo.currentIndex() < 0): @@ -107,14 +108,13 @@ def launch_cal_dialog(self): stage = dlg.get_stage() res = dlg.get_resolution() extent = dlg.get_extent() - name = dlg.get_name() - self.start_cal_thread(stage, res, extent, name) + self.start_cal_thread(stage, res, extent) - def start_cal_thread(self, stage, res, extent, name): + def start_cal_thread(self, stage, res, extent): self.model.cal_in_progress = True self.cal_thread = QThread() - self.cal_worker = CalibrationWorker(name, stage, res, extent) + self.cal_worker = CalibrationWorker(stage, res, extent) self.cal_worker.moveToThread(self.cal_thread) self.cal_thread.started.connect(self.cal_worker.run) self.cal_worker.calibration_point_reached.connect(self.handle_cal_point_reached) @@ -157,11 +157,10 @@ def load_cal(self): def save_cal(self): - if (self.cal_combo.currentIndex() < 0): + cal_selected = self.selected_calibration() + if cal_selected is None: self.msg_posted.emit('No calibration selected.') return - else: - cal_selected = self.model.calibrations[self.cal_combo.currentText()] suggested_filename = os.path.join(config['calibration_path'], cal_selected.name + '.pkl') filename = QFileDialog.getSaveFileName(self, 'Save calibration file', From 5115afadebf570ad4efa9b5054cd7e09fd759343 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 21:53:39 -0800 Subject: [PATCH 27/62] minor updates --- .gitignore | 3 ++- parallax/mock_sim.py | 11 ++++++++++- parallax/stage.py | 5 +++-- 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 06636ba9..b95acb05 100644 --- a/.gitignore +++ b/.gitignore @@ -10,4 +10,5 @@ parallax/*.log .idea *.egg-info *-workspace -*.log \ No newline at end of file +*.log +console_history diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 244536b7..bf62ada0 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -331,13 +331,22 @@ def __init__(self): cb_size = 8 checkers = CheckerBoard(views=[], size=cb_size, colors=[0.4, 0.6]) s = 1e3 - checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, -2000], scale=[s, s, s]) + checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, -4000], scale=[s, s, s]) self.items.append(checkers) if config['mock_sim']['show_axes']: axis = Axis(views=[]) axis.transform.set_params(scale=[s, s, s]) self.items.append(axis) + axis = Axis(views=[]) + axis.transform.set_params(scale=[s, s, s], offset=[0, 0, -4000]) + self.items.append(axis) + axis = Axis(views=[]) + axis.transform.set_params(scale=[s, s, s], offset=[2000, 0, 0]) + self.items.append(axis) + axis = Axis(views=[]) + axis.transform.set_params(scale=[s, s, s], offset=[0, 2000, 0]) + self.items.append(axis) def add_camera(self, cam): view = GraphicsView3D(background=(128, 128, 128)) diff --git a/parallax/stage.py b/parallax/stage.py index 504da2e3..e8bd6c1a 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -13,10 +13,11 @@ def list_stages(): stages.extend(NewScaleStage.scan_for_stages()) if len(stages) == 0: tr1 = coorx.AffineTransform(dims=(3, 3)) - tr1.rotate(90, (1, 0, 0)) + tr1.translate([0, 0, 500]) + tr1.rotate(60, (1, 0, 0)) tr1.rotate(90, (0, 0, 1)) tr2 = tr1.copy() - tr2.rotate(90, (0, 0, 1)) + tr2.rotate(50, (0, 0, 1)) stages.extend([ MockStage(transform=tr1), From 8b23e35e1570c795cdcf4340c7337ad20ce169fc Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 22:39:43 -0800 Subject: [PATCH 28/62] Autoload calibrations, add move to selected button --- parallax/calibration.py | 6 ++-- parallax/control_panel.py | 71 +++++++++++++++++++++++++++++--------- parallax/geometry_panel.py | 8 ++--- parallax/model.py | 5 +++ 4 files changed, 65 insertions(+), 25 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index 247b51bb..e1c8393d 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -22,13 +22,11 @@ def add_points(self, img_pt1, img_pt2, obj_pt): self.img_points2.append(img_pt2) self.obj_points.append(obj_pt) - def triangulate(self, lcorr, rcorr): + def triangulate(self, img_point): """ l/rcorr = [xc, yc] """ - concat = np.hstack([lcorr, rcorr]) - cpt = coorx.Point(concat, f'{lcorr.system.name}+{rcorr.system.name}') - return self.transform.map(cpt) + return self.transform.map(img_point) def calibrate(self): cam1 = self.img_points1[0].system.name diff --git a/parallax/control_panel.py b/parallax/control_panel.py index 87d3ff7e..50cccc12 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -1,3 +1,4 @@ +import os, re, pickle from PyQt5.QtWidgets import QWidget, QLabel, QPushButton, QFrame from PyQt5.QtWidgets import QVBoxLayout, QGridLayout from PyQt5.QtCore import pyqtSignal, Qt @@ -7,6 +8,7 @@ from .helper import FONT_BOLD from .dialogs import StageSettingsDialog, TargetDialog from .stage_dropdown import StageDropdown +from .config import config JOG_UM_DEFAULT = 250 CJOG_UM_DEFAULT = 50 @@ -68,6 +70,8 @@ def __init__(self, model): self.settings_button.setIcon(QIcon('../img/gear.png')) self.settings_button.clicked.connect(self.handle_settings) + self.calibration_label = QLabel("") + self.xcontrol = AxisControl('x') self.xcontrol.jog_requested.connect(self.jog) self.xcontrol.center_requested.connect(self.center) @@ -81,19 +85,24 @@ def __init__(self, model): self.zero_button = QPushButton('Set Relative Origin') self.zero_button.clicked.connect(self.zero) - self.move_target_button = QPushButton('Move to Target') + self.move_target_button = QPushButton('Move to ...') self.move_target_button.clicked.connect(self.move_to_target) + self.move_selected_button = QPushButton('Move to Selected') + self.move_selected_button.clicked.connect(self.move_to_selected) + # layout main_layout = QGridLayout() main_layout.addWidget(self.main_label, 0,0, 1,3) main_layout.addWidget(self.dropdown, 1,0, 1,2) main_layout.addWidget(self.settings_button, 1,2, 1,1) - main_layout.addWidget(self.xcontrol, 2,0, 1,1) - main_layout.addWidget(self.ycontrol, 2,1, 1,1) - main_layout.addWidget(self.zcontrol, 2,2, 1,1) - main_layout.addWidget(self.zero_button, 3,0, 1,3) - main_layout.addWidget(self.move_target_button, 4,0, 1,3) + main_layout.addWidget(self.calibration_label, 2,0, 1,3) + main_layout.addWidget(self.xcontrol, 3,0, 1,1) + main_layout.addWidget(self.ycontrol, 3,1, 1,1) + main_layout.addWidget(self.zcontrol, 3,2, 1,1) + main_layout.addWidget(self.zero_button, 4,0, 1,3) + main_layout.addWidget(self.move_target_button, 5,0, 1,1) + main_layout.addWidget(self.move_selected_button, 5,1, 1,2) self.setLayout(main_layout) # frame border @@ -119,6 +128,7 @@ def handle_stage_selection(self, index): stage = self.dropdown.current_stage() self.set_stage(stage) self.update_coordinates() + self.update_calibration() def set_stage(self, stage): if self.stage is not None: @@ -133,19 +143,22 @@ def move_to_target(self, *args): params = dlg.get_params() pt = params['point'] if self.stage: - if isinstance(pt, coorx.Point) and pt.system.name != self.stage.get_name(): - raise Exception(f"Not moving stage {self.stage.get_name()} to coordinate in system {pt.system.name}") - x, y, z = pt - self.stage.move_to_target_3d(x, y, z, relative=params['relative'], safe=True) - if params['relative']: - self.msg_posted.emit('Moved to relative position: ' - '[{0:.2f}, {1:.2f}, {2:.2f}]'.format(x, y, z)) - else: - self.msg_posted.emit('Moved to absolute position: ' - '[{0:.2f}, {1:.2f}, {2:.2f}]'.format(x, y, z)) + self.move_to_point(pt, params['relative']) self.update_coordinates() self.target_reached.emit() + def move_to_point(self, pt, relative): + if isinstance(pt, coorx.Point): + if pt.system.name != self.stage.get_name(): + raise Exception(f"Not moving stage {self.stage.get_name()} to coordinate in system {pt.system.name}") + if relative: + raise Exception(f"Not moving to relative point in system {pt.system.name}") + + x, y, z = pt + self.stage.move_to_target_3d(x, y, z, relative=relative, safe=True) + absrel = "relative" if relative else "absolute" + self.msg_posted.emit(f'Moved to {absrel} position: [{x:.2f}, {y:.2f}, {z:.2f}]') + def handle_settings(self, *args): if self.stage: dlg = StageSettingsDialog(self.stage, self.jog_um, self.cjog_um) @@ -189,3 +202,29 @@ def halt(self): def stage_position_changed(self, stage, pos): self.update_coordinates() + + def update_calibration(self): + # search for and load calibration appropriate for the selected stage + cal_files = os.listdir(config['calibration_path']) + found_cal = None + for cf in sorted(cal_files, reverse=True): + m = re.match(f'(.*)-{self.stage.get_name()}-(\d\d\d\d-\d\d-\d\d)-(\d+)-(\d+)-(\d+).pkl', cf) + if m is None: + continue + found_cal = cf + break + if found_cal is None: + self.calibration = None + self.calibration_label.setText('(no calibration)') + else: + mg = m.groups() + with open(os.path.join(config['calibration_path'], found_cal), 'rb') as f: + self.calibration = pickle.load(f) + self.calibration_label.setText(f'calibrated {mg[1]} {mg[2]}:{mg[3]}:{mg[4]} for {mg[0]}') + + def move_to_selected(self): + if self.calibration is None: + raise Exception(f"No calibration loaded for {self.stage.get_name()}") + img_pt = self.model.get_image_point() + stage_pt = self.calibration.triangulate(img_pt) + self.move_to_point(stage_pt, relative=False) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 4c1c219e..0e9faab3 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -74,19 +74,17 @@ def __init__(self, model): self.setLineWidth(2) def triangulate(self): - cal_selected = self.selected_calibration() if cal_selected is None: self.msg_posted.emit('No calibration selected.') return - if None in (self.model.lcorr, self.model.rcorr): + corr_pt = self.model.get_image_point() + if corr_pt is None: self.msg_posted.emit('No correspondence points selected.') return - else: - lcorr, rcorr = self.model.lcorr, self.model.rcorr - obj_point = cal_selected.triangulate(lcorr, rcorr) + obj_point = cal_selected.triangulate(corr_pt) self.model.set_last_object_point(obj_point) x,y,z = obj_point diff --git a/parallax/model.py b/parallax/model.py index 453f7855..2d5a2da9 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -1,6 +1,7 @@ from PyQt5.QtCore import pyqtSignal from PyQt5.QtCore import QObject, pyqtSignal import numpy as np +import coorx import serial.tools.list_ports from mis_focus_controller import FocusController @@ -40,6 +41,10 @@ def get_camera(self, camera_name): def set_last_object_point(self, obj_point): self.obj_point_last = obj_point + def get_image_point(self): + concat = np.hstack([self.lcorr, self.rcorr]) + return coorx.Point(concat, f'{self.lcorr.system.name}+{self.rcorr.system.name}') + def add_calibration(self, cal): self.calibrations[cal.name] = cal From fd44984962a49866f09e356313b2eebe18052b17 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 23:26:31 -0800 Subject: [PATCH 29/62] update calibrations after generating a new one --- parallax/calibration.py | 24 +++++++++++++++++++++--- parallax/control_panel.py | 34 ++++++++++++++++++++-------------- parallax/geometry_panel.py | 14 ++++++-------- parallax/model.py | 25 +++++++++++++++++++++++++ 4 files changed, 72 insertions(+), 25 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index e1c8393d..e62e5787 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -1,4 +1,5 @@ import time +import pickle import numpy as np import cv2 as cv import coorx @@ -6,16 +7,29 @@ class Calibration: + + date_format = r"%Y-%m-%d-%H-%M-%S" + file_regex = r'([^-]+)-([^-]+)-((\d\d\d\d-\d\d-\d\d)-(\d+)-(\d+)-(\d+)).pkl' + + @staticmethod + def load(filename): + with open(filename, 'rb') as f: + return pickle.load(f) + def __init__(self, img_size): self.img_size = img_size self.img_points1 = [] self.img_points2 = [] self.obj_points = [] + def save(self, filename): + with open(filename, 'wb') as f: + pickle.dump(self, f) + @property def name(self): - date = time.strftime("%Y-%m-%d-%H-%M-%S", self.timestamp) - return f"{self.camera_names[0]}-{self.camera_names[1]}-{self.stage_name}-{date}" + date = time.strftime(self.date_format, self.timestamp) + return f"{self.from_cs}-{self.to_cs}-{date}" def add_points(self, img_pt1, img_pt2, obj_pt): self.img_points1.append(img_pt1) @@ -32,11 +46,15 @@ def calibrate(self): cam1 = self.img_points1[0].system.name cam2 = self.img_points2[0].system.name stage = self.obj_points[0].system.name + + self.from_cs = f"{cam1}+{cam2}" + self.to_cs = stage self.camera_names = (cam1, cam2) self.stage_name = stage + self.timestamp = time.localtime() - self.transform = StereoCameraTransform(from_cs=f"{cam1}+{cam2}", to_cs=stage) + self.transform = StereoCameraTransform(from_cs=self.from_cs, to_cs=self.to_cs) self.transform.set_mapping( np.array(self.img_points1), np.array(self.img_points2), diff --git a/parallax/control_panel.py b/parallax/control_panel.py index 50cccc12..d1a04b0a 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -1,4 +1,4 @@ -import os, re, pickle +import time from PyQt5.QtWidgets import QWidget, QLabel, QPushButton, QFrame from PyQt5.QtWidgets import QVBoxLayout, QGridLayout from PyQt5.QtCore import pyqtSignal, Qt @@ -8,6 +8,7 @@ from .helper import FONT_BOLD from .dialogs import StageSettingsDialog, TargetDialog from .stage_dropdown import StageDropdown +from .calibration import Calibration from .config import config JOG_UM_DEFAULT = 250 @@ -113,6 +114,8 @@ def __init__(self, model): self.jog_um = JOG_UM_DEFAULT self.cjog_um = CJOG_UM_DEFAULT + self.model.calibrations_changed.connect(self.update_calibration) + def update_coordinates(self, *args): xa, ya, za = self.stage.get_position() xo, yo, zo = self.stage.get_origin() @@ -204,23 +207,26 @@ def stage_position_changed(self, stage, pos): self.update_coordinates() def update_calibration(self): + if self.stage is None: + return + # search for and load calibration appropriate for the selected stage - cal_files = os.listdir(config['calibration_path']) - found_cal = None - for cf in sorted(cal_files, reverse=True): - m = re.match(f'(.*)-{self.stage.get_name()}-(\d\d\d\d-\d\d-\d\d)-(\d+)-(\d+)-(\d+).pkl', cf) - if m is None: - continue - found_cal = cf - break - if found_cal is None: + cals = self.model.list_calibrations() + cals = [cal for cal in cals if cal['to_cs'] == self.stage.get_name()] + cals = sorted(cals, key=lambda cal: cal['timestamp']) + + if len(cals) == 0: self.calibration = None self.calibration_label.setText('(no calibration)') else: - mg = m.groups() - with open(os.path.join(config['calibration_path'], found_cal), 'rb') as f: - self.calibration = pickle.load(f) - self.calibration_label.setText(f'calibrated {mg[1]} {mg[2]}:{mg[3]}:{mg[4]} for {mg[0]}') + cal_spec = cals[-1] + if 'calibration' in cal_spec: + cal = cal_spec['calibration'] + else: + cal = Calibration.load(cal_spec['file']) + self.calibration = cal + ts_str = time.strftime(r"%Y-%m-%d %H:%M:%S", cal.timestamp) + self.calibration_label.setText(f'calibrated {ts_str} for {cal.to_cs}') def move_to_selected(self): if self.calibration is None: diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 0e9faab3..db241a4f 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -1,5 +1,5 @@ -from PyQt5.QtWidgets import QGridLayout, QVBoxLayout, QHBoxLayout -from PyQt5.QtWidgets import QPushButton, QFrame, QWidget, QComboBox, QLabel +from PyQt5.QtWidgets import QGridLayout, QVBoxLayout +from PyQt5.QtWidgets import QPushButton, QFrame, QComboBox, QLabel from PyQt5.QtWidgets import QFileDialog from PyQt5.QtCore import pyqtSignal, Qt, QThread @@ -9,8 +9,8 @@ from .helper import FONT_BOLD from .dialogs import CalibrationDialog from .rigid_body_transform_tool import RigidBodyTransformTool, PointTransformWidget -from .calibration import Calibration from .calibration_worker import CalibrationWorker +from .calibration import Calibration from .config import config class GeometryPanel(QFrame): @@ -148,9 +148,8 @@ def load_cal(self): filename = QFileDialog.getOpenFileName(self, 'Load calibration file', config['calibration_path'], 'Pickle files (*.pkl)')[0] if filename: - with open(filename, 'rb') as f: - cal = pickle.load(f) - self.model.add_calibration(cal) + cal = Calibration.load(filename) + self.model.add_calibration(cal) self.update_cals() def save_cal(self): @@ -165,8 +164,7 @@ def save_cal(self): suggested_filename, 'Pickle files (*.pkl)')[0] if filename: - with open(filename, 'wb') as f: - pickle.dump(cal_selected, f) + cal_selected.save(filename) self.msg_posted.emit('Saved calibration %s to: %s' % (cal_selected.name, filename)) def update_cals(self): diff --git a/parallax/model.py b/parallax/model.py index 2d5a2da9..12ea0326 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -1,5 +1,6 @@ from PyQt5.QtCore import pyqtSignal from PyQt5.QtCore import QObject, pyqtSignal +import os, re, time import numpy as np import coorx import serial.tools.list_ports @@ -7,10 +8,13 @@ from .camera import list_cameras, close_cameras from .stage import list_stages, close_stages +from .config import config +from .calibration import Calibration class Model(QObject): msg_posted = pyqtSignal(str) + calibrations_changed = pyqtSignal() def __init__(self): QObject.__init__(self) @@ -47,6 +51,27 @@ def get_image_point(self): def add_calibration(self, cal): self.calibrations[cal.name] = cal + self.calibrations_changed.emit() + + def list_calibrations(self): + """List all known calibrations, including those loaded in memory and + those stored in a standard location / filename. + """ + cal_path = config['calibration_path'] + cal_files = os.listdir(cal_path) + calibrations = [] + for cf in sorted(cal_files, reverse=True): + m = re.match(Calibration.file_regex, cf) + if m is None: + continue + mg = m.groups() + ts = time.strptime(mg[2], Calibration.date_format) + calibrations.append({'file': os.path.join(cal_path, cf), 'from_cs': mg[0], 'to_cs': mg[1], 'timestamp': ts}) + for cal in self.calibrations.values(): + calibrations.append({'calibration': cal, 'from_cs': cal.from_cs, 'to_cs': cal.to_cs, 'timestamp': cal.timestamp}) + + assert len(calibrations) > 0 + return calibrations def set_calibration(self, calibration): self.calibration = calibration From 30f1557953fbd341784a0eb319ec3010547d6155 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Sun, 12 Feb 2023 23:59:42 -0800 Subject: [PATCH 30/62] Add extra move commands --- parallax/control_panel.py | 40 +++++++++++++++++++++++++++++++++++---- parallax/stage.py | 5 +++-- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index d1a04b0a..cfbbb7d9 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -3,6 +3,7 @@ from PyQt5.QtWidgets import QVBoxLayout, QGridLayout from PyQt5.QtCore import pyqtSignal, Qt from PyQt5.QtGui import QIcon +import pyqtgraph as pg import coorx from .helper import FONT_BOLD @@ -92,6 +93,15 @@ def __init__(self, model): self.move_selected_button = QPushButton('Move to Selected') self.move_selected_button.clicked.connect(self.move_to_selected) + self.approach_selected_button = QPushButton('Approach Selected') + self.approach_selected_button.clicked.connect(self.approach_selected) + self.approach_distance_spin = pg.SpinBox(value=3e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6) + + self.move_to_depth_button = QPushButton('Advance Depth') + self.move_to_depth_button.clicked.connect(self.move_to_depth) + self.depth_spin = pg.SpinBox(value=1e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6) + self.depth_speed_spin = pg.SpinBox(value=10e-6, suffix='m/s', siPrefix=True, bounds=[1e-6, 1e-3], dec=True, step=0.5, minStep=1e-6) + # layout main_layout = QGridLayout() main_layout.addWidget(self.main_label, 0,0, 1,3) @@ -104,6 +114,11 @@ def __init__(self, model): main_layout.addWidget(self.zero_button, 4,0, 1,3) main_layout.addWidget(self.move_target_button, 5,0, 1,1) main_layout.addWidget(self.move_selected_button, 5,1, 1,2) + main_layout.addWidget(self.approach_selected_button, 6,0, 1,1) + main_layout.addWidget(self.approach_distance_spin, 6,1, 1,1) + main_layout.addWidget(self.move_to_depth_button, 7,0, 1,1) + main_layout.addWidget(self.depth_spin, 7,1, 1,1) + main_layout.addWidget(self.depth_speed_spin, 7,2, 1,1) self.setLayout(main_layout) # frame border @@ -150,7 +165,7 @@ def move_to_target(self, *args): self.update_coordinates() self.target_reached.emit() - def move_to_point(self, pt, relative): + def move_to_point(self, pt, relative, **kwds): if isinstance(pt, coorx.Point): if pt.system.name != self.stage.get_name(): raise Exception(f"Not moving stage {self.stage.get_name()} to coordinate in system {pt.system.name}") @@ -158,7 +173,7 @@ def move_to_point(self, pt, relative): raise Exception(f"Not moving to relative point in system {pt.system.name}") x, y, z = pt - self.stage.move_to_target_3d(x, y, z, relative=relative, safe=True) + self.stage.move_to_target_3d(x, y, z, relative=relative, safe=True, **kwds) absrel = "relative" if relative else "absolute" self.msg_posted.emit(f'Moved to {absrel} position: [{x:.2f}, {y:.2f}, {z:.2f}]') @@ -228,9 +243,26 @@ def update_calibration(self): ts_str = time.strftime(r"%Y-%m-%d %H:%M:%S", cal.timestamp) self.calibration_label.setText(f'calibrated {ts_str} for {cal.to_cs}') - def move_to_selected(self): + def get_stage_point(self): if self.calibration is None: raise Exception(f"No calibration loaded for {self.stage.get_name()}") img_pt = self.model.get_image_point() - stage_pt = self.calibration.triangulate(img_pt) + return self.calibration.triangulate(img_pt) + + def move_to_selected(self): + stage_pt = self.get_stage_point() self.move_to_point(stage_pt, relative=False) + + def approach_selected(self): + stage_pt = self.get_stage_point() + depth = self.approach_distance_spin.value() * 1e6 + stage_pt.coordinates[2] += depth + self.move_to_point(stage_pt, relative=False, block=False) + + def move_to_depth(self): + pos = self.stage.get_position() + depth = self.depth_spin.value() * 1e6 + speed = self.depth_speed_spin.value() * 1e6 + pos.coordinates[2] -= depth + self.move_to_point(pos, relative=False, speed=speed, block=False) + diff --git a/parallax/stage.py b/parallax/stage.py index e8bd6c1a..271a3da5 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -188,14 +188,15 @@ def get_accel(self): def get_position(self): return Point(self.pos.copy(), self.get_name()) - def move_to_target_3d(self, x, y, z, relative=False, safe=False, block=True): + def move_to_target_3d(self, x, y, z, relative=False, safe=False, speed=None, block=True): if relative: xo,yo,zo = self.get_origin() x += xo y += yo z += zo - move_cmd = MoveFuture(self, pos=np.array([x, y, z]), speed=self.speed, accel=self.accel) + speed = speed or self.speed + move_cmd = MoveFuture(self, pos=np.array([x, y, z]), speed=speed, accel=self.accel) self.move_queue.put(move_cmd) if block: move_cmd.wait() From e780f011a1a3b6f083db540344d317c24a813303 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 00:20:18 -0800 Subject: [PATCH 31/62] fix mock stage speed --- parallax/control_panel.py | 8 ++++---- parallax/stage.py | 22 +++++++++++++++------- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index cfbbb7d9..9c7365ea 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -95,12 +95,12 @@ def __init__(self, model): self.approach_selected_button = QPushButton('Approach Selected') self.approach_selected_button.clicked.connect(self.approach_selected) - self.approach_distance_spin = pg.SpinBox(value=3e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6) + self.approach_distance_spin = pg.SpinBox(value=3e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6, compactHeight=False) self.move_to_depth_button = QPushButton('Advance Depth') self.move_to_depth_button.clicked.connect(self.move_to_depth) - self.depth_spin = pg.SpinBox(value=1e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6) - self.depth_speed_spin = pg.SpinBox(value=10e-6, suffix='m/s', siPrefix=True, bounds=[1e-6, 1e-3], dec=True, step=0.5, minStep=1e-6) + self.depth_spin = pg.SpinBox(value=1e-3, suffix='m', siPrefix=True, bounds=[1e-6, 20e-3], dec=True, step=0.5, minStep=1e-6, compactHeight=False) + self.depth_speed_spin = pg.SpinBox(value=10e-6, suffix='m/s', siPrefix=True, bounds=[1e-6, 1e-3], dec=True, step=0.5, minStep=1e-6, compactHeight=False) # layout main_layout = QGridLayout() @@ -251,7 +251,7 @@ def get_stage_point(self): def move_to_selected(self): stage_pt = self.get_stage_point() - self.move_to_point(stage_pt, relative=False) + self.move_to_point(stage_pt, relative=False, block=False) def approach_selected(self): stage_pt = self.get_stage_point() diff --git a/parallax/stage.py b/parallax/stage.py index 271a3da5..42fed722 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -208,6 +208,9 @@ def move_distance_1d(self, axis, distance): pos[ax_ind] += distance return self.move_to_target_3d(*pos) + def halt(self): + self.move_queue.put('halt') + def update_pos(self, pos): # stage has moved; update and emit self.pos = pos @@ -223,23 +226,28 @@ def update_pos(self, pos): def thread_loop(self): current_move = None while True: - try: - next_move = self.move_queue.get(block=False) - except queue.Empty: - next_move = None + next_move = None + while not self.move_queue.empty(): + next_move = self.move_queue.get() if next_move is not None: if current_move is not None: - current_move.finish(interrupted=True, message="interrupted by another move request") + if next_move == 'halt': + message = 'interrupted by halt request' + else: + message = "interrupted by another move request" + current_move.finish(interrupted=True, message=message) + if next_move == 'halt': + next_move = None current_move = next_move last_update = time.perf_counter() if current_move is not None: now = time.perf_counter() - dt = now = last_update + dt = now - last_update last_update = now - pos = self.get_position() + pos = self.get_position().coordinates target = current_move.pos dx = target - pos dist_to_go = np.linalg.norm(dx) From b67fb6bb086cc596e4746f74f435d773d69b7e55 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 01:11:30 -0800 Subject: [PATCH 32/62] Add button for copying selected point --- parallax/control_panel.py | 25 ++++++++++++++++++++++--- parallax/dialogs.py | 4 +--- parallax/main_window.py | 1 + parallax/model.py | 4 ++++ parallax/stage.py | 2 +- 5 files changed, 29 insertions(+), 7 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index 9c7365ea..a2cf3d04 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -2,7 +2,7 @@ from PyQt5.QtWidgets import QWidget, QLabel, QPushButton, QFrame from PyQt5.QtWidgets import QVBoxLayout, QGridLayout from PyQt5.QtCore import pyqtSignal, Qt -from PyQt5.QtGui import QIcon +from PyQt5.QtGui import QIcon, QGuiApplication import pyqtgraph as pg import coorx @@ -58,7 +58,7 @@ class ControlPanel(QFrame): def __init__(self, model): QFrame.__init__(self) self.model = model - + self.calibration = None # widgets self.main_label = QLabel('Stage Control') @@ -73,6 +73,8 @@ def __init__(self, model): self.settings_button.clicked.connect(self.handle_settings) self.calibration_label = QLabel("") + self.cal_pt_btn = QPushButton('') + self.cal_pt_btn.clicked.connect(self.copy_cal_pt) self.xcontrol = AxisControl('x') self.xcontrol.jog_requested.connect(self.jog) @@ -107,7 +109,8 @@ def __init__(self, model): main_layout.addWidget(self.main_label, 0,0, 1,3) main_layout.addWidget(self.dropdown, 1,0, 1,2) main_layout.addWidget(self.settings_button, 1,2, 1,1) - main_layout.addWidget(self.calibration_label, 2,0, 1,3) + main_layout.addWidget(self.calibration_label, 2,0, 1,2) + main_layout.addWidget(self.cal_pt_btn, 2,2, 1,1) main_layout.addWidget(self.xcontrol, 3,0, 1,1) main_layout.addWidget(self.ycontrol, 3,1, 1,1) main_layout.addWidget(self.zcontrol, 3,2, 1,1) @@ -130,6 +133,7 @@ def __init__(self, model): self.cjog_um = CJOG_UM_DEFAULT self.model.calibrations_changed.connect(self.update_calibration) + self.model.corr_pts_changed.connect(self.update_cal_pt) def update_coordinates(self, *args): xa, ya, za = self.stage.get_position() @@ -244,9 +248,13 @@ def update_calibration(self): self.calibration_label.setText(f'calibrated {ts_str} for {cal.to_cs}') def get_stage_point(self): + if self.stage is None: + raise Exception("No stage selected") if self.calibration is None: raise Exception(f"No calibration loaded for {self.stage.get_name()}") img_pt = self.model.get_image_point() + if img_pt is None: + raise Exception("No correspondence points selected") return self.calibration.triangulate(img_pt) def move_to_selected(self): @@ -266,3 +274,14 @@ def move_to_depth(self): pos.coordinates[2] -= depth self.move_to_point(pos, relative=False, speed=speed, block=False) + def update_cal_pt(self): + try: + stage_pt = self.get_stage_point() + x = stage_pt.coordinates + self.cal_pt_btn.setText(f'{x[0]:0.0f}, {x[1]:0.0f}, {x[2]:0.0f}') + except Exception: + self.cal_pt_btn.setText('') + + def copy_cal_pt(self): + cb = QGuiApplication.clipboard() + cb.setText(self.cal_pt_btn.text()) diff --git a/parallax/dialogs.py b/parallax/dialogs.py index b76a2644..b56071bd 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -1,12 +1,10 @@ -from PyQt5.QtWidgets import QPushButton, QLabel, QRadioButton, QSpinBox +from PyQt5.QtWidgets import QPushButton, QLabel, QSpinBox from PyQt5.QtWidgets import QGridLayout from PyQt5.QtWidgets import QDialog, QLineEdit, QDialogButtonBox from PyQt5.QtCore import Qt from PyQt5.QtGui import QDoubleValidator import numpy as np -import time -import datetime from .toggle_switch import ToggleSwitch from .helper import FONT_BOLD diff --git a/parallax/main_window.py b/parallax/main_window.py index 409e505b..41d07376 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -185,6 +185,7 @@ def refresh(self): def clear_selected(self): for screen in self.screens: screen.clear_selected() + self.model.set_correspondence_points((None, None)) def zoom_out(self): for screen in self.screens: diff --git a/parallax/model.py b/parallax/model.py index 12ea0326..27b0e071 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -15,6 +15,7 @@ class Model(QObject): msg_posted = pyqtSignal(str) calibrations_changed = pyqtSignal() + corr_pts_changed = pyqtSignal() def __init__(self): QObject.__init__(self) @@ -46,6 +47,8 @@ def set_last_object_point(self, obj_point): self.obj_point_last = obj_point def get_image_point(self): + if None in (self.lcorr, self.rcorr): + return None concat = np.hstack([self.lcorr, self.rcorr]) return coorx.Point(concat, f'{self.lcorr.system.name}+{self.rcorr.system.name}') @@ -79,6 +82,7 @@ def set_calibration(self, calibration): def set_correspondence_points(self, pts): self.lcorr = pts[0] self.rcorr = pts[1] + self.corr_pts_changed.emit() def scan_for_cameras(self): self.cameras = list_cameras() diff --git a/parallax/stage.py b/parallax/stage.py index 42fed722..fb9d80e5 100755 --- a/parallax/stage.py +++ b/parallax/stage.py @@ -148,7 +148,7 @@ def __init__(self, transform): super().__init__() self.base_transform = transform self.transform = coorx.AffineTransform(dims=(3, 3)) - self.speed = 1000 # um/s + self.speed = 8000 # um/s self.accel = 5000 # um/s^2 self.pos = np.array([0, 0, 0]) self.name = f"mock_stage_{MockStage.n_mock_stages}" From 948cf914d8c3396e00c64a3609a336d897c46e98 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 02:45:01 -0800 Subject: [PATCH 33/62] make mock sim cmera grab threada-safe --- parallax/mock_sim.py | 10 +- parallax/threadrun.py | 340 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 346 insertions(+), 4 deletions(-) create mode 100644 parallax/threadrun.py diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index bf62ada0..f7ed2d7d 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -2,9 +2,10 @@ import cv2 as cv import numpy as np import coorx -from parallax.calibration import CameraTransform, StereoCameraTransform -from parallax.lib import find_checker_corners -from parallax.config import config +from .calibration import CameraTransform, StereoCameraTransform +from .lib import find_checker_corners +from .config import config +from .threadrun import runInGuiThread class CameraTransform(coorx.CompositeTransform): @@ -254,7 +255,8 @@ def item_changed(self, item): def get_array(self): if self.cached_frame is None: self.prepare_for_paint.emit() - self.cached_frame = pg.imageToArray(pg.QtGui.QImage(self.grab()), copy=True, transpose=False)[..., :3] + img_arr = runInGuiThread(self.grab) + self.cached_frame = pg.imageToArray(pg.QtGui.QImage(img_arr), copy=True, transpose=False)[..., :3] return self.cached_frame def update(self): diff --git a/parallax/threadrun.py b/parallax/threadrun.py new file mode 100644 index 00000000..4dfb0ae7 --- /dev/null +++ b/parallax/threadrun.py @@ -0,0 +1,340 @@ +"""Borrowed from ACQ4 +""" +import time, sys, threading, traceback, functools + +from PyQt5 import QtCore, QtWidgets + + +def runInThread(thread, func, *args, **kwds): + """Run a function in another thread and return the result. + + The remote thread must be running a Qt event loop. + """ + return ThreadCallFuture(thread, func, *args, **kwds)() + + +def runInGuiThread(func, *args, **kwds): + """Run a function the main GUI thread and return the result. + """ + isGuiThread = QtCore.QThread.currentThread() == QtCore.QCoreApplication.instance().thread() + if isGuiThread: + return func(*args, **kwds) + return ThreadCallFuture(None, func, *args, **kwds)() + + +class Future(QtCore.QObject): + """Used to track the progress of an asynchronous task. + + The simplest subclasses reimplement percentDone() and call _taskDone() when finished. + """ + sigFinished = QtCore.pyqtSignal(object) # self + sigStateChanged = QtCore.pyqtSignal(object, object) # self, state + + class StopRequested(Exception): + """Raised by _checkStop if stop() has been invoked. + """ + + class Timeout(Exception): + """Raised by wait() if the timeout period elapses. + """ + + def __init__(self): + QtCore.QObject.__init__(self) + + self.startTime = time.perf_counter() + + self._isDone = False + self._wasInterrupted = False + self._errorMessage = None + self._excInfo = None + self._stopRequested = False + self._state = 'starting' + self._errorMonitorThread = None + self.finishedEvent = threading.Event() + + def currentState(self): + """Return the current state of this future. + + The state can be any string used to indicate the progress this future is making in its task. + """ + return self._state + + def setState(self, state): + """Set the current state of this future. + + The state can be any string used to indicate the progress this future is making in its task. + """ + if state == self._state: + return + self._state = state + self.sigStateChanged.emit(self, state) + + def percentDone(self): + """Return the percent of the task that has completed. + + Must be reimplemented in subclasses. + """ + raise NotImplementedError("method must be reimplmented in subclass") + + def stop(self, reason="task stop requested"): + """Stop the task (nicely). + + This method may return another future if stopping the task is expected to + take time. + + Subclasses may extend this method and/or use _checkStop to determine whether + stop() has been called. + """ + if self.isDone(): + return + + if reason is not None: + self._errorMessage = reason + self._stopRequested = True + + def _taskDone(self, interrupted=False, error=None, state=None, excInfo=None): + """Called by subclasses when the task is done (regardless of the reason) + """ + if self._isDone: + raise Exception("_taskDone has already been called.") + self._isDone = True + if error is not None: + # error message may have been set earlier + self._errorMessage = error + self._excInfo = excInfo + self._wasInterrupted = interrupted + if interrupted: + self.setState(state or 'interrupted: %s' % error) + else: + self.setState(state or 'complete') + self.finishedEvent.set() + self.sigFinished.emit(self) + + def wasInterrupted(self): + """Return True if the task was interrupted before completing (due to an error or a stop request). + """ + return self._wasInterrupted + + def isDone(self): + """Return True if the task has completed successfully or was interrupted. + """ + return self._isDone + + def errorMessage(self): + """Return a string description of the reason for a task failure, + or None if there was no failure (or if the reason is unknown). + """ + return self._errorMessage + + def wait(self, timeout=None, updates=False, pollInterval=0.1): + """Block until the task has completed, has been interrupted, or the + specified timeout has elapsed. + + If *updates* is True, process Qt events while waiting. + + If a timeout is specified and the task takes too long, then raise Future.Timeout. + If the task ends incomplete for another reason, then raise RuntimeError. + """ + start = time.perf_counter() + while True: + if (timeout is not None) and (time.perf_counter() > start + timeout): + raise self.Timeout("Timeout waiting for task to complete.") + + if self.isDone(): + break + + if updates is True: + Qt.QTest.qWait(min(1, int(pollInterval * 1000))) + else: + self._wait(pollInterval) + + if self.wasInterrupted(): + err = self.errorMessage() + if err is None: + # This would be a fantastic place to "raise from self._excInfo[1]" once we move to py3 + raise RuntimeError(f"Task {self} did not complete (no error message).") + else: + raise RuntimeError(f"Task {self} did not complete: {err}") + + def _wait(self, duration): + """Default sleep implementation used by wait(); may be overridden to return early. + """ + self.finishedEvent.wait(timeout=duration) + + def _checkStop(self, delay=0): + """Raise self.StopRequested if self.stop() has been called. + + This may be used by subclasses to periodically check for stop requests. + + The optional *delay* argument causes this method to sleep while periodically + checking for a stop request. + """ + if delay == 0 and self._stopRequested: + raise self.StopRequested() + + stop = time.perf_counter() + delay + while True: + now = time.perf_counter() + if now > stop: + return + + time.sleep(max(0, min(0.1, stop-now))) + if self._stopRequested: + raise self.StopRequested() + + def sleep(self, duration, interval=0.2): + """Sleep for the specified duration (in seconds) while checking for stop requests. + """ + start = time.time() + while time.time() < start + duration: + self._checkStop() + time.sleep(interval) + + def waitFor(self, futures, timeout=20.0): + """Wait for multiple futures to complete while also checking for stop requests on self. + """ + if not isinstance(futures, (list, tuple)): + futures = [futures] + if len(futures) == 0: + return + start = time.time() + while True: + self._checkStop() + allDone = True + for fut in futures[:]: + try: + fut.wait(0.1) + futures.remove(fut) + except fut.Timeout: + allDone = False + break + if allDone: + break + if timeout is not None and time.time() - start > timeout: + raise futures[0].Timeout("Timed out waiting for %r" % futures) + + def raiseErrors(self, message, pollInterval=1.0): + """Monitor this future for errors and raise if any occur. + + This allows the caller to discard a future, but still expect errors to be delivered to the user. Note + that errors are raised from a background thread. + + Parameters + ---------- + message : str + Exception message to raise. May include "{stack}" to insert the stack trace of the caller, and "{error}" + to insert the original formatted exception. + pollInterval : float | None + Interval in seconds to poll for errors. This is only used with Futures that require a poller; + Futures that immediately report errors when they occur will not use a poller. + """ + if self._errorMonitorThread is not None: + return + originalFrame = sys._getframe().f_back + monitorFn = functools.partial(self._monitorErrors, message=message, pollInterval=pollInterval, originalFrame=originalFrame) + self._errorMonitorThread = threading.Thread(target=monitorFn, daemon=True) + self._errorMonitorThread.start() + + def _monitorErrors(self, message, pollInterval, originalFrame): + try: + self.wait(pollInterval=pollInterval) + except Exception as exc: + if '{stack}' in message: + stack = ''.join(traceback.format_stack(originalFrame)) + else: + stack = None + + try: + formattedMsg = message.format(stack=stack, error=traceback.format_exception_only(type(exc), exc)) + except Exception as exc2: + formattedMsg = message + f" [additional error formatting error message: {exc2}]" + raise RuntimeError(formattedMsg) from exc + + +class ThreadCallFuture(Future): + sigRequestCall = QtCore.pyqtSignal() + def __init__(self, thread, func, *args, **kwds): + Future.__init__(self) + self.func = func + self.args = args + self.kwds = kwds + self.exc = None + + if thread is None: + thread = QtWidgets.QApplication.instance().thread() + self.moveToThread(thread) + self.sigRequestCall.connect(self._callRequested) + self.sigRequestCall.emit() + + def _callRequested(self): + try: + self.ret = self.func(*self.args, **self.kwds) + self._taskDone() + except Exception as exc: + self.exc = exc + err = ''.join(traceback.format_exception(*sys.exc_info())) + self._taskDone(interrupted=True, error=err) + + def __call__(self): + self.wait() + if self.exc is not None: + raise self.exc + else: + return self.ret + + + + + +class _FuturePollThread(threading.Thread): + """Thread used to poll the state of a future. + + Used when a Future subclass does not automatically call _taskDone, but instead requires + a periodic check. May + """ + def __init__(self, future, pollInterval, originalFrame): + threading.Thread.__init__(self, daemon=True) + self.future = future + self.pollInterval = pollInterval + self._stop = False + + def run(self): + while not self._stop: + if self.future.isDone(): + break + if self.future._raiseErrors: + raise + time.sleep(self.pollInterval) + + def stop(self): + self._stop = True + self.join() + + +class MultiFuture(Future): + """Future tracking progress of multiple sub-futures. + """ + def __init__(self, futures): + self.futures = futures + Future.__init__(self) + + def stop(self, reason="task stop requested"): + for f in self.futures: + f.stop(reason=reason) + return Future.stop(self, reason) + + def percentDone(self): + return min([f.percentDone() for f in self.futures]) + + def wasInterrupted(self): + return any([f.wasInterrupted() for f in self.futures]) + + def isDone(self): + return all([f.isDone() for f in self.futures]) + + def errorMessage(self): + return "; ".join([f.errorMessage() or '' for f in self.futures]) + + def currentState(self): + return "; ".join([f.currentState() or '' for f in self.futures]) + From 762dba2e7ad1bb3f1d244df3a3a8dafb81a81801 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 02:45:47 -0800 Subject: [PATCH 34/62] easier way to get calibrations per stage --- parallax/control_panel.py | 14 +++----------- parallax/model.py | 20 ++++++++++++++++++++ 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index a2cf3d04..dc0a67c6 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -230,20 +230,12 @@ def update_calibration(self): return # search for and load calibration appropriate for the selected stage - cals = self.model.list_calibrations() - cals = [cal for cal in cals if cal['to_cs'] == self.stage.get_name()] - cals = sorted(cals, key=lambda cal: cal['timestamp']) + cal = self.model.get_calibration(self.stage) + self.calibration = cal - if len(cals) == 0: - self.calibration = None + if cal is None: self.calibration_label.setText('(no calibration)') else: - cal_spec = cals[-1] - if 'calibration' in cal_spec: - cal = cal_spec['calibration'] - else: - cal = Calibration.load(cal_spec['file']) - self.calibration = cal ts_str = time.strftime(r"%Y-%m-%d %H:%M:%S", cal.timestamp) self.calibration_label.setText(f'calibrated {ts_str} for {cal.to_cs}') diff --git a/parallax/model.py b/parallax/model.py index 27b0e071..4936684e 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -17,8 +17,11 @@ class Model(QObject): calibrations_changed = pyqtSignal() corr_pts_changed = pyqtSignal() + instance = None + def __init__(self): QObject.__init__(self) + Model.instance = self self.cameras = [] self.focos = [] @@ -76,6 +79,23 @@ def list_calibrations(self): assert len(calibrations) > 0 return calibrations + def get_calibration(self, stage): + """Return the most recent calibration known for this stage + """ + cals = self.list_calibrations() + cals = [cal for cal in cals if cal['to_cs'] == stage.get_name()] + cals = sorted(cals, key=lambda cal: cal['timestamp']) + + if len(cals) == 0: + return None + else: + cal_spec = cals[-1] + if 'calibration' in cal_spec: + cal = cal_spec['calibration'] + else: + cal = Calibration.load(cal_spec['file']) + return cal + def set_calibration(self, calibration): self.calibration = calibration From 0e0461e3be9ed581542cfd10ea040de019ca9613 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 02:46:17 -0800 Subject: [PATCH 35/62] Add tool for collecting training data --- parallax/camera.py | 8 ++++- parallax/dialogs.py | 60 ++++++++++++++++++++++++++++++- parallax/main_window.py | 12 ++++++- parallax/training_data.py | 74 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 151 insertions(+), 3 deletions(-) create mode 100644 parallax/training_data.py diff --git a/parallax/camera.py b/parallax/camera.py index 54a16267..23a07f1a 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -3,6 +3,7 @@ import threading import numpy as np import logging +import pyqtgraph as pg from .mock_sim import MockSim logger = logging.getLogger(__name__) @@ -201,8 +202,13 @@ def get_last_image_data(self): return image + def save_last_image(self, filename): + arr = self.get_last_image_data()[..., [2,1,0]] + img = pg.makeQImage(arr, alpha=False, transpose=False) + img.save(filename) + @property def camera_tr(self): """Transform mapping from simulated global coordinate system to camera image pixels. """ - return self.sim.cameras[self]['view'].camera_tr \ No newline at end of file + return self.sim.cameras[self]['view'].camera_tr diff --git a/parallax/dialogs.py b/parallax/dialogs.py index b56071bd..6a5e8df9 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -3,7 +3,7 @@ from PyQt5.QtWidgets import QDialog, QLineEdit, QDialogButtonBox from PyQt5.QtCore import Qt from PyQt5.QtGui import QDoubleValidator - +import pyqtgraph as pg import numpy as np from .toggle_switch import ToggleSwitch @@ -351,3 +351,61 @@ def get_params(self): y = float(self.yedit.text()) z = float(self.zedit.text()) return x,y,z + + +class TrainingDataDialog(QDialog): + + def __init__(self, model): + QDialog.__init__(self) + self.model = model + + self.setWindowTitle('Training Data Generator') + + self.stage_label = QLabel('Select a Stage:') + self.stage_label.setAlignment(Qt.AlignCenter) + self.stage_label.setFont(FONT_BOLD) + + self.stage_dropdown = StageDropdown(self.model) + self.stage_dropdown.activated.connect(self.update_status) + + self.img_count_label = QLabel('Image Count:') + self.img_count_label.setAlignment(Qt.AlignCenter) + self.img_count_box = QSpinBox() + self.img_count_box.setMinimum(1) + self.img_count_box.setValue(100) + + self.extent_label = QLabel('Extent:') + self.extent_label.setAlignment(Qt.AlignCenter) + self.extent_spin = pg.SpinBox(value=4e-3, suffix='m', siPrefix=True, bounds=[0.1e-3, 20e-3], dec=True, step=0.5, minStep=1e-6, compactHeight=False) + + self.go_button = QPushButton('Start Data Collection') + self.go_button.setEnabled(False) + self.go_button.clicked.connect(self.go) + + layout = QGridLayout() + layout.addWidget(self.stage_label, 0,0, 1,1) + layout.addWidget(self.stage_dropdown, 0,1, 1,1) + layout.addWidget(self.img_count_label, 1,0, 1,1) + layout.addWidget(self.img_count_box, 1,1, 1,1) + layout.addWidget(self.extent_label, 2,0, 1,1) + layout.addWidget(self.extent_spin, 2,1, 1,1) + layout.addWidget(self.go_button, 4,0, 1,2) + self.setLayout(layout) + + self.setMinimumWidth(300) + + def get_stage(self): + return self.stage_dropdown.current_stage() + + def get_img_count(self): + return self.img_count_box.value() + + def get_extent(self): + return self.extent_spin.value() * 1e6 + + def go(self): + self.accept() + + def update_status(self): + if self.stage_dropdown.is_selected(): + self.go_button.setEnabled(True) diff --git a/parallax/main_window.py b/parallax/main_window.py index 41d07376..63627753 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -1,4 +1,4 @@ -from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter +from PyQt5.QtWidgets import QApplication, QWidget, QHBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QIcon import pyqtgraph.console @@ -11,6 +11,7 @@ from .rigid_body_transform_tool import RigidBodyTransformTool from .stage_manager import StageManager from .config import config +from .training_data import TrainingDataCollector class MainWindow(QMainWindow): @@ -22,6 +23,8 @@ def __init__(self, model): # allow main window to be accessed globally model.main_window = self + self.data_collector = None + self.widget = MainWidget(model) self.setCentralWidget(self.widget) self.resize(1280, 900) @@ -42,6 +45,8 @@ def __init__(self, model): self.rbt_action.triggered.connect(self.launch_rbt) self.console_action = QAction("Python Console") self.console_action.triggered.connect(self.show_console) + self.training_data_action = QAction("Collect Training Data") + self.training_data_action.triggered.connect(self.collect_training_data) self.about_action = QAction("About") self.about_action.triggered.connect(self.launch_about) @@ -61,6 +66,7 @@ def __init__(self, model): self.tools_menu = self.menuBar().addMenu("Tools") self.tools_menu.addAction(self.rbt_action) self.tools_menu.addAction(self.console_action) + self.tools_menu.addAction(self.training_data_action) self.help_menu = self.menuBar().addMenu("Help") self.help_menu.addAction(self.about_action) @@ -116,6 +122,10 @@ def refresh_focus_controllers(self): for screen in self.screens(): screen.update_focus_control_menu() + def collect_training_data(self): + self.data_collector = TrainingDataCollector(self.model) + self.data_collector.start() + class MainWidget(QSplitter): def __init__(self, model): diff --git a/parallax/training_data.py b/parallax/training_data.py new file mode 100644 index 00000000..86c6a2ee --- /dev/null +++ b/parallax/training_data.py @@ -0,0 +1,74 @@ +import threading, pickle, os +import numpy as np +from PyQt5 import QtWidgets, QtCore +from .dialogs import TrainingDataDialog + + +class TrainingDataCollector(QtCore.QObject): + def __init__(self, model): + QtCore.QObject.__init__(self) + self.model = model + + def start(self): + dlg = TrainingDataDialog(self.model) + dlg.exec_() + + self.stage = dlg.get_stage() + self.img_count = dlg.get_img_count() + self.extent = dlg.get_extent() + self.path = QtWidgets.QFileDialog.getExistingDirectory(parent=None, caption="Select Storage Directory") + + self.start_pos = self.stage.get_position() + self.stage_cal = self.model.get_calibration(self.stage) + + self.thread = threading.Thread(target=self.thread_run, daemon=True) + self.thread.start() + + def thread_run(self): + meta_file = os.path.join(self.path, 'meta.pkl') + if os.path.exists(meta_file): + # todo: just append + raise Exception("Already data in this folder!") + trials = [] + meta = { + 'calibration': self.stage_cal, + 'stage': self.stage.get_name(), + 'trials': trials, + } + + # move electrode out of fov for background images + pos = self.start_pos.coordinates.copy() + pos[2] += 10000 + self.stage.move_to_target_3d(*pos, block=True) + imgs = self.save_images('background') + meta['background'] = imgs + + for i in range(self.img_count): + + # first image in random location + rnd = np.random.uniform(-self.extent/2, self.extent/2, size=3) + pos1 = self.start_pos.coordinates + rnd + self.stage.move_to_target_3d(*pos1, block=True) + images1 = self.save_images(f'{i:04d}-a') + + # take a second image slightly shifted + pos2 = pos1.copy() + pos2[2] += 10 + self.stage.move_to_target_3d(*pos2, block=True) + images2 = self.save_images(f'{i:04d}-b') + + trials.append([ + {'pos': pos1, 'images': images1}, + {'pos': pos2, 'images': images2}, + ]) + + with open(meta_file, 'wb') as fh: + pickle.dump(meta, fh) + + def save_images(self, name): + images = [] + for camera in self.model.cameras: + filename = f'{name}-{camera.name()}.png' + camera.save_last_image(os.path.join(self.path, filename)) + images.append({'camera': camera.name(), 'image': filename}) + return images \ No newline at end of file From 77cb9a4f675b8edca21397a7690a707da28ad3bb Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 16:47:51 -0800 Subject: [PATCH 36/62] bail out of training data collection if user cancels --- parallax/training_data.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/parallax/training_data.py b/parallax/training_data.py index 86c6a2ee..7da78430 100644 --- a/parallax/training_data.py +++ b/parallax/training_data.py @@ -12,11 +12,15 @@ def __init__(self, model): def start(self): dlg = TrainingDataDialog(self.model) dlg.exec_() + if dlg.result() != dlg.Accepted: + return self.stage = dlg.get_stage() self.img_count = dlg.get_img_count() self.extent = dlg.get_extent() self.path = QtWidgets.QFileDialog.getExistingDirectory(parent=None, caption="Select Storage Directory") + if self.path == '': + return self.start_pos = self.stage.get_position() self.stage_cal = self.model.get_calibration(self.stage) From 70bc5c05443b88ee652e568f07a682ac6cad372b Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 16:48:42 -0800 Subject: [PATCH 37/62] remove notes file --- zz_notes | 66 -------------------------------------------------------- 1 file changed, 66 deletions(-) delete mode 100644 zz_notes diff --git a/zz_notes b/zz_notes deleted file mode 100644 index 3d33f83b..00000000 --- a/zz_notes +++ /dev/null @@ -1,66 +0,0 @@ --Package maintenance / code quality: - - Standardize package structure - root code folder named parallax - setup.py - relative imports - - camelCase vs snake_case? - I recommend snake_case (regret using camelCase for acq4) - - Unit tests? - don't put tests in __main__ - - Continuous integration - - Avoid `from X import *` - - lib and Helper have some overlap (getIntrinsicsFromChecherboard, DLT) - - rename / reorg lib and Helper - -- Use QtPy? - This wraps multiple python-Qt libraries so that we can change versions more easily - Otherwise we will need to transition to Qt6 in the near future - -- Add error display / logging facilities - Users will appreciate this - We want good records kept in case the software destroys an electrode / ruins an experiment - -- Add console for realtime debugging - -- Simple 3D rendering of electrodes in MockCamera - so we can test triangulation / calibration - -- Put camera polling in a background thread - I assume it takes some time to request a frame from the camera, - and that the GIL is unlocked during this time - -- Do we want to depend on pyspin for camera access, or write a thin layer to allow adapting other sources later? - Possibly acq4 camera devices? - -- Device configuration system - specify what cameras / stages to look for - any relevant physical details that could be modified in the future and require user config - -- Coorx for transforms - np arrays as standard coordinate object? - coordinate system-aware points? - -- parallax : project name collisions (python packages, neural dsp, .. ) - -- What features may eventually be added? - - Other device models (cameras, stages) - - Other device types (focal / widefield photostim? indicator imaging? behavioral? others?) - - Automatic electrode calibration - - Animal data tracking (pull MRI and desired electrode coordinates from server) - - Automatic electrode delivery - -- Things we might import from acq4: - stage device infrastructure - camera device infrastructure - device management - imaging pipeline - error logging - benefits: - device abstractions to allow extensibility later (as opposed to pyspin) - shared codebase with other allen institute projects - device transform graph - pre-optimized imaging pipeline - device configuration system - drawbacks: - lots of baggage, but perhaps this can be well hidden - From a7e26238ebf5b460bc7f438ab5ec80c53a2ad415 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 16:56:58 -0800 Subject: [PATCH 38/62] Add training data collector --- parallax/dialogs.py | 64 ++++++++++++++++++++++++++++++-- parallax/main_window.py | 14 ++++++- parallax/training_data.py | 78 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 151 insertions(+), 5 deletions(-) create mode 100644 parallax/training_data.py diff --git a/parallax/dialogs.py b/parallax/dialogs.py index 176fcba8..97fe678c 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -1,12 +1,10 @@ -from PyQt5.QtWidgets import QPushButton, QLabel, QRadioButton, QSpinBox +from PyQt5.QtWidgets import QPushButton, QLabel, QSpinBox from PyQt5.QtWidgets import QGridLayout from PyQt5.QtWidgets import QDialog, QLineEdit, QDialogButtonBox from PyQt5.QtCore import Qt from PyQt5.QtGui import QDoubleValidator - +import pyqtgraph as pg import numpy as np -import time -import datetime from .toggle_switch import ToggleSwitch from .helper import FONT_BOLD @@ -343,3 +341,61 @@ def get_params(self): y = float(self.yedit.text()) z = float(self.zedit.text()) return x,y,z + + +class TrainingDataDialog(QDialog): + + def __init__(self, model): + QDialog.__init__(self) + self.model = model + + self.setWindowTitle('Training Data Generator') + + self.stage_label = QLabel('Select a Stage:') + self.stage_label.setAlignment(Qt.AlignCenter) + self.stage_label.setFont(FONT_BOLD) + + self.stage_dropdown = StageDropdown(self.model) + self.stage_dropdown.activated.connect(self.update_status) + + self.img_count_label = QLabel('Image Count:') + self.img_count_label.setAlignment(Qt.AlignCenter) + self.img_count_box = QSpinBox() + self.img_count_box.setMinimum(1) + self.img_count_box.setValue(100) + + self.extent_label = QLabel('Extent:') + self.extent_label.setAlignment(Qt.AlignCenter) + self.extent_spin = pg.SpinBox(value=4e-3, suffix='m', siPrefix=True, bounds=[0.1e-3, 20e-3], dec=True, step=0.5, minStep=1e-6, compactHeight=False) + + self.go_button = QPushButton('Start Data Collection') + self.go_button.setEnabled(False) + self.go_button.clicked.connect(self.go) + + layout = QGridLayout() + layout.addWidget(self.stage_label, 0,0, 1,1) + layout.addWidget(self.stage_dropdown, 0,1, 1,1) + layout.addWidget(self.img_count_label, 1,0, 1,1) + layout.addWidget(self.img_count_box, 1,1, 1,1) + layout.addWidget(self.extent_label, 2,0, 1,1) + layout.addWidget(self.extent_spin, 2,1, 1,1) + layout.addWidget(self.go_button, 4,0, 1,2) + self.setLayout(layout) + + self.setMinimumWidth(300) + + def get_stage(self): + return self.stage_dropdown.current_stage() + + def get_img_count(self): + return self.img_count_box.value() + + def get_extent(self): + return self.extent_spin.value() * 1e6 + + def go(self): + self.accept() + + def update_status(self): + if self.stage_dropdown.is_selected(): + self.go_button.setEnabled(True) diff --git a/parallax/main_window.py b/parallax/main_window.py index 8ea747ba..3ee0b85b 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -1,4 +1,4 @@ -from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QHBoxLayout, QMainWindow, QAction +from PyQt5.QtWidgets import QApplication, QWidget, QHBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QIcon import pyqtgraph.console @@ -10,6 +10,7 @@ from .dialogs import AboutDialog from .rigid_body_transform_tool import RigidBodyTransformTool from .stage_manager import StageManager +from .training_data import TrainingDataCollector class MainWindow(QMainWindow): @@ -18,6 +19,11 @@ def __init__(self, model): QMainWindow.__init__(self) self.model = model + # allow main window to be accessed globally + model.main_window = self + + self.data_collector = None + self.widget = MainWidget(model) self.setCentralWidget(self.widget) @@ -37,6 +43,8 @@ def __init__(self, model): self.rbt_action.triggered.connect(self.launch_rbt) self.console_action = QAction("Python Console") self.console_action.triggered.connect(self.show_console) + self.training_data_action = QAction("Collect Training Data") + self.training_data_action.triggered.connect(self.collect_training_data) self.about_action = QAction("About") self.about_action.triggered.connect(self.launch_about) @@ -56,6 +64,7 @@ def __init__(self, model): self.tools_menu = self.menuBar().addMenu("Tools") self.tools_menu.addAction(self.rbt_action) self.tools_menu.addAction(self.console_action) + self.tools_menu.addAction(self.training_data_action) self.help_menu = self.menuBar().addMenu("Help") self.help_menu.addAction(self.about_action) @@ -106,6 +115,9 @@ def refresh_focus_controllers(self): for screen in self.screens(): screen.update_focus_control_menu() + def collect_training_data(self): + self.data_collector = TrainingDataCollector(self.model) + self.data_collector.start() class MainWidget(QWidget): diff --git a/parallax/training_data.py b/parallax/training_data.py new file mode 100644 index 00000000..7da78430 --- /dev/null +++ b/parallax/training_data.py @@ -0,0 +1,78 @@ +import threading, pickle, os +import numpy as np +from PyQt5 import QtWidgets, QtCore +from .dialogs import TrainingDataDialog + + +class TrainingDataCollector(QtCore.QObject): + def __init__(self, model): + QtCore.QObject.__init__(self) + self.model = model + + def start(self): + dlg = TrainingDataDialog(self.model) + dlg.exec_() + if dlg.result() != dlg.Accepted: + return + + self.stage = dlg.get_stage() + self.img_count = dlg.get_img_count() + self.extent = dlg.get_extent() + self.path = QtWidgets.QFileDialog.getExistingDirectory(parent=None, caption="Select Storage Directory") + if self.path == '': + return + + self.start_pos = self.stage.get_position() + self.stage_cal = self.model.get_calibration(self.stage) + + self.thread = threading.Thread(target=self.thread_run, daemon=True) + self.thread.start() + + def thread_run(self): + meta_file = os.path.join(self.path, 'meta.pkl') + if os.path.exists(meta_file): + # todo: just append + raise Exception("Already data in this folder!") + trials = [] + meta = { + 'calibration': self.stage_cal, + 'stage': self.stage.get_name(), + 'trials': trials, + } + + # move electrode out of fov for background images + pos = self.start_pos.coordinates.copy() + pos[2] += 10000 + self.stage.move_to_target_3d(*pos, block=True) + imgs = self.save_images('background') + meta['background'] = imgs + + for i in range(self.img_count): + + # first image in random location + rnd = np.random.uniform(-self.extent/2, self.extent/2, size=3) + pos1 = self.start_pos.coordinates + rnd + self.stage.move_to_target_3d(*pos1, block=True) + images1 = self.save_images(f'{i:04d}-a') + + # take a second image slightly shifted + pos2 = pos1.copy() + pos2[2] += 10 + self.stage.move_to_target_3d(*pos2, block=True) + images2 = self.save_images(f'{i:04d}-b') + + trials.append([ + {'pos': pos1, 'images': images1}, + {'pos': pos2, 'images': images2}, + ]) + + with open(meta_file, 'wb') as fh: + pickle.dump(meta, fh) + + def save_images(self, name): + images = [] + for camera in self.model.cameras: + filename = f'{name}-{camera.name()}.png' + camera.save_last_image(os.path.join(self.path, filename)) + images.append({'camera': camera.name(), 'image': filename}) + return images \ No newline at end of file From 56ca49a61089676ca971a42e08cffaaa47fb252f Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 17:00:48 -0800 Subject: [PATCH 39/62] minor fixes --- parallax/main_window.py | 2 +- parallax/training_data.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/main_window.py b/parallax/main_window.py index 3ee0b85b..9e1ed0f5 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -1,4 +1,4 @@ -from PyQt5.QtWidgets import QApplication, QWidget, QHBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter +from PyQt5.QtWidgets import QApplication, QWidget, QHBoxLayout, QVBoxLayout, QGridLayout, QMainWindow, QAction, QSplitter from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QIcon import pyqtgraph.console diff --git a/parallax/training_data.py b/parallax/training_data.py index 7da78430..df39e347 100644 --- a/parallax/training_data.py +++ b/parallax/training_data.py @@ -23,7 +23,7 @@ def start(self): return self.start_pos = self.stage.get_position() - self.stage_cal = self.model.get_calibration(self.stage) + self.stage_cal = list(self.model.calibrations.values())[0] self.thread = threading.Thread(target=self.thread_run, daemon=True) self.thread.start() From df1f28fbf3faf9d711b32b9c8786e2b5db9e8fa3 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 13 Feb 2023 20:23:19 -0800 Subject: [PATCH 40/62] added script for annotating training images --- tools/annotate_training_data.py | 128 ++++++++++++++++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 tools/annotate_training_data.py diff --git a/tools/annotate_training_data.py b/tools/annotate_training_data.py new file mode 100644 index 00000000..f748a93f --- /dev/null +++ b/tools/annotate_training_data.py @@ -0,0 +1,128 @@ +import pyqtgraph as pg +import json + + +class MainWindow(pg.GraphicsView): + def __init__(self, meta_file, img_files): + super().__init__() + self.img_files = img_files + + self.view = pg.ViewBox() + self.view.invertY() + self.setCentralItem(self.view) + + self.img_item = pg.QtWidgets.QGraphicsPixmapItem() + self.view.addItem(self.img_item) + + self.line_item = pg.QtWidgets.QGraphicsLineItem() + self.line_item.setPen(pg.mkPen('r')) + self.circle_item = pg.QtWidgets.QGraphicsEllipseItem() + self.circle_item.setPen(pg.mkPen('r')) + self.view.addItem(self.line_item) + self.view.addItem(self.circle_item) + + self.next_click = 0 + self.attached_pt = None + self.loaded_file = None + + self.meta_file = meta_file + if os.path.exists(meta_file): + self.meta = json.load(open(meta_file, 'r')) + else: + self.meta = {} + + self.load_image(0) + + def keyPressEvent(self, ev): + if ev.key() == pg.QtCore.Qt.Key_Left: + self.load_image(self.current_index - 1) + elif ev.key() == pg.QtCore.Qt.Key_Right: + self.load_image(self.current_index + 1) + else: + print(ev.key()) + + def mousePressEvent(self, ev): + # print('press', ev) + if ev.button() == pg.QtCore.Qt.LeftButton: + self.attached_pt = self.next_click + self.update_pos(ev.pos()) + ev.accept() + return + # return super().mousePressEvent(ev) + + def mouseReleaseEvent(self, ev): + # print('release', ev) + self.attached_pt = None + self.next_click = (self.next_click + 1) % 2 + + def mouseMoveEvent(self, ev): + # print('move', ev) + self.update_pos(ev.pos()) + ev.accept() + + def update_pos(self, pos): + pos = self.view.mapDeviceToView(pos) + if self.attached_pt == 0: + self.set_pts(pos, None) + elif self.attached_pt == 1: + self.set_pts(None, pos) + else: + return + self.update_meta() + + def set_pts(self, pt1, pt2): + line = self.line_item.line() + if pt1 is not None: + line.setP1(pt1) + self.circle_item.setRect(pt1.x()-10, pt1.y()-10, 20, 20) + self.circle_item.setVisible(True) + if pt2 is not None: + line.setP2(pt2) + self.line_item.setVisible(True) + self.line_item.setLine(line) + + def hide_line(self): + self.line_item.setVisible(False) + self.circle_item.setVisible(False) + + def update_meta(self): + line = self.line_item.line() + self.meta[self.loaded_file] = { + 'pt1': (line.x1(), line.y1()), + 'pt2': (line.x2(), line.y2()), + } + json.dump(self.meta, open(self.meta_file, 'w')) + + def load_image(self, index): + filename = self.img_files[index] + pxm = pg.QtGui.QPixmap() + pxm.load(filename) + self.img_item.setPixmap(pxm) + self.img_item.pxm = pxm + self.view.autoRange(padding=0) + self.current_index = index + self.setWindowTitle(filename) + self.loaded_file = filename + + meta = self.meta.get(filename, {}) + pt1 = meta.get('pt1', None) + pt2 = meta.get('pt2', None) + if None in (pt1, pt2): + self.hide_line() + else: + self.set_pts(pg.QtCore.QPointF(*pt1), pg.QtCore.QPointF(*pt2)) + + +if __name__ == '__main__': + import os, sys + + app = pg.mkQApp() + + meta_file = sys.argv[1] + img_files = sys.argv[2:] + win = MainWindow(meta_file, img_files) + win.resize(1000, 800) + win.show() + + if sys.flags.interactive == 0: + app.exec_() \ No newline at end of file From adfe9f6edbf4a2ce7d58e76c7d89eb2e9af2fe84 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Tue, 14 Feb 2023 22:52:25 -0800 Subject: [PATCH 41/62] Add template based autocalibration --- parallax/calibration.py | 1 + parallax/calibration_worker.py | 39 +++++++++++++++++++++++++++++++--- parallax/config.py | 22 ++++++++++--------- parallax/geometry_panel.py | 18 +++++++++++++++- parallax/main_window.py | 11 ---------- parallax/screen_widget.py | 13 +++++++++--- run-parallax.py | 6 ++++-- 7 files changed, 80 insertions(+), 30 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index e62e5787..e1900e49 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -21,6 +21,7 @@ def __init__(self, img_size): self.img_points1 = [] self.img_points2 = [] self.obj_points = [] + self.template_images = {} def save(self, filename): with open(filename, 'wb') as f: diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 2bfe57fe..723232ab 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -1,6 +1,6 @@ from PyQt5.QtCore import QObject, pyqtSignal import numpy as np -import time +import cv2 as cv import queue from .calibration import Calibration from .helper import WF, HF @@ -9,17 +9,22 @@ class CalibrationWorker(QObject): finished = pyqtSignal() calibration_point_reached = pyqtSignal(int, int, float, float, float) + suggested_corr_points = pyqtSignal(object) RESOLUTION_DEFAULT = 3 EXTENT_UM_DEFAULT = 2000 - def __init__(self, stage, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_UM_DEFAULT, + template_match_method = cv.TM_CCORR_NORMED + template_radius = 200 + + def __init__(self, stage, cameras, resolution=RESOLUTION_DEFAULT, extent_um=EXTENT_UM_DEFAULT, parent=None): # resolution is number of steps per dimension, for 3 dimensions # (so default value of 3 will yield 3^3 = 27 calibration points) # extent_um is the extent in microns for each dimension, centered on zero QObject.__init__(self) self.stage = stage + self.cameras = cameras self.resolution = resolution self.extent_um = extent_um @@ -41,10 +46,16 @@ def run(self): self.stage.move_to_target_3d(x,y,z, relative=True, safe=False) self.calibration_point_reached.emit(n, self.num_cal, x, y, z) + if n > 0: + self.match_templates() + # wait for correspondence points to arrive lcorr, rcorr = self.corr_point_queue.get() - # add to calibration + if n == 0: + self.collect_templates([lcorr, rcorr]) + + # add to calibration self.calibration.add_points(lcorr, rcorr, self.stage.get_position()) n += 1 self.finished.emit() @@ -53,4 +64,26 @@ def get_calibration(self): self.calibration.calibrate() return self.calibration + def collect_templates(self, pts): + r = self.template_radius + for cam, pt in zip(self.cameras, pts): + img = cam.get_last_image_data() + row, col = int(pt[1]), int(pt[0]) + template = img[row-r:row+r, col-r:col+r] + self.calibration.template_images[cam.name()] = template + + def match_templates(self): + method = self.template_match_method + result = {} + for cam in self.cameras: + img = cam.get_last_image_data() + template = self.calibration.template_images[cam.name()] + res = cv.matchTemplate(img, template, method) + if method in (cv.TM_SQDIFF, cv.TM_SQDIFF_NORMED): + ext = res.argmin() + else: + ext = res.argmax() + mx = np.array(np.unravel_index(ext, res.shape)) + result[cam.name()] = mx[::-1] + self.template_radius + self.suggested_corr_points.emit(result) diff --git a/parallax/config.py b/parallax/config.py index 0a06b2c9..64c5a9a5 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -2,16 +2,17 @@ # global configuration config = { - 'views': [{}, {}], - 'cameras': [], - 'stages': [], - 'mock_sim': { - 'show_checkers': True, - 'show_axes': True, + "views": [{}, {}], + "cameras": [], + "stages": [], + "mock_sim": { + "show_checkers": True, + "show_axes": True, + "auto_select_corr_points": True, }, - 'calibration_path': './calibrations', - 'console_history_file': './console_history', - 'console_edit_command': 'code -g {fileName}:{lineNum}', + "calibration_path": "./calibrations", + "console_history_file": "./console_history", + "console_edit_command": "code -g {fileName}:{lineNum}", } @@ -25,7 +26,7 @@ def parse_cli_args(): return args -def init_config(args, model, main_window): +def init_config(args): global config if args.config is not None: loaded_config = json.load(open(args.config, 'r')) @@ -34,6 +35,7 @@ def init_config(args, model, main_window): raise KeyError(f"Invalid config key {k}") config[k] = v +def post_init_config(model, main_window): for i,view in enumerate(config['views']): screen_widget_ctrl = main_window.widget.add_screen() if 'default_camera' in view: diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index db241a4f..43acdc61 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -112,10 +112,11 @@ def launch_cal_dialog(self): def start_cal_thread(self, stage, res, extent): self.model.cal_in_progress = True self.cal_thread = QThread() - self.cal_worker = CalibrationWorker(stage, res, extent) + self.cal_worker = CalibrationWorker(stage, self.model.cameras, res, extent) self.cal_worker.moveToThread(self.cal_thread) self.cal_thread.started.connect(self.cal_worker.run) self.cal_worker.calibration_point_reached.connect(self.handle_cal_point_reached) + self.cal_worker.suggested_corr_points.connect(self.show_suggested_corr_points) self.cal_thread.finished.connect(self.handle_cal_finished) self.cal_worker.finished.connect(self.cal_thread.quit) self.cal_thread.finished.connect(self.cal_thread.deleteLater) @@ -125,6 +126,7 @@ def start_cal_thread(self, stage, res, extent): def handle_cal_point_reached(self, n, num_cal, x,y,z): self.msg_posted.emit('Calibration point %d (of %d) reached: [%f, %f, %f]' % (n+1,num_cal, x,y,z)) self.msg_posted.emit('Highlight correspondence points and press C to continue') + self.auto_select_cal_point() self.cal_point_reached.emit() def register_corr_points_cal(self): @@ -217,3 +219,17 @@ def show_rbt_tool(self): self.rbt_tool.generated.connect(self.update_transforms) self.rbt_tool.show() + def auto_select_cal_point(self): + # auto-calibrate mock stage + stage = self.cal_worker.stage + if config['mock_sim']['auto_select_corr_points'] and hasattr(stage, 'get_tip_position'): + tip_pos = stage.get_tip_position() + for screen in self.model.main_window.screens(): + pos = screen.camera.camera_tr.map(tip_pos.coordinates) + screen.set_selected(pos[:2]) + + def show_suggested_corr_points(self, pts): + screens = {screen.screen_widget.camera.name():screen for screen in self.model.main_window.screens()} + for cam_name, pt in pts.items(): + screens[cam_name].set_selected(pt) + \ No newline at end of file diff --git a/parallax/main_window.py b/parallax/main_window.py index 63627753..b9ca1a57 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -165,7 +165,6 @@ def __init__(self, model): self.geo_panel.msg_posted.connect(self.msg_log.post) self.geo_panel.cal_point_reached.connect(self.clear_selected) self.geo_panel.cal_point_reached.connect(self.zoom_out) - self.geo_panel.cal_point_reached.connect(self.auto_select_cal_point) self.model.msg_posted.connect(self.msg_log.post) def add_screen(self): @@ -212,13 +211,3 @@ def update_corr(self): # send correspondence points to model pts = [ctrl.screen_widget.get_selected() for ctrl in self.screens] self.model.set_correspondence_points(pts) - - def auto_select_cal_point(self): - # auto-calibrate mock stage - stage = self.geo_panel.cal_worker.stage - if hasattr(stage, 'get_tip_position'): - tip_pos = stage.get_tip_position() - for ctrl in self.model.main_window.screens(): - screen = ctrl.screen_widget - pos = screen.camera.camera_tr.map(tip_pos.coordinates) - screen.set_selected(pos[:2]) diff --git a/parallax/screen_widget.py b/parallax/screen_widget.py index 48aef8a8..69d0387a 100755 --- a/parallax/screen_widget.py +++ b/parallax/screen_widget.py @@ -52,6 +52,15 @@ def zoom_out(self): def clear_selected(self): self.screen_widget.clear_selected() + def get_selected(self): + return self.screen_widget.get_selected() + + def set_selected(self, pos): + self.screen_widget.set_selected(pos) + + def clear_selected(self): + self.screen_widget.clear_selected() + class ScreenWidget(pg.GraphicsView): @@ -137,9 +146,7 @@ def update_focus_control_menu(self): def image_clicked(self, event): if event.button() == QtCore.Qt.MouseButton.LeftButton: - self.click_target.setPos(event.pos()) - self.click_target.setVisible(True) - self.selected.emit(*self.get_selected()) + self.set_selected(event.pos()) elif event.button() == QtCore.Qt.MouseButton.MiddleButton: self.zoom_out() diff --git a/run-parallax.py b/run-parallax.py index 3a47b764..3b9efeeb 100755 --- a/run-parallax.py +++ b/run-parallax.py @@ -17,13 +17,15 @@ app = QApplication([]) +args = parallax.config.parse_cli_args() +parallax.config.init_config(args) + model = Model() atexit.register(model.clean) main_window = MainWindow(model) main_window.show() -args = parallax.config.parse_cli_args() -parallax.config.init_config(args, model, main_window) +parallax.config.post_init_config(model, main_window) app.exec() From f469b67072437cd918109fe9e3602c56a1fdaef0 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Tue, 14 Feb 2023 23:47:59 -0800 Subject: [PATCH 42/62] Add fast template matching --- parallax/calibration_worker.py | 47 +++++++++++++++++++++++++++++----- parallax/dialogs.py | 2 +- 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 723232ab..67bb59bd 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -1,3 +1,4 @@ +import time from PyQt5.QtCore import QObject, pyqtSignal import numpy as np import cv2 as cv @@ -47,6 +48,7 @@ def run(self): self.calibration_point_reached.emit(n, self.num_cal, x, y, z) if n > 0: + time.sleep(1.0) # let camera catch up self.match_templates() # wait for correspondence points to arrive @@ -78,12 +80,45 @@ def match_templates(self): for cam in self.cameras: img = cam.get_last_image_data() template = self.calibration.template_images[cam.name()] - res = cv.matchTemplate(img, template, method) - if method in (cv.TM_SQDIFF, cv.TM_SQDIFF_NORMED): - ext = res.argmin() - else: - ext = res.argmax() - mx = np.array(np.unravel_index(ext, res.shape)) + res, mx = fast_template_match(img, template, method) result[cam.name()] = mx[::-1] + self.template_radius self.suggested_corr_points.emit(result) + + +def template_match(img, template, method): + # should we look for min or max in match results + ext_method = 'argmax' + if method in (cv.TM_SQDIFF, cv.TM_SQDIFF_NORMED): + ext_method = 'argmin' + res = cv.matchTemplate(img, template, method) + ext = getattr(res, ext_method)() + mx = np.array(np.unravel_index(ext, res.shape)) + return res, mx + + +def fast_template_match(img, template, method, downsample=10): + """2-stage template match for better performance + """ + + # first convert to greyscale + img = img.mean(axis=2).astype('ubyte') + template = template.mean(axis=2).astype('ubyte') + + # do a quick template match on downsampled images + img2 = img[::downsample, ::downsample] + template2 = template[::downsample, ::downsample] + + res, mx = template_match(img2, template2, method) + mx = mx * downsample + + crop = [ + (max(0, mx[0] - downsample*2), mx[0] + template.shape[0] + downsample*2), + (max(0, mx[1] - downsample*2), mx[1] + template.shape[1] + downsample*2), + ] + + img3 = img[crop[0][0]:crop[0][1], crop[1][0]:crop[1][1]] + res, mx = template_match(img3, template, method) + mx = mx + [crop[0][0], crop[1][0]] + + return res, mx diff --git a/parallax/dialogs.py b/parallax/dialogs.py index 6a5e8df9..89a7537e 100755 --- a/parallax/dialogs.py +++ b/parallax/dialogs.py @@ -238,7 +238,7 @@ def populate_random(self): def get_params(self): params = {} if self.obj_point is None: - params['point'] = np.array([self.xedit.text(), self.yedit.text(), self.zedit.text()]) + params['point'] = np.array([float(self.xedit.text()), float(self.yedit.text()), float(self.zedit.text())]) params['relative'] = self.abs_rel_toggle.isChecked() else: params['point'] = self.obj_point From 11b9994c901e120b139347a5bf1aa144c9fbdf90 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Wed, 15 Feb 2023 00:08:43 -0800 Subject: [PATCH 43/62] move mock autocalibration to CalibrationWorker --- parallax/calibration_worker.py | 30 ++++++++++++++++++++++++------ parallax/config.py | 3 --- parallax/geometry_panel.py | 10 ---------- parallax/mock_sim.py | 2 +- 4 files changed, 25 insertions(+), 20 deletions(-) diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 67bb59bd..63e91216 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -5,6 +5,8 @@ import queue from .calibration import Calibration from .helper import WF, HF +from .model import Model +from .config import config class CalibrationWorker(QObject): @@ -44,20 +46,36 @@ def run(self): for x in np.linspace(mn, mx, self.resolution): for y in np.linspace(mn, mx, self.resolution): for z in np.linspace(mn, mx, self.resolution): + + # move stage to next point and let the user know we are ready for clicks self.stage.move_to_target_3d(x,y,z, relative=True, safe=False) self.calibration_point_reached.emit(n, self.num_cal, x, y, z) - if n > 0: - time.sleep(1.0) # let camera catch up - self.match_templates() - - # wait for correspondence points to arrive + # If we are using a mock stage, automatically + # select known correspondence points + if config['mock_sim']['auto_select_corr_points'] and hasattr(self.stage, 'get_tip_position'): + tip_pos = self.stage.get_tip_position() + pts = {} + for screen in Model.instance.main_window.screens(): + camera = screen.screen_widget.camera + pos = camera.camera_tr.map(tip_pos.coordinates) + pts[camera.name()] = pos[:2] + self.suggested_corr_points.emit(pts) + else: + # Attempt to select correspondence points based on template match + if n > 0: + time.sleep(0.25) # let camera catch up + self.match_templates() + + # wait for user to confirm correspondence points lcorr, rcorr = self.corr_point_queue.get() if n == 0: + # Add template images to calibration (maybe this could be a + # running average instead of one-time?) self.collect_templates([lcorr, rcorr]) - # add to calibration + # add points to calibration self.calibration.add_points(lcorr, rcorr, self.stage.get_position()) n += 1 self.finished.emit() diff --git a/parallax/config.py b/parallax/config.py index 64c5a9a5..8e33cc1e 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -16,9 +16,6 @@ } - - - def parse_cli_args(): parser = argparse.ArgumentParser(prog='parallax') parser.add_argument('--config', type=str, default=None, help='configuration file to load at startup') diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 43acdc61..8d0cd38e 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -126,7 +126,6 @@ def start_cal_thread(self, stage, res, extent): def handle_cal_point_reached(self, n, num_cal, x,y,z): self.msg_posted.emit('Calibration point %d (of %d) reached: [%f, %f, %f]' % (n+1,num_cal, x,y,z)) self.msg_posted.emit('Highlight correspondence points and press C to continue') - self.auto_select_cal_point() self.cal_point_reached.emit() def register_corr_points_cal(self): @@ -219,15 +218,6 @@ def show_rbt_tool(self): self.rbt_tool.generated.connect(self.update_transforms) self.rbt_tool.show() - def auto_select_cal_point(self): - # auto-calibrate mock stage - stage = self.cal_worker.stage - if config['mock_sim']['auto_select_corr_points'] and hasattr(stage, 'get_tip_position'): - tip_pos = stage.get_tip_position() - for screen in self.model.main_window.screens(): - pos = screen.camera.camera_tr.map(tip_pos.coordinates) - screen.set_selected(pos[:2]) - def show_suggested_corr_points(self, pts): screens = {screen.screen_widget.camera.name():screen for screen in self.model.main_window.screens()} for cam_name, pt in pts.items(): diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index f7ed2d7d..7e815f6f 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -329,10 +329,10 @@ def __init__(self): self.stages = {} self.items = [] + s = 1e3 if config['mock_sim']['show_checkers']: cb_size = 8 checkers = CheckerBoard(views=[], size=cb_size, colors=[0.4, 0.6]) - s = 1e3 checkers.transform.set_params(offset=[-s*cb_size/2, -s*cb_size/2, -4000], scale=[s, s, s]) self.items.append(checkers) From ca3fd1573a2bb728f02000a95e3743545d97e53f Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Mon, 6 Mar 2023 14:42:40 -0800 Subject: [PATCH 44/62] minor fixes --- parallax/config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/config.py b/parallax/config.py index 8e33cc1e..9ce7f34b 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -34,7 +34,7 @@ def init_config(args): def post_init_config(model, main_window): for i,view in enumerate(config['views']): - screen_widget_ctrl = main_window.widget.add_screen() + screen_widget = main_window.widget.add_screen() if 'default_camera' in view: camera = model.get_camera(view['default_camera']) - screen_widget_ctrl.screen_widget.set_camera(camera) + screen_widget.set_camera(camera) From c98ff9d8393a0a285f17002ef4d1e7835ca140d2 Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 14:32:24 -0800 Subject: [PATCH 45/62] instantiate MockCameras even if PySpin is installed --- parallax/camera.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/parallax/camera.py b/parallax/camera.py index 23a07f1a..0367b407 100755 --- a/parallax/camera.py +++ b/parallax/camera.py @@ -18,13 +18,12 @@ def list_cameras(): cameras = [] + cameras.extend([ + MockCamera(camera_params={'pitch': 70, 'yaw': 120}), + MockCamera(camera_params={'pitch': 70, 'yaw': 150}), + ]) if PySpin is not None: cameras.extend(PySpinCamera.list_cameras()) - else: - cameras.extend([ - MockCamera(camera_params={'pitch': 70, 'yaw': 120}), - MockCamera(camera_params={'pitch': 70, 'yaw': 150}), - ]) return cameras From 83843d00f1ad6beab3349b7b7a0aadd78a840bd0 Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 14:42:23 -0800 Subject: [PATCH 46/62] config.py: dont assume these directories exist --- parallax/config.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/config.py b/parallax/config.py index 9ce7f34b..eef5bdb4 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -10,8 +10,8 @@ "show_axes": True, "auto_select_corr_points": True, }, - "calibration_path": "./calibrations", - "console_history_file": "./console_history", + "calibration_path": "./", + "console_history_file": "./", "console_edit_command": "code -g {fileName}:{lineNum}", } From 667ae12e0a928f86ddcd3562963c51144fb620ea Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 14:43:54 -0800 Subject: [PATCH 47/62] rm "assert len(calibrations) > 0" --- parallax/model.py | 1 - 1 file changed, 1 deletion(-) diff --git a/parallax/model.py b/parallax/model.py index 7d7b71f0..71b6c5a3 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -79,7 +79,6 @@ def list_calibrations(self): for cal in self.calibrations.values(): calibrations.append({'calibration': cal, 'from_cs': cal.from_cs, 'to_cs': cal.to_cs, 'timestamp': cal.timestamp}) - assert len(calibrations) > 0 return calibrations def get_calibration(self, stage): From 1f29ae7863233cfa804622905e21c327418f240f Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 17:49:01 -0800 Subject: [PATCH 48/62] remove call to CalibrationDialog.get_name --- parallax/geometry_panel.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 0f9b4b3b..7706a9eb 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -105,8 +105,7 @@ def cal_start_stop(self): stage = dlg.get_stage() res = dlg.get_resolution() extent = dlg.get_extent() - name = dlg.get_name() - self.start_cal_thread(stage, res, extent, name) + self.start_cal_thread(stage, res, extent) elif self.cal_start_stop_button.text() == 'Stop': self.stop_cal_thread() @@ -232,4 +231,4 @@ def show_suggested_corr_points(self, pts): screens = {screen.screen_widget.camera.name():screen for screen in self.model.main_window.screens()} for cam_name, pt in pts.items(): screens[cam_name].set_selected(pt) - \ No newline at end of file + From 701bd166c33410ebbd269bddc21d3769b21e6ebb Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 17:59:31 -0800 Subject: [PATCH 49/62] ScreenWidget: update camera, foco menus on init --- parallax/screen_widget.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/parallax/screen_widget.py b/parallax/screen_widget.py index fab23218..4d53904f 100755 --- a/parallax/screen_widget.py +++ b/parallax/screen_widget.py @@ -57,6 +57,8 @@ def __init__(self, filename=None, model=None, parent=None): self.detector_menu = self.parallax_menu.addMenu("Detectors") self.view_box.menu.insertMenu(self.view_box.menu.actions()[0], self.parallax_menu) + self.update_camera_menu() + self.update_focus_control_menu() self.update_filter_menu() self.update_detector_menu() From 017eebbe00464b8784a02c8ae710503571e4155e Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 18:23:12 -0800 Subject: [PATCH 50/62] fix 'ScreenWidget' object has no attribute 'screen_widget' --- parallax/calibration_worker.py | 2 +- parallax/geometry_panel.py | 2 +- parallax/main_window.py | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/parallax/calibration_worker.py b/parallax/calibration_worker.py index 1d8f52cb..16eb0787 100644 --- a/parallax/calibration_worker.py +++ b/parallax/calibration_worker.py @@ -62,7 +62,7 @@ def run(self): tip_pos = self.stage.get_tip_position() pts = {} for screen in Model.instance.main_window.screens(): - camera = screen.screen_widget.camera + camera = screen.camera pos = camera.camera_tr.map(tip_pos.coordinates) pts[camera.name()] = pos[:2] self.suggested_corr_points.emit(pts) diff --git a/parallax/geometry_panel.py b/parallax/geometry_panel.py index 7706a9eb..225e30aa 100644 --- a/parallax/geometry_panel.py +++ b/parallax/geometry_panel.py @@ -228,7 +228,7 @@ def show_rbt_tool(self): self.rbt_tool.show() def show_suggested_corr_points(self, pts): - screens = {screen.screen_widget.camera.name():screen for screen in self.model.main_window.screens()} + screens = {screen.camera.name():screen for screen in self.model.main_window.screens()} for cam_name, pt in pts.items(): screens[cam_name].set_selected(pt) diff --git a/parallax/main_window.py b/parallax/main_window.py index 4683cd4b..0cd9be8b 100644 --- a/parallax/main_window.py +++ b/parallax/main_window.py @@ -230,5 +230,6 @@ def save_camera_frames(self): def update_corr(self): # send correspondence points to model - pts = [ctrl.screen_widget.get_selected() for ctrl in self.screens] + pts = [ctrl.get_selected() for ctrl in self.screens] self.model.set_correspondence_points(pts) + From cf775615d1ac557e0bc6385455e42f6c9f15dbe8 Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 18:33:38 -0800 Subject: [PATCH 51/62] fix blank buttons in ControlPanel --- parallax/control_panel.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/control_panel.py b/parallax/control_panel.py index 4b1fa728..96fb0884 100644 --- a/parallax/control_panel.py +++ b/parallax/control_panel.py @@ -69,11 +69,11 @@ def __init__(self, model): self.dropdown.activated.connect(self.handle_stage_selection) self.settings_button = QPushButton() - self.settings_button.setIcon(QIcon('../img/gear.png')) + self.settings_button.setIcon(QIcon('./img/gear.png')) self.settings_button.clicked.connect(self.handle_settings) self.calibration_label = QLabel("") - self.cal_pt_btn = QPushButton('') + self.cal_pt_btn = QPushButton('Copy Calibration Point') self.cal_pt_btn.clicked.connect(self.copy_cal_pt) self.xcontrol = AxisControl('x') From 7009cfadf9ef54cbad7a185138c28c423a96f9c1 Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Tue, 7 Mar 2023 18:45:13 -0800 Subject: [PATCH 52/62] implement start_accuracy_test --- parallax/accuracy_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallax/accuracy_test.py b/parallax/accuracy_test.py index 13d50cd5..3996fdb9 100644 --- a/parallax/accuracy_test.py +++ b/parallax/accuracy_test.py @@ -93,7 +93,7 @@ def __init__(self, model, parent=None): self.setMinimumWidth(300) def start_accuracy_test(self): - print('TODO: start_accuracy_test') + self.model.start_accuracy_test(self.get_params()) def get_params(self): params = {} From 57f56acac65eee26cfc11e197043f5906885b1da Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Wed, 8 Mar 2023 17:01:30 -0800 Subject: [PATCH 53/62] StageDropdown: s/get_current_stage/current_stage --- parallax/accuracy_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallax/accuracy_test.py b/parallax/accuracy_test.py index 3996fdb9..a0644918 100644 --- a/parallax/accuracy_test.py +++ b/parallax/accuracy_test.py @@ -97,7 +97,7 @@ def start_accuracy_test(self): def get_params(self): params = {} - params['stage'] = self.stage_dropdown.get_current_stage() + params['stage'] = self.stage_dropdown.current_stage() params['cal'] = self.model.calibrations[self.cal_dropdown.currentText()] params['npoints'] = int(self.npoints_edit.text()) params['extent_um'] = float(self.extent_edit.text()) From ea383e39b5ef217f7097633bc21f0a96f232541c Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Wed, 8 Mar 2023 17:04:57 -0800 Subject: [PATCH 54/62] model: add {set,clear}_{lcorr,rcorr} methods --- parallax/model.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/parallax/model.py b/parallax/model.py index 71b6c5a3..4f625d37 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -140,6 +140,18 @@ def add_transform(self, name, transform): def get_transform(self, name): return self.transforms[name] + def set_lcorr(self, xc, yc): + self.lcorr = [xc, yc] + + def clear_lcorr(self): + self.lcorr = False + + def set_rcorr(self, xc, yc): + self.rcorr = [xc, yc] + + def clear_rcorr(self): + self.rcorr = False + def handle_accutest_point_reached(self, i, npoints): self.msg_posted.emit('Accuracy test point %d (of %d) reached.' % (i+1,npoints)) self.clear_lcorr() From 4896a3945e79ae692127bcec324bdb8c3ca0b627 Mon Sep 17 00:00:00 2001 From: Chris Chronopoulos Date: Wed, 8 Mar 2023 17:50:46 -0800 Subject: [PATCH 55/62] fix accuracy test --- parallax/accuracy_test.py | 6 +++--- parallax/model.py | 9 ++++----- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/parallax/accuracy_test.py b/parallax/accuracy_test.py index a0644918..0834e77f 100644 --- a/parallax/accuracy_test.py +++ b/parallax/accuracy_test.py @@ -230,9 +230,9 @@ def __init__(self, params): self.ready_to_go = False - def register_corr_points(self, lcorr, rcorr): - xyz_recon = self.cal.triangulate(lcorr, rcorr) - self.results.append(self.last_stage_point + xyz_recon.tolist()) + def register_corr_points(self, corr_pt): + xyz_recon = self.cal.triangulate(corr_pt) + self.results.append(self.last_stage_point + xyz_recon.coordinates.tolist()) def carry_on(self): self.ready_to_go = True diff --git a/parallax/model.py b/parallax/model.py index 4f625d37..2ab7f26a 100755 --- a/parallax/model.py +++ b/parallax/model.py @@ -159,11 +159,10 @@ def handle_accutest_point_reached(self, i, npoints): self.msg_posted.emit('Highlight correspondence points and press C to continue') def register_corr_points_accutest(self): - lcorr, rcorr = self.lcorr, self.rcorr - if (lcorr and rcorr): - self.accutest_worker.register_corr_points(lcorr, rcorr) - self.msg_posted.emit('Correspondence points registered: (%d,%d) and (%d,%d)' % \ - (lcorr[0],lcorr[1], rcorr[0],rcorr[1])) + corr_pt = self.get_image_point() + if corr_pt is not None: + self.accutest_worker.register_corr_points(corr_pt) + self.msg_posted.emit('Correspondence points registered.') self.accutest_worker.carry_on() else: self.msg_posted.emit('Highlight correspondence points and press C to continue') From 6cedd8e989360309529b54300307b7de5000a0ff Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Tue, 21 Mar 2023 19:16:57 -0700 Subject: [PATCH 56/62] Fix console command history --- parallax/config.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/parallax/config.py b/parallax/config.py index eef5bdb4..25c99a06 100644 --- a/parallax/config.py +++ b/parallax/config.py @@ -1,4 +1,6 @@ -import json, argparse +import os, json, argparse + +package_path = os.path.split(os.path.dirname(__file__))[0] # global configuration config = { @@ -11,7 +13,7 @@ "auto_select_corr_points": True, }, "calibration_path": "./", - "console_history_file": "./", + "console_history_file": os.path.join(package_path, 'console.log'), "console_edit_command": "code -g {fileName}:{lineNum}", } From be1ebfa23e3a3c70b5ee00d25bf75c8e08510c17 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Tue, 21 Mar 2023 19:17:11 -0700 Subject: [PATCH 57/62] Fix delayed render --- parallax/mock_sim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index 7e815f6f..eee51140 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -99,7 +99,7 @@ def update_transform(self): def transform_changed(self, event): self.rendered = False - self.view.update() + runInGuiThread(self.view.update) def render(self): self.clear_graphics_items() From ef6e030b7b0b03f0a6bc6ee1ec3e5901db29933f Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 23 Mar 2023 15:19:26 -0700 Subject: [PATCH 58/62] fix camera calibration test in mock_camera --- mock_camera.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/mock_camera.py b/mock_camera.py index 50c2ad2b..7250edda 100644 --- a/mock_camera.py +++ b/mock_camera.py @@ -29,7 +29,8 @@ def set_camera(self, cam, **kwds): camera_params = dict( pitch=30, distance=15, - distortion=(-0.1, 0, 0, 0, 0), + distortion=(-0.1, 0.01, -0.001, 0, 0), + # distortion=(2.49765866e-02, -1.10638222e+01, -1.22811774e-04, 4.89346001e-03, -3.28053580e-01), ) win.set_camera(0, yaw=-5, **camera_params) win.set_camera(1, yaw=5, **camera_params) @@ -59,10 +60,11 @@ def set_camera(self, cam, **kwds): # tr.rotate(45, [0, 0, 1]) def test(n_images=10): - ret, mtx, dist, rvecs, tvecs = calibrate_camera(win, n_images=n_images, cb_size=(cb_size-1, cb_size-1)) - # print(f"Distortion coefficients: {dist}") - # print(f"Intrinsic matrix: {mtx}") - pg.image(undistort_image(win.get_array().transpose(1, 0, 2), mtx, dist)) + view = win.views[0] + ret, mtx, dist, rvecs, tvecs = calibrate_camera(view, n_images=n_images, cb_size=(cb_size-1, cb_size-1)) + print(f"Distortion coefficients: {dist}") + print(f"Intrinsic matrix: {mtx}") + pg.image(undistort_image(view.get_array().transpose(1, 0, 2), mtx, dist)) return mtx, dist From b4a73978a441495b43d3d18da24e0080ccda9fa4 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 23 Mar 2023 15:19:47 -0700 Subject: [PATCH 59/62] move mock_camera to tools --- mock_camera.py => tools/mock_camera.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename mock_camera.py => tools/mock_camera.py (100%) diff --git a/mock_camera.py b/tools/mock_camera.py similarity index 100% rename from mock_camera.py rename to tools/mock_camera.py From 04efbb14a40c3a97a8541bd27cadf0d479d954a0 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 23 Mar 2023 15:20:08 -0700 Subject: [PATCH 60/62] Use better initial guesses for camera calibration --- parallax/calibration.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/parallax/calibration.py b/parallax/calibration.py index e1900e49..73e41153 100755 --- a/parallax/calibration.py +++ b/parallax/calibration.py @@ -69,11 +69,12 @@ class CameraTransform(coorx.BaseTransform): """ # initial intrinsic / distortion coefficients imtx = np.array([ - [1.81982227e+04, 0.00000000e+00, 2.59310865e+03], - [0.00000000e+00, 1.89774632e+04, 1.48105977e+03], - [0.00000000e+00, 0.00000000e+00, 1.00000000e+00] - ]) - idist = np.array([[ 1.70600649e+00, -9.85797706e+01, 4.53808433e-03, -2.13200143e-02, 1.79088477e+03]]) + [4.44851950e+04, 0.00000000e+00, 2.59310865e+03], + [0.00000000e+00, 4.44738925e+04, 1.48105977e+03], + [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], + dtype='float32' + ) + idist = np.array([[0., 0., 0., 0., 0.]], dtype='float32') def __init__(self, mtx=None, dist=None, **kwds): super().__init__(dims=(2, 2), **kwds) @@ -102,7 +103,8 @@ def set_mapping(self, img_pts, obj_pts, img_size): rmse, mtx, dist, rvecs, tvecs = cv.calibrateCamera( obj_pts.astype('float32')[np.newaxis, ...], img_pts_undist[np.newaxis, ...], - img_size, self.imtx, self.idist, + img_size, + self.imtx, self.idist, flags=cv.CALIB_USE_INTRINSIC_GUESS + cv.CALIB_FIX_PRINCIPAL_POINT, ) From 6ee3d7c69ebe6c49a6c0a1a8381888b2f6719f16 Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 23 Mar 2023 15:38:13 -0700 Subject: [PATCH 61/62] fix simple camera calibration in mock_camera --- tools/mock_camera.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/mock_camera.py b/tools/mock_camera.py index 7250edda..9f18bf47 100644 --- a/tools/mock_camera.py +++ b/tools/mock_camera.py @@ -64,14 +64,14 @@ def test(n_images=10): ret, mtx, dist, rvecs, tvecs = calibrate_camera(view, n_images=n_images, cb_size=(cb_size-1, cb_size-1)) print(f"Distortion coefficients: {dist}") print(f"Intrinsic matrix: {mtx}") - pg.image(undistort_image(view.get_array().transpose(1, 0, 2), mtx, dist)) + pg.image(undistort_image(view.get_array(), mtx, dist).transpose(1, 0, 2)) return mtx, dist - def test2(): + def test2(n_images=10): """Can we invert opencv's undistortion? """ - ret, mtx, dist, rvecs, tvecs = calibrate_camera(win.views[0], n_images=10, cb_size=(cb_size-1, cb_size-1)) + ret, mtx, dist, rvecs, tvecs = calibrate_camera(win.views[0], n_images=n_images, cb_size=(cb_size-1, cb_size-1)) print(mtx) print(dist) img = win.views[0].get_array() From 73e4e1a7065366c8afb0454d30009244491fdddc Mon Sep 17 00:00:00 2001 From: Luke Campagnola Date: Thu, 23 Mar 2023 17:18:01 -0700 Subject: [PATCH 62/62] fix mock_camera script update on mouse drag --- parallax/mock_sim.py | 1 + 1 file changed, 1 insertion(+) diff --git a/parallax/mock_sim.py b/parallax/mock_sim.py index eee51140..6c4fbdac 100644 --- a/parallax/mock_sim.py +++ b/parallax/mock_sim.py @@ -261,6 +261,7 @@ def get_array(self): def update(self): self.cached_frame = None + self.scene().update() super().update()