Mocksim merge #18
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -6,3 +6,9 @@ parallax/*.log | |
| *.npy | ||
| *.pkl | ||
| *.csv | ||
| *.json | ||
| .idea | ||
| *.egg-info | ||
| *-workspace | ||
| *.log | ||
| console_history | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| import pyqtgraph as pg | ||
| import coorx | ||
| 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)): | ||
| 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() | ||
|
|
||
| app = pg.mkQApp() | ||
| win = StereoView() | ||
| win.resize(1600, 600) | ||
| win.show() | ||
|
|
||
| camera_params = dict( | ||
| pitch=30, | ||
| distance=15, | ||
| distortion=(-0.1, 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]) | ||
| 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.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]) | ||
|
|
||
|
|
||
| # 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(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)) | ||
| 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.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]) | ||
|
|
||
| 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[:,0], y=undistorted_pts[:,1], brush='r', pen=None) | ||
| v2.view.addItem(s1) | ||
|
|
||
| s2 = pg.ScatterPlotItem(x=distorted_pts[:,0], y=distorted_pts[:,1], brush='r', pen=None) | ||
| v1.view.addItem(s2) |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,99 +1,156 @@ | ||
| import time | ||
| import pickle | ||
| import numpy as np | ||
| import cv2 as cv | ||
| import coorx | ||
| from . import lib | ||
|
|
||
|
|
||
| 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 = [] | ||
| self.template_images = {} | ||
|
There was a problem hiding this comment. 2 templates are stored per calibration here - think about this |
||
|
|
||
| def save(self, filename): | ||
| with open(filename, 'wb') as f: | ||
| pickle.dump(self, f) | ||
|
|
||
| @property | ||
| def name(self): | ||
| 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) | ||
| self.img_points2.append(img_pt2) | ||
| self.obj_points.append(obj_pt) | ||
|
|
||
| def triangulate(self, img_point): | ||
| """ | ||
| l/rcorr = [xc, yc] | ||
| """ | ||
| return self.transform.map(img_point) | ||
|
|
||
| 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=self.from_cs, to_cs=self.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): | ||
| """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.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) | ||
|
|
||
| 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, | ||
| ) | ||
|
|
||
| # 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() | ||
| self.camera_tr2 = CameraTransform() | ||
| self.proj1 = None | ||
| self.proj2 = None | ||
|
|
||
| def set_mapping(self, img_points1, img_points2, obj_points, img_size): | ||
| self.camera_tr1.set_mapping(img_points1, obj_points, img_size) | ||
| self.camera_tr2.set_mapping(img_points2, obj_points, img_size) | ||
|
|
||
| self.proj1 = self.camera_tr1.proj_matrix | ||
| self.proj2 = self.camera_tr2.proj_matrix | ||
|
|
||
| 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) | ||
| 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) | ||
|
|
||
| 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 | ||
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
remove this and use data dir config options to set load/save paths on dialogs