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)