def make_default_system():
'''helper function to generate an instance of MultiCameraSystem'''
lookat = np.array( (0.0, 0.0, 0.0))
center1 = np.array( (0.0, 0.0, 0.9) )
distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] )
cam1 = CameraModel.load_camera_simple(name='cam1',
fov_x_degrees=90,
eye=center1,
lookat=lookat,
distortion_coefficients=distortion1,
)
center2 = np.array( (0.5, -0.8, 0.0) )
cam2 = CameraModel.load_camera_simple(name='cam2',
fov_x_degrees=90,
eye=center2,
lookat=lookat,
)
center3 = np.array( (0.5, 0.5, 0.0) )
cam3 = CameraModel.load_camera_simple(name='cam3',
fov_x_degrees=90,
eye=center3,
lookat=lookat,
)
system = MultiCameraSystem([cam1,cam2,cam3])
return system
def make_default_system(with_separate_distorions=False):
"""helper function to generate an instance of MultiCameraSystem"""
if with_separate_distorions:
raise NotImplementedError
camxs = np.linspace(-2, 2, 3)
camzs = np.linspace(-2, 2, 3).tolist()
camzs.pop(1)
cameras = []
lookat = np.array((0.0, 0, 0))
up = np.array((0.0, 0, 1))
for enum, camx in enumerate(camxs):
center = np.array((camx, -5, 0))
name = "camx_%d" % (enum + 1,)
cam = CameraModel.load_camera_simple(name=name, fov_x_degrees=45, eye=center, lookat=lookat, up=up)
cameras.append(cam)
for enum, camz in enumerate(camzs):
center = np.array((0, -5, camz))
name = "camz_%d" % (enum + 1,)
cam = CameraModel.load_camera_simple(name=name, fov_x_degrees=45, eye=center, lookat=lookat, up=up)
cameras.append(cam)
system = MultiCameraSystem(cameras)
return system
def make_cam_from_params(self, params):
if 1:
t = params[:3]
rod = params[3:]
rmat = rodrigues2matrix( rod )
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = rmat
cam_model = CameraModel.from_dict(d)
return cam_model
C = params[:3]
quat = params[3:]
qmag = np.sqrt(np.sum(quat**2))
quat = quat/qmag
R,rquat=get_rotation_matrix_and_quaternion(quat)
t = -np.dot(R, C)
d = self.intrinsic_dict.copy()
d['translation'] = t
d['Q'] = R
cam_model = CameraModel.from_dict(d)
return cam_model
def test_equals():
system = make_default_system()
assert system != 1234
system2 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)])
system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(3)])
assert system2 != system3
system4 = make_default_system()
d = system4.to_dict()
d['camera_system'][0]['width'] += 1
system5 = MultiCameraSystem.from_dict( d )
assert system4 != system5
def check_flip(distortion=False):
if distortion:
d = [0.1, 0.2, 0.3, 0.4, 0.5]
else:
d = None
# build camera
center_expected = np.array( [10, 5, 20] )
lookat_expected = center_expected + np.array( [1, 2, 0] )
up_expected = np.array( [0, 0, 1] )
width, height = 640, 480
M = np.array( [[ 300.0, 0, 321, 0],
[ 0, 298.0, 240, 0],
[ 0, 0, 1, 0]])
cam1 = CameraModel.load_camera_from_M( M, width=width, height=height,
distortion_coefficients=d )
cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected)
del cam1
pts = np.array([lookat_expected,
lookat_expected+up_expected,
[1,2,3],
[4,5,6]])
pix_actual = cam.project_3d_to_pixel( pts )
# Flipped camera gives same 3D->2D transform but different look direction.
cf = cam.get_flipped_camera()
assert not np.allclose( cam.get_lookat(), cf.get_lookat() )
pix_actual_flipped = cf.project_3d_to_pixel( pts )
assert np.allclose( pix_actual, pix_actual_flipped )
def test_align():
system1 = make_default_system()
system2 = system1.get_aligned_copy( system1 ) # This should be a no-op.
assert system1==system2
system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)])
nose.tools.assert_raises(ValueError, system3.get_aligned_copy, system1)
def check_built_from_M(cam_opts):
"""check that M is preserved in load_camera_from_M() factory"""
cam_orig = _build_test_camera(**cam_opts)
if cam_orig.is_distorted_and_skewed():
raise SkipTest('do not expect that skewed camera passes this test')
M_orig = cam_orig.M
cam = CameraModel.load_camera_from_M( M_orig )
assert np.allclose( cam.M, M_orig)
def get_camera_for_boards(rows,width=0,height=0):
info_dict = {}
for row in rows:
r = row[0]
info_str = '%d %d %f'%(r['rows'], r['columns'], r['size'])
if info_str not in info_dict:
# create entry
info = camera_calibration.calibrator.ChessboardInfo()
info.dim = r['size']
info.n_cols = r['columns']
info.n_rows = r['rows']
info_dict[info_str] = {'info':info,
'corners':[]}
this_list = info_dict[info_str]['corners']
this_corners = r['points']
assert len(this_corners)==r['rows']*r['columns']
this_list.append( this_corners )
boards = []
goodcorners = []
mpl_debug = False
mpl_lines = []
for k in info_dict:
info = info_dict[k]['info']
for xys in info_dict[k]['corners']:
goodcorners.append( (xys,info) )
if mpl_debug:
N = info.n_cols
xs = []; ys=[]
for (x,y) in xys:
xs.append(x); ys.append(y)
while len(xs):
this_xs = xs[:N]
this_ys = ys[:N]
del xs[:N]
del ys[:N]
mpl_lines.append( (this_xs, this_ys ) )
if mpl_debug:
import matplotlib.pyplot as plt
for (this_xs, this_ys) in mpl_lines:
plt.plot( this_xs, this_ys )
plt.show()
cal = camera_calibration.calibrator.MonoCalibrator(boards)
cal.size = (width,height)
r = cal.cal_fromcorners(goodcorners)
msg = cal.as_message()
buf = roslib.message.strify_message(msg)
obj = yaml.safe_load(buf)
cam = CameraModel.from_dict(obj,
extrinsics_required=False)
return cam
def test_equality():
center = np.array( (0, 0.0, 5) )
lookat = center + np.array( (0,1,0))
cam_apple1 = CameraModel.load_camera_simple(fov_x_degrees=90,
eye=center,
lookat=lookat,
name='apple')
cam_apple2 = CameraModel.load_camera_simple(fov_x_degrees=90,
eye=center,
lookat=lookat,
name='apple')
cam_orange = CameraModel.load_camera_simple(fov_x_degrees=30,
eye=center,
lookat=lookat,
name='orange')
assert cam_apple1==cam_apple2
assert cam_apple1!=cam_orange
assert not cam_apple1==cam_orange
def check_roundtrip_ros_tf(cam_opts):
cam1 = _build_test_camera(**cam_opts)
translation, rotation = cam1.get_ROS_tf()
i = cam1.get_intrinsics_as_bunch()
cam2 = CameraModel.load_camera_from_ROS_tf( translation=translation,
rotation=rotation,
intrinsics = i,
name = cam1.name)
assert cam1==cam2
def test_getters():
system1 = make_default_system()
d = system1.to_dict()
names1 = list(system1.get_camera_dict().keys())
names2 = [cd['name'] for cd in d['camera_system']]
assert set(names1)==set(names2)
for idx in range(len(names1)):
cam1 = system1.get_camera( names1[idx] )
cam2 = CameraModel.from_dict(d['camera_system'][idx])
assert cam1==cam2
def calc_mean_reproj_error(self,msg):
ros_cam = CameraModel._from_parts(intrinsics=msg)
all_ims = []
for imd in self.db:
ros_pix = ros_cam.project_3d_to_pixel(imd['cc'], distorted=True)
d = (ros_pix-imd['pix'])**2
drows = np.sqrt(np.sum(d, axis=1))
mean_d = np.mean(drows)
all_ims.append(mean_d)
mean_err = np.mean(all_ims)
return mean_err
def check_align(cam_opts):
cam_orig = _build_test_camera(**cam_opts)
M_orig = cam_orig.M
cam_orig = CameraModel.load_camera_from_M( M_orig )
R1 = np.eye(3)
R2 = np.zeros((3,3))
R2[0,1] = 1
R2[1,0] = 1
R2[2,2] = -1
t1 = np.array( (0.0, 0.0, 0.0) )
t2 = np.array( (0.0, 0.0, 0.1) )
t3 = np.array( (0.1, 0.0, 0.0) )
for s in [1.0, 2.0]:
for R in [R1, R2]:
for t in [t1, t2, t3]:
cam_actual = cam_orig.get_aligned_camera( s, R, t )
M_expected = mcsc_align.align_M( s,R,t, M_orig )
cam_expected = CameraModel.load_camera_from_M( M_expected )
assert cam_actual==cam_expected
def test_simple_projection():
# get some 3D points
pts_3d = _build_points_3d()
if DRAW:
fig = plt.figure(figsize=(8,12))
ax1 = fig.add_subplot(3,1,1, projection='3d')
ax1.scatter( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2])
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
# build a camera calibration matrix
focal_length = 1200
width, height = 640,480
R = np.eye(3) # look at +Z
c = np.array( (9.99, 19.99, 20) )
M = make_M( focal_length, width, height, R, c)['M']
# now, project these 3D points into our image plane
pts_3d_H = np.vstack( (pts_3d.T, np.ones( (1,len(pts_3d))))) # make homog.
undist_rst_simple = np.dot(M, pts_3d_H) # multiply
undist_simple = undist_rst_simple[:2,:]/undist_rst_simple[2,:] # project
if DRAW:
ax2 = fig.add_subplot(3,1,2)
ax2.plot( undist_simple[0,:], undist_simple[1,:], 'b.')
ax2.set_xlim(0,width)
ax2.set_ylim(height,0)
ax2.set_title('matrix multiply')
# build a camera model from our M and project onto image plane
cam = CameraModel.load_camera_from_M( M, width=width, height=height )
undist_full = cam.project_3d_to_pixel(pts_3d).T
if DRAW:
plot_camera( ax1, cam, scale=10, axes_size=5.0 )
sz = 20
x = 5
y = 8
z = 19
ax1.auto_scale_xyz( [x,x+sz], [y,y+sz], [z,z+sz] )
ax3 = fig.add_subplot(3,1,3)
ax3.plot( undist_full[0,:], undist_full[1,:], 'b.')
ax3.set_xlim(0,width)
ax3.set_ylim(height,0)
ax3.set_title('pymvg')
if DRAW:
plt.show()
assert np.allclose( undist_full, undist_simple )
def new_data(self):
with self._lock:
if (self.translation is None or
self.rotation is None or
self.intrinsics is None):
return
newcam = CameraModel.load_camera_from_ROS_tf( translation=self.translation,
rotation=self.rotation,
intrinsics=self.intrinsics,
name=self.get_frame_id(),
)
self.cam = newcam
self.draw()
def check_distortion_yamlfile_roundtrip(cam_opts):
"""check that roundtrip of camera model to/from a yaml file works"""
cam = _build_test_camera(**cam_opts)
fname = tempfile.mktemp(suffix='.yaml')
cam.save_intrinsics_to_yamlfile(fname)
try:
cam2 = CameraModel.load_camera_from_file( fname, extrinsics_required=False )
finally:
os.unlink(fname)
distorted = np.array( [[100.0,100],
[100,200],
[100,300],
[100,400]] )
orig_undistorted = cam.undistort( distorted )
reloaded_undistorted = cam2.undistort( distorted )
assert np.allclose( orig_undistorted, reloaded_undistorted )
def test_lookat():
dist = 5.0
# build camera
center_expected = np.array( [10, 5, 20] )
lookat_expected = center_expected + np.array( [dist, 0, 0] ) # looking in +X
up_expected = np.array( [0, 0, 1] )
f = 300.0 # focal length
width, height = 640, 480
cx, cy = width/2.0, height/2.0
M = np.array( [[ f, 0, cx, 0],
[ 0, f, cy, 0],
[ 0, 0, 1, 0]])
cam1 = CameraModel.load_camera_from_M( M, width=width, height=height)
cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected)
del cam1
# check that the extrinsic parameters were what we expected
(center_actual,lookat_actual,up_actual) = cam.get_view()
lookdir_expected = normalize( lookat_expected - center_expected )
lookdir_actual = normalize( lookat_actual - center_actual )
assert np.allclose( center_actual, center_expected )
assert np.allclose( lookdir_actual, lookdir_expected )
assert np.allclose( up_actual, up_expected )
# check that the extrinsics work as expected
pts = np.array([lookat_expected,
lookat_expected+up_expected])
eye_actual = cam.project_3d_to_camera_frame( pts )
eye_expected = [[0, 0, dist], # camera looks at +Z
[0,-1, dist], # with -Y as up
]
assert np.allclose( eye_actual, eye_expected )
# now check some basics of the projection
pix_actual = cam.project_3d_to_pixel( pts )
pix_expected = [[cx,cy], # center pixel on the camera
[cx,cy-(f/dist)]]
assert np.allclose( pix_actual, pix_expected )
def generate_camera(self):
(width,height)=(self.width,self.height)=(640,480)
center = 1,2,3
rot_axis = np.array((4,5,6.7))
rot_axis = rot_axis / np.sum(rot_axis**2)
rquat = tf.transformations.quaternion_about_axis(0.1, (rot_axis.tolist()))
rmat,_ = get_rotation_matrix_and_quaternion(rquat)
parts = make_M( 1234.56, width, height,
rmat, center)
if self.use_distortion:
dist = [-0.4, .2, 0, 0, 0]
else:
dist = [0, 0, 0, 0, 0]
self.cam = CameraModel.load_camera_from_M(parts['M'],
width=width,height=height,
distortion_coefficients=dist)
def build_example_system(n=6,z=5.0):
base = CameraModel.load_camera_default()
x = np.linspace(0, 2*n, n)
theta = np.linspace(0, 2*np.pi, n)
cams = []
for i in range(n):
# cameras are spaced parallel to the x axis
center = np.array( (x[i], 0.0, z) )
# cameras are looking at +y
lookat = center + np.array( (0,1,0))
# camera up direction rotates around the y axis
up = -np.sin(theta[i]), 0, np.cos(theta[i])
cam = base.get_view_camera(center,lookat,up)
cam.name = 'theta: %.0f'%( np.degrees(theta[i]) )
cams.append(cam)
system = MultiCameraSystem(cams)
return system
def test_problem_M():
"""check a particular M which previously caused problems"""
# This M (found by the DLT method) was causing me problems.
d = {'width': 848,
'name': 'camera',
'height': 480}
M = np.array([[ -1.70677031e+03, -4.10373295e+03, -3.88568028e+02, 6.89034515e+02],
[ -6.19019195e+02, -1.01292091e+03, -2.67534989e+03, 4.51847857e+02],
[ -4.52548832e+00, -3.78900498e+00, -7.35860226e-01, 1.00000000e+00]])
cam = CameraModel.load_camera_from_M( M, **d)
#assert np.allclose( cam.M, M) # we don't expect this since the intrinsic matrix may not be scaled
verts = np.array([[ 0.042306, 0.015338, 0.036328, 1.0],
[ 0.03323, 0.030344, 0.041542, 1.0],
[ 0.036396, 0.026464, 0.052408, 1.0]])
actual = cam.project_3d_to_pixel(verts[:,:3])
expectedh = np.dot( M, verts.T )
expected = (expectedh[:2]/expectedh[2]).T
assert np.allclose( expected, actual )
def check_bagfile_roundtrip(cam_opts):
"""check that roundtrip of camera model to/from a bagfile works"""
cam = _build_test_camera(**cam_opts)
fname = tempfile.mktemp(suffix='.bag')
try:
with open(fname,mode='wb') as fd:
cam.save_to_bagfile(fd, roslib)
with open(fname,mode='r') as fd:
bag = rosbag.Bag(fd, 'r')
cam2 = CameraModel.load_camera_from_opened_bagfile( bag )
finally:
os.unlink(fname)
verts = np.array([[ 0.042306, 0.015338, 0.036328],
[ 0.03323, 0.030344, 0.041542],
[ 0.03323, 0.030344, 0.041542],
[ 0.03323, 0.030344, 0.041542],
[ 0.036396, 0.026464, 0.052408]])
expected = cam.project_3d_to_pixel(verts)
actual = cam2.project_3d_to_pixel(verts)
assert np.allclose( expected, actual )
def from_dict(cls, d):
cam_dict_list = d['camera_system']
cams = [CameraModel.from_dict(cd) for cd in cam_dict_list]
return MultiCameraSystem(cameras=cams)
def from_dict(cls, d):
cam_dict_list = d['camera_system']
cams = [CameraModel.from_dict(cd) for cd in cam_dict_list]
return MultiCameraSystem( cameras=cams )
def from_mcsc(cls, dirname):
'''create MultiCameraSystem from output directory of MultiCamSelfCal'''
# FIXME: This is a bit convoluted because it's been converted
# from multiple layers of internal code. It should really be
# simplified and cleaned up.
do_normalize_pmat = True
all_Pmat = {}
all_Res = {}
all_K = {}
all_distortion = {}
opj = os.path.join
with open(opj(dirname, 'camera_order.txt'), mode='r') as fd:
cam_ids = fd.read().strip().split('\n')
with open(os.path.join(dirname, 'Res.dat'), 'r') as res_fd:
for i, cam_id in enumerate(cam_ids):
fname = 'camera%d.Pmat.cal' % (i + 1)
pmat = np.loadtxt(opj(dirname, fname)) # 3 rows x 4 columns
if do_normalize_pmat:
pmat_orig = pmat
pmat = normalize_M(pmat)
all_Pmat[cam_id] = pmat
all_Res[cam_id] = map(int, res_fd.readline().split())
# load non linear parameters
rad_files = [f for f in os.listdir(dirname) if f.endswith('.rad')]
for cam_id_enum, cam_id in enumerate(cam_ids):
filename = os.path.join(dirname,
'basename%d.rad' % (cam_id_enum + 1, ))
if os.path.exists(filename):
K, distortion = parse_radfile(filename)
all_K[cam_id] = K
all_distortion[cam_id] = distortion
else:
if len(rad_files):
raise RuntimeError(
'.rad files present but none named "%s"' % filename)
warnings.warn('no non-linear data (e.g. radial distortion) '
'in calibration for %s' % cam_id)
all_K[cam_id] = None
all_distortion[cam_id] = None
cameras = []
for cam_id in cam_ids:
w, h = all_Res[cam_id]
Pmat = all_Pmat[cam_id]
M = Pmat[:, :3]
K, R = my_rq(M)
if not is_rotation_matrix(R):
# RQ may return left-handed rotation matrix. Make right-handed.
R2 = -R
K2 = -K
assert np.allclose(np.dot(K2, R2), np.dot(K, R))
K, R = K2, R2
P = np.zeros((3, 4))
P[:3, :3] = K
KK = all_K[cam_id] # from rad file or None
distortion = all_distortion[cam_id]
# (ab)use PyMVG's rectification to do coordinate transform
# for MCSC's undistortion.
# The intrinsic parameters used for 3D -> 2D.
ex = P[0, 0]
bx = P[0, 2]
Sx = P[0, 3]
ey = P[1, 1]
by = P[1, 2]
Sy = P[1, 3]
if KK is None:
rect = np.eye(3)
KK = P[:, :3]
else:
# Parameters used to define undistortion coordinates.
fx = KK[0, 0]
fy = KK[1, 1]
cx = KK[0, 2]
cy = KK[1, 2]
rect = np.array([[ex / fx, 0, (bx + Sx - cx) / fx],
[0, ey / fy, (by + Sy - cy) / fy], [0, 0,
1]]).T
if distortion is None:
distortion = np.zeros((5, ))
C = center(Pmat)
rot = R
t = -np.dot(rot, C)[:, 0]
d = {
'width': w,
'height': h,
'P': P,
'K': KK,
'R': rect,
'translation': t,
'Q': rot,
'D': distortion,
'name': cam_id,
}
cam = CameraModel.from_dict(d)
cameras.append(cam)
return MultiCameraSystem(cameras=cameras)
def from_pymvg_str(cls, buf):
d = json.loads(buf)
assert d['__pymvg_file_version__'] == '1.0'
cam_dict_list = d['camera_system']
cams = [CameraModel.from_dict(cd) for cd in cam_dict_list]
return MultiCameraSystem(cameras=cams)
def get_sample_camera():
yaml_str = """header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
height: 494
width: 659
distortion_model: plumb_bob
D:
- -0.331416226762
- 0.143584747016
- 0.00314558656669
- -0.00393597842852
- 0.0
K:
- 516.385667641
- 0.0
- 339.167079537
- 0.0
- 516.125799368
- 227.379935241
- 0.0
- 0.0
- 1.0
R:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
P:
- 444.369750977
- 0.0
- 337.107817516
- 0.0
- 0.0
- 474.186859131
- 225.062742824
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False"""
d = yaml.load(yaml_str)
cam1 = CameraModel.from_dict(d,extrinsics_required=False)
eye = (10,20,30)
lookat = (11,20,30)
up = (0,-1,0)
cam = cam1.get_view_camera(eye, lookat, up)
return cam
for idx in range(len(calib_id)):
print ""
print "~~~~~~~~~~~~~~~~~~~~~~~~~~"
print "Importing {0}".format(calib_id[idx])
pmat = calib[idx][0]
distortion = calib[idx][1]
name = calib_id[idx]
width = cam_settings[name]["f7"]["width"]
height = cam_settings[name]["f7"]["height"]
print pmat
print distortion
camera = CameraModel.load_camera_from_M(pmat, width=width, height=height, name=name,
distortion_coefficients=distortion)
cameras.append(camera)
system = MultiCameraSystem(cameras)
system.save_to_pymvg_file(join(CALIB, "camera_system.json"))
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# plot_system(ax, system)
for name in system.get_names():
plot_camera(ax, system.get_camera(name))
ax.set_xlabel('x')
ax.set_ylabel('y')
def launch_calibration(self, method, vdisp):
def _pump_ui():
if os.environ.get('RUNNING_NOSE') != '1':
Gtk.main_iteration_do(False) #run once, no block
while Gtk.events_pending(): #run remaining
Gtk.main_iteration()
orig_data = []
for row in self.point_store:
if row[VDISP] == vdisp:
orig_data.append(
[row[TEXU], row[TEXV], row[DISPLAYX], row[DISPLAYY]])
orig_data = np.array(orig_data)
uv = orig_data[:, :2]
XYZ = self.geom.model.texcoord2worldcoord(uv)
xy = orig_data[:, 2:4]
assert method in EXTRINSIC_CALIBRATION_METHODS
if method in ('DLT', 'RANSAC DLT'):
ransac = method.startswith('RANSAC')
r = dlt.dlt(XYZ, xy, ransac=ransac)
c1 = CameraModel.load_camera_from_M(
r['pmat'],
width=self.dsc.width,
height=self.dsc.height,
)
_pump_ui()
if 0:
c2 = c1.get_flipped_camera()
# slightly hacky way to find best camera direction
obj = self.geom.model.get_center()
d1 = np.sqrt(np.sum((c1.get_lookat() - obj)**2))
d2 = np.sqrt(np.sum((c2.get_lookat() - obj)**2))
if d1 < d2:
#print 'using normal camera'
camera = c1
else:
print 'using flipped camera'
camera = c2
elif 1:
farr = self.geom.compute_for_camera_view(c1,
what='texture_coords')
_pump_ui()
u = farr[:, :, 0]
good = ~np.isnan(u)
npix = np.sum(np.nonzero(good))
if npix == 0:
print 'using flipped camera, otherwise npix = 0'
camera = c1.get_flipped_camera()
else:
camera = c1
else:
camera = c1
elif method in ['extrinsic only', 'iterative extrinsic only']:
assert self.display_intrinsic_cam is not None, 'need intrinsic calibration'
di = self.dsc.get_display_info()
mirror = None
if 'virtualDisplays' in di:
found = False
for d in di['virtualDisplays']:
if d['id'] == vdisp:
found = True
mirror = d.get('mirror', None)
break
assert found
if mirror is not None:
cami = self.display_intrinsic_cam.get_mirror_camera(
axis=mirror)
else:
cami = self.display_intrinsic_cam
_pump_ui()
if method == 'iterative extrinsic only':
result = fit_extrinsics_iterative(cami, XYZ, xy)
else:
result = fit_extrinsics(cami, XYZ, xy)
_pump_ui()
c1 = result['cam']
if 1:
farr = self.geom.compute_for_camera_view(c1,
what='texture_coords')
_pump_ui()
u = farr[:, :, 0]
good = ~np.isnan(u)
npix = np.sum(np.nonzero(good))
if npix == 0:
print 'using flipped camera, otherwise npix = 0'
camera = c1.get_flipped_camera()
else:
camera = c1
else:
camera = c1
del result
else:
raise ValueError('unknown calibration method %r' % method)
_pump_ui()
projected_points = camera.project_3d_to_pixel(XYZ)
reproj_error = np.sum((projected_points - xy)**2, axis=1)
mre = np.mean(reproj_error)
for row in self.vdisp_store:
if row[VS_VDISP] == vdisp:
row[VS_MRE] = mre
row[VS_CAMERA_OBJECT] = camera
_pump_ui()
self.update_bg_image()
def fit_extrinsics(base_cam,X3d,x2d,geom=None):
assert x2d.ndim==2
assert x2d.shape[1]==2
assert X3d.ndim==2
assert X3d.shape[1]==3
if 0:
fname = 'x2d_'+base_cam.name + '.png'
fname = fname.replace('/','-')
save_point_image(fname, (base_cam.width, base_cam.height), x2d )
#print 'saved pt debug image to',fname
ipts = np.array(x2d,dtype=np.float64)
opts = np.array(X3d,dtype=np.float64)
K = np.array(base_cam.get_K(), dtype=np.float64)
dist_coeffs = np.array( base_cam.get_D(), dtype=np.float64)
retval, rvec, tvec = cv2.solvePnP( opts, ipts,
K,
dist_coeffs)
assert retval
# we get two possible cameras back, figure out which one has objects in front
rmata = rodrigues2matrix( rvec )
intrinsics = base_cam.to_dict()
del intrinsics['Q']
del intrinsics['translation']
del intrinsics['name']
d = intrinsics.copy()
d['translation'] = tvec
d['Q'] = rmata
d['name'] = base_cam.name
cam_model_a = CameraModel.from_dict(d)
mza = np.mean(cam_model_a.project_3d_to_camera_frame(X3d)[:,2])
# don't bother with second - it does not have a valid rotation matrix
if 1:
founda = cam_model_a.project_3d_to_pixel(X3d)
erra = np.mean(np.sqrt(np.sum((founda-x2d)**2, axis=1)))
cam_model = cam_model_a
if 1:
found = cam_model.project_3d_to_pixel(X3d)
orig = x2d
reproj_error = np.sqrt(np.sum((found-orig)**2, axis=1))
cum = np.mean(reproj_error)
mean_cam_z = np.mean(cam_model.project_3d_to_camera_frame(X3d)[:,2])
if (mean_cam_z < 0 or cum > 20) and 0:
# hmm, we have a flipped view of the camera.
print '-'*80,'HACK ON'
center, lookat, up = cam_model.get_view()
#cam2 = cam_model.get_view_camera( -center, lookat, -up )
cam2 = cam_model.get_view_camera( center, lookat, up )
cam2.name=base_cam.name
return fit_extrinsics_iterative(cam2,X3d,x2d, geom=geom)
result = dict(cam=cam_model,
mean_err=cum,
mean_cam_z = mean_cam_z,
)
return result
def test_simple_camera():
center = np.array( (0, 0.0, 5) )
lookat = center + np.array( (0,1,0))
cam = CameraModel.load_camera_simple(fov_x_degrees=90,
eye=center,
lookat=lookat)
def test_pymvg_roundtrip():
from pymvg.camera_model import CameraModel
from pymvg.multi_camera_system import MultiCameraSystem
from flydra_core.reconstruct import Reconstructor
# ----------- with no distortion ------------------------
center1 = np.array( (0, 0.0, 5) )
center2 = np.array( (1, 0.0, 5) )
lookat = np.array( (0,1,0))
cam1 = CameraModel.load_camera_simple(fov_x_degrees=90,
name='cam1',
eye=center1,
lookat=lookat)
cam2 = CameraModel.load_camera_simple(fov_x_degrees=90,
name='cam2',
eye=center2,
lookat=lookat)
mvg = MultiCameraSystem( cameras=[cam1, cam2] )
R = Reconstructor.from_pymvg(mvg)
mvg2 = R.convert_to_pymvg()
cam_ids = ['cam1','cam2']
for distorted in [True,False]:
for cam_id in cam_ids:
v1 = mvg.find2d( cam_id, lookat, distorted=distorted )
v2 = R.find2d( cam_id, lookat, distorted=distorted )
v3 = mvg2.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
assert np.allclose(v1,v3)
# ----------- with distortion ------------------------
cam1dd = cam1.to_dict()
cam1dd['D'] = (0.1, 0.2, 0.3, 0.4, 0.0)
cam1d = CameraModel.from_dict(cam1dd)
cam2dd = cam2.to_dict()
cam2dd['D'] = (0.11, 0.21, 0.31, 0.41, 0.0)
cam2d = CameraModel.from_dict(cam2dd)
mvgd = MultiCameraSystem( cameras=[cam1d, cam2d] )
Rd = Reconstructor.from_pymvg(mvgd)
mvg2d = Rd.convert_to_pymvg()
cam_ids = ['cam1','cam2']
for distorted in [True,False]:
for cam_id in cam_ids:
v1 = mvgd.find2d( cam_id, lookat, distorted=distorted )
v2 = Rd.find2d( cam_id, lookat, distorted=distorted )
v3 = mvg2d.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
assert np.allclose(v1,v3)
# ------------ with distortion at different focal length ------
mydir = os.path.dirname(__file__)
caldir = os.path.join(mydir,'sample_calibration')
print mydir
print caldir
R3 = Reconstructor(caldir)
mvg3 = R3.convert_to_pymvg()
#R4 = Reconstructor.from_pymvg(mvg3)
mvg3b = MultiCameraSystem.from_mcsc( caldir )
for distorted in [True,False]:
for cam_id in R3.cam_ids:
v1 = R3.find2d( cam_id, lookat, distorted=distorted )
v2 = mvg3.find2d( cam_id, lookat, distorted=distorted )
#v3 = R4.find2d( cam_id, lookat, distorted=distorted )
v4 = mvg3b.find2d( cam_id, lookat, distorted=distorted )
assert np.allclose(v1,v2)
#assert np.allclose(v1,v3)
assert np.allclose(v1,v4)
def test_no_duplicate_names():
cam1a = CameraModel.load_camera_simple(name='cam1')
cam1b = CameraModel.load_camera_simple(name='cam1')
cams = [cam1a,cam1b]
nose.tools.assert_raises(ValueError, MultiCameraSystem, cams)
def get_sample_camera():
yaml_str = """header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
height: 494
width: 659
distortion_model: plumb_bob
D:
- -0.331416226762
- 0.143584747016
- 0.00314558656669
- -0.00393597842852
- 0.0
K:
- 516.385667641
- 0.0
- 339.167079537
- 0.0
- 516.125799368
- 227.379935241
- 0.0
- 0.0
- 1.0
R:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
P:
- 444.369750977
- 0.0
- 337.107817516
- 0.0
- 0.0
- 474.186859131
- 225.062742824
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False"""
d = yaml.load(yaml_str)
cam1 = CameraModel.from_dict(d,extrinsics_required=False)
eye = (10,20,30)
lookat = (11,20,30)
up = (0,-1,0)
cam = cam1.get_view_camera(eye, lookat, up)
return cam
def launch_calibration(self, method, vdisp ):
def _pump_ui():
if os.environ.get('RUNNING_NOSE') != '1':
Gtk.main_iteration_do(False) #run once, no block
while Gtk.events_pending(): #run remaining
Gtk.main_iteration()
orig_data = []
for row in self.point_store:
if row[VDISP]==vdisp:
orig_data.append( [ row[TEXU], row[TEXV], row[DISPLAYX], row[DISPLAYY] ] )
orig_data = np.array(orig_data)
uv = orig_data[:,:2]
XYZ = self.geom.model.texcoord2worldcoord(uv)
xy = orig_data[:,2:4]
assert method in EXTRINSIC_CALIBRATION_METHODS
if method in ('DLT','RANSAC DLT'):
ransac = method.startswith('RANSAC')
r = dlt.dlt(XYZ, xy, ransac=ransac )
c1 = CameraModel.load_camera_from_M( r['pmat'],
width=self.dsc.width,
height=self.dsc.height,
)
_pump_ui()
if 0:
c2 = c1.get_flipped_camera()
# slightly hacky way to find best camera direction
obj = self.geom.model.get_center()
d1 = np.sqrt( np.sum( (c1.get_lookat() - obj)**2 ))
d2 = np.sqrt( np.sum( (c2.get_lookat() - obj)**2 ))
if d1 < d2:
#print 'using normal camera'
camera = c1
else:
print 'using flipped camera'
camera = c2
elif 1:
farr = self.geom.compute_for_camera_view( c1,
what='texture_coords' )
_pump_ui()
u = farr[:,:,0]
good = ~np.isnan( u )
npix=np.sum( np.nonzero( good ) )
if npix==0:
print 'using flipped camera, otherwise npix = 0'
camera = c1.get_flipped_camera()
else:
camera = c1
else:
camera = c1
elif method in ['extrinsic only','iterative extrinsic only']:
assert self.display_intrinsic_cam is not None, 'need intrinsic calibration'
di = self.dsc.get_display_info()
mirror = None
if 'virtualDisplays' in di:
found = False
for d in di['virtualDisplays']:
if d['id'] == vdisp:
found = True
mirror = d.get('mirror',None)
break
assert found
if mirror is not None:
cami = self.display_intrinsic_cam.get_mirror_camera(axis=mirror)
else:
cami = self.display_intrinsic_cam
_pump_ui()
if method == 'iterative extrinsic only':
result = fit_extrinsics_iterative(cami,XYZ,xy)
else:
result = fit_extrinsics(cami,XYZ,xy)
_pump_ui()
c1 = result['cam']
if 1:
farr = self.geom.compute_for_camera_view( c1,
what='texture_coords' )
_pump_ui()
u = farr[:,:,0]
good = ~np.isnan( u )
npix=np.sum( np.nonzero( good ) )
if npix==0:
print 'using flipped camera, otherwise npix = 0'
camera = c1.get_flipped_camera()
else:
camera = c1
else:
camera = c1
del result
else:
raise ValueError('unknown calibration method %r'%method)
_pump_ui()
projected_points = camera.project_3d_to_pixel( XYZ )
reproj_error = np.sum( (projected_points - xy)**2, axis=1)
mre = np.mean(reproj_error)
for row in self.vdisp_store:
if row[VS_VDISP]==vdisp:
row[VS_MRE] = mre
row[VS_CAMERA_OBJECT] = camera
_pump_ui()
self.update_bg_image()
def test_basic_dlt():
results = dlt.dlt(XYZ, xy, ransac=False)
assert results['mean_reprojection_error'] < 6.0
c1 = CameraModel.load_camera_from_M( results['pmat'] )
def from_mcsc(cls, dirname ):
'''create MultiCameraSystem from output directory of MultiCamSelfCal'''
# FIXME: This is a bit convoluted because it's been converted
# from multiple layers of internal code. It should really be
# simplified and cleaned up.
do_normalize_pmat=True
all_Pmat = {}
all_Res = {}
all_K = {}
all_distortion = {}
opj = os.path.join
with open(opj(dirname,'camera_order.txt'),mode='r') as fd:
cam_ids = fd.read().strip().split('\n')
with open(os.path.join(dirname,'Res.dat'),'r') as res_fd:
for i, cam_id in enumerate(cam_ids):
fname = 'camera%d.Pmat.cal'%(i+1)
pmat = np.loadtxt(opj(dirname,fname)) # 3 rows x 4 columns
if do_normalize_pmat:
pmat_orig = pmat
pmat = normalize_M(pmat)
all_Pmat[cam_id] = pmat
all_Res[cam_id] = map(int,res_fd.readline().split())
# load non linear parameters
rad_files = [ f for f in os.listdir(dirname) if f.endswith('.rad') ]
for cam_id_enum, cam_id in enumerate(cam_ids):
filename = os.path.join(dirname,
'basename%d.rad'%(cam_id_enum+1,))
if os.path.exists(filename):
K, distortion = parse_radfile(filename)
all_K[cam_id] = K
all_distortion[cam_id] = distortion
else:
if len(rad_files):
raise RuntimeError(
'.rad files present but none named "%s"'%filename)
warnings.warn('no non-linear data (e.g. radial distortion) '
'in calibration for %s'%cam_id)
all_K[cam_id] = None
all_distortion[cam_id] = None
cameras = []
for cam_id in cam_ids:
w,h = all_Res[cam_id]
Pmat = all_Pmat[cam_id]
M = Pmat[:,:3]
K,R = my_rq(M)
if not is_rotation_matrix(R):
# RQ may return left-handed rotation matrix. Make right-handed.
R2 = -R
K2 = -K
assert np.allclose(np.dot(K2,R2), np.dot(K,R))
K,R = K2,R2
P = np.zeros((3,4))
P[:3,:3] = K
KK = all_K[cam_id] # from rad file or None
distortion = all_distortion[cam_id]
# (ab)use PyMVG's rectification to do coordinate transform
# for MCSC's undistortion.
# The intrinsic parameters used for 3D -> 2D.
ex = P[0,0]
bx = P[0,2]
Sx = P[0,3]
ey = P[1,1]
by = P[1,2]
Sy = P[1,3]
if KK is None:
rect = np.eye(3)
KK = P[:,:3]
else:
# Parameters used to define undistortion coordinates.
fx = KK[0,0]
fy = KK[1,1]
cx = KK[0,2]
cy = KK[1,2]
rect = np.array([[ ex/fx, 0, (bx+Sx-cx)/fx ],
[ 0, ey/fy, (by+Sy-cy)/fy ],
[ 0, 0, 1 ]]).T
if distortion is None:
distortion = np.zeros((5,))
C = center(Pmat)
rot = R
t = -np.dot(rot, C)[:,0]
d = {'width':w,
'height':h,
'P':P,
'K':KK,
'R':rect,
'translation':t,
'Q':rot,
'D':distortion,
'name':cam_id,
}
cam = CameraModel.from_dict(d)
cameras.append( cam )
return MultiCameraSystem( cameras=cameras )
def from_pymvg_str(cls, buf):
d = json.loads(buf)
assert d['__pymvg_file_version__']=='1.0'
cam_dict_list = d['camera_system']
cams = [CameraModel.from_dict(cd) for cd in cam_dict_list]
return MultiCameraSystem( cameras=cams )
def fit_extrinsics(base_cam,X3d,x2d,geom=None):
assert x2d.ndim==2
assert x2d.shape[1]==2
assert X3d.ndim==2
assert X3d.shape[1]==3
if 0:
fname = 'x2d_'+base_cam.name + '.png'
fname = fname.replace('/','-')
save_point_image(fname, (base_cam.width, base_cam.height), x2d )
#print 'saved pt debug image to',fname
ipts = np.array(x2d,dtype=np.float64)
opts = np.array(X3d,dtype=np.float64)
K = np.array(base_cam.get_K(), dtype=np.float64)
dist_coeffs = np.array( base_cam.get_D(), dtype=np.float64)
retval, rvec, tvec = cv2.solvePnP( opts, ipts,
K,
dist_coeffs)
assert retval
# we get two possible cameras back, figure out which one has objects in front
rmata = rodrigues2matrix( rvec )
intrinsics = base_cam.to_dict()
del intrinsics['Q']
del intrinsics['translation']
del intrinsics['name']
d = intrinsics.copy()
d['translation'] = tvec
d['Q'] = rmata
d['name'] = base_cam.name
cam_model_a = CameraModel.from_dict(d)
mza = np.mean(cam_model_a.project_3d_to_camera_frame(X3d)[:,2])
# don't bother with second - it does not have a valid rotation matrix
if 1:
founda = cam_model_a.project_3d_to_pixel(X3d)
erra = np.mean(np.sqrt(np.sum((founda-x2d)**2, axis=1)))
cam_model = cam_model_a
if 1:
found = cam_model.project_3d_to_pixel(X3d)
orig = x2d
reproj_error = np.sqrt(np.sum((found-orig)**2, axis=1))
cum = np.mean(reproj_error)
mean_cam_z = np.mean(cam_model.project_3d_to_camera_frame(X3d)[:,2])
if (mean_cam_z < 0 or cum > 20) and 0:
# hmm, we have a flipped view of the camera.
print '-'*80,'HACK ON'
center, lookat, up = cam_model.get_view()
#cam2 = cam_model.get_view_camera( -center, lookat, -up )
cam2 = cam_model.get_view_camera( center, lookat, up )
cam2.name=base_cam.name
return fit_extrinsics_iterative(cam2,X3d,x2d, geom=geom)
result = dict(cam=cam_model,
mean_err=cum,
mean_cam_z = mean_cam_z,
)
return result
