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 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 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 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_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 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 __init__(self, camera_number, intrinsic_properties,
extrinsic_properties):
self.number = camera_number
self.frames = None
r = extrinsic_properties['rotationVector']
self.Rvec = get_3d_vector(r)
self.R = cv2.Rodrigues(self.Rvec)[0]
translation = extrinsic_properties['translationVector']
self.T = np.array(get_3d_vector(translation))
matrix_parameters = intrinsic_properties['calibrationMatrix']
self.camera_matrix = np.matrix(
[[matrix_parameters['fx'], 0, matrix_parameters['cx']],
[0, matrix_parameters['fy'], matrix_parameters['cy']], [0, 0, 1]],
dtype=np.float64)
coef = intrinsic_properties['distortionCoefficients']
self.distortion_coefficients = np.float64(
[coef['k1'], coef['k2'], coef['p1'], coef['p2'], coef['k3']])
self.reprojectionError = float(
intrinsic_properties['reprojectionError'])
self.image_size = (640, 480)
self.model = CameraModel.from_dict({
'translation':
self.T,
'Q':
self.R,
'width':
self.image_size[0],
'height':
self.image_size[1],
'P':
np.concatenate((self.camera_matrix, np.zeros((3, 1))), axis=1),
'K':
self.camera_matrix,
'D':
self.distortion_coefficients,
'name':
'Camera-' + str(camera_number),
'R':
None
})
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 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 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 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 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 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
def check_dict_roundtrip(cam_opts):
cam = _build_test_camera(**cam_opts)
d = cam.to_dict()
cam2 = CameraModel.from_dict(d)
assert cam==cam2
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_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)
