Skip to content

Mocksim merge #18

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 65 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 47 commits
Commits
Show all changes
65 commits
Select commit Hold shift + click to select a range
7625d0b
start generic stage interface
campagnola Jan 4, 2023
addba05
Merge branch 'master' of github.com:AllenNeuralDynamics/parallax into…
campagnola Feb 10, 2023
0cca383
mock stage initial function
campagnola Feb 10, 2023
ecbcd95
add mock camera prototype
campagnola Feb 9, 2023
1bc6fa1
add radial distortion
campagnola Feb 9, 2023
c173639
fix distortion
campagnola Feb 9, 2023
4900969
add stereo rendering
campagnola Feb 9, 2023
3be68e2
tangential distortion + invert attempt
campagnola Feb 9, 2023
1761738
minor edits
campagnola Feb 10, 2023
c661925
moved mock code into package, mock camera test script is working again
campagnola Feb 10, 2023
8e35419
mock camera is rendering
campagnola Feb 10, 2023
1f7caed
mock sim working
campagnola Feb 10, 2023
f14092a
Update .gitignore
campagnola Feb 11, 2023
3eae984
Don't change CWD if possible -- this is global
campagnola Feb 11, 2023
903a987
Add config system with calibration path + mock sim options
campagnola Feb 11, 2023
3f41e1e
GUI updates:
campagnola Feb 11, 2023
9649654
Get calibration working again
campagnola Feb 11, 2023
01457ef
mock sim tweaks
campagnola Feb 11, 2023
3f5e0d5
Calibration working correctly on mock stage
campagnola Feb 12, 2023
cb0268b
add console command history and editor support
campagnola Feb 12, 2023
d089a40
move more calibration code to individual camera transforms
campagnola Feb 12, 2023
e84a6c5
make it easier to access selected calibration
campagnola Feb 12, 2023
a44d570
fix esc key
campagnola Feb 12, 2023
c29c569
store CS names with calibration
campagnola Feb 13, 2023
7a86da3
more efficient mock graphics
campagnola Feb 13, 2023
c5fe533
show stage position as it moves
campagnola Feb 13, 2023
6e8c8dc
use system names in calibration file name
campagnola Feb 13, 2023
5115afa
minor updates
campagnola Feb 13, 2023
8b23e35
Autoload calibrations, add move to selected button
campagnola Feb 13, 2023
fd44984
update calibrations after generating a new one
campagnola Feb 13, 2023
30f1557
Add extra move commands
campagnola Feb 13, 2023
e780f01
fix mock stage speed
campagnola Feb 13, 2023
b67fb6b
Add button for copying selected point
campagnola Feb 13, 2023
948cf91
make mock sim cmera grab threada-safe
campagnola Feb 13, 2023
762dba2
easier way to get calibrations per stage
campagnola Feb 13, 2023
0e0461e
Add tool for collecting training data
campagnola Feb 13, 2023
77cb9a4
bail out of training data collection if user cancels
campagnola Feb 14, 2023
70bc5c0
remove notes file
campagnola Feb 14, 2023
a7e2623
Add training data collector
campagnola Feb 14, 2023
56ca49a
minor fixes
campagnola Feb 14, 2023
df1f28f
added script for annotating training images
campagnola Feb 14, 2023
5cfaf2c
Merge branch 'training-data' into mock-sim
campagnola Feb 15, 2023
adfe9f6
Add template based autocalibration
campagnola Feb 15, 2023
f469b67
Add fast template matching
campagnola Feb 15, 2023
11b9994
move mock autocalibration to CalibrationWorker
campagnola Feb 15, 2023
ff8f580
Merge remote-tracking branch 'ai/master' into mocksim-merge
campagnola Mar 6, 2023
ca3fd15
minor fixes
campagnola Mar 6, 2023
c98ff9d
instantiate MockCameras even if PySpin is installed
chronopoulos Mar 7, 2023
83843d0
config.py: dont assume these directories exist
chronopoulos Mar 7, 2023
667ae12
rm "assert len(calibrations) > 0"
chronopoulos Mar 7, 2023
1f29ae7
remove call to CalibrationDialog.get_name
chronopoulos Mar 8, 2023
701bd16
ScreenWidget: update camera, foco menus on init
chronopoulos Mar 8, 2023
017eebb
fix 'ScreenWidget' object has no attribute 'screen_widget'
chronopoulos Mar 8, 2023
cf77561
fix blank buttons in ControlPanel
chronopoulos Mar 8, 2023
7009cfa
implement start_accuracy_test
chronopoulos Mar 8, 2023
57f56ac
StageDropdown: s/get_current_stage/current_stage
chronopoulos Mar 9, 2023
ea383e3
model: add {set,clear}_{lcorr,rcorr} methods
chronopoulos Mar 9, 2023
4896a39
fix accuracy test
chronopoulos Mar 9, 2023
6cedd8e
Fix console command history
campagnola Mar 22, 2023
be1ebfa
Fix delayed render
campagnola Mar 22, 2023
ef6e030
fix camera calibration test in mock_camera
campagnola Mar 23, 2023
b4a7397
move mock_camera to tools
campagnola Mar 23, 2023
04efbb1
Use better initial guesses for camera calibration
campagnola Mar 23, 2023
6ee3d7c
fix simple camera calibration in mock_camera
campagnola Mar 23, 2023
73e4e1a
fix mock_camera script update on mouse drag
campagnola Mar 24, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,9 @@ parallax/*.log
*.npy
*.pkl
*.csv
*.json
.idea
*.egg-info
*-workspace
*.log
console_history
92 changes: 92 additions & 0 deletions mock_camera.py
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)
4 changes: 2 additions & 2 deletions parallax/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,5 +5,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__)))
Copy link
Collaborator Author

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

221 changes: 139 additions & 82 deletions parallax/calibration.py
Original file line number Diff line number Diff line change
@@ -1,99 +1,156 @@
#!/usr/bin/python3

import time
import pickle
import numpy as np
import cv2 as cv
import coorx
from . import lib
from .helper import WF, HF


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):
self.name = name
self.set_initial_intrinsics_default()

def set_name(self, name):
self.name = name

def set_origin(self, origin):
self.origin = origin
date_format = r"%Y-%m-%d-%H-%M-%S"
file_regex = r'([^-]+)-([^-]+)-((\d\d\d\d-\d\d-\d\d)-(\d+)-(\d+)-(\d+)).pkl'

def get_origin(self):
return self.origin
@staticmethod
def load(filename):
with open(filename, 'rb') as f:
return pickle.load(f)

def set_initial_intrinsics(self, mtx1, mtx2, dist1, dist2):
def __init__(self, img_size):
self.img_size = img_size
self.img_points1 = []
self.img_points2 = []
self.obj_points = []
self.template_images = {}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

2 templates are stored per calibration here - think about this


self.imtx1 = mtx1
self.imtx2 = mtx2
self.idist1 = dist1
self.idist2 = dist2
def save(self, filename):
with open(filename, 'wb') as f:
pickle.dump(self, f)

def set_initial_intrinsics_default(self):
@property
def name(self):
date = time.strftime(self.date_format, self.timestamp)
return f"{self.from_cs}-{self.to_cs}-{date}"

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 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):
def triangulate(self, img_point):
"""
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]

def calibrate(self, img_points1, img_points2, obj_points, origin):

self.set_origin(origin)

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_points1 = lib.undistort_image_points(img_points1, self.imtx1, self.idist1)
img_points2 = lib.undistort_image_points(img_points2, self.imtx2, self.idist2)

# calibrate each camera against these points
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)

# 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.rmse1 = rmse1
self.rmse2 = rmse2


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
Loading