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 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 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 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 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 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 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 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 build_multi_camera_system(cameras, no_distortion=False): """ Build a multi-camera system with pymvg package for triangulation Args: cameras: list of camera parameters Returns: cams_system: a multi-cameras system """ pymvg_cameras = [] for (name, camera) in cameras: R, T, f, c, k, p = unfold_camera_param(camera, avg_f=False) camera_matrix = np.array( [[f[0], 0, c[0]], [0, f[1], c[1]], [0, 0, 1]], dtype=float) proj_matrix = np.zeros((3, 4)) proj_matrix[:3, :3] = camera_matrix distortion = np.array([k[0], k[1], p[0], p[1], k[2]]) distortion.shape = (5,) T = -np.matmul(R, T) M = camera_matrix.dot(np.concatenate((R, T), axis=1)) camera = CameraModel.load_camera_from_M( M, name=name, distortion_coefficients=None if no_distortion else distortion) pymvg_cameras.append(camera) return MultiCameraSystem(pymvg_cameras)
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 check_built_from_M(cam_opts): """check that M is preserved in load_camera_from_M() factory""" cam_orig = _build_test_camera(**cam_opts) M_orig = cam_orig.get_M() cam = CameraModel.load_camera_from_M( M_orig ) assert np.allclose( cam.get_M(), M_orig)
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 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 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()