def test_formats(): urlyaml = io.decode_url('package://motmot_ros_utils/tests/Basler_21029383.yaml') urlrad = io.decode_url('package://motmot_ros_utils/tests/Basler_21029383.rad') rad = '/tmp/test.rad' formats.camera_calibration_yaml_to_radfile(urlyaml, rad) assert open(urlrad,'r').read() == open(rad,'r').read()
def __init__(self, flydra_calib, laser_pkl, undistort_flydra_points, visualize): self.flydra_calib = flydra_calib self.laser_pkl = laser_pkl self.undistort_flydra_points = undistort_flydra_points self.visualize = visualize #make sure the flydra cameras have associated calibration information. We might #not actually use the undistorted points (depends on the --undistort argument) name_map = MultiCalSelfCam.get_camera_names_map(self.flydra_calib) for c in MultiCalSelfCam.read_calibration_names(self.flydra_calib): camera_calibration_yaml_to_radfile( decode_url('package://flycave/calibration/cameras/%s.yaml' % c), os.path.join(self.flydra_calib,name_map[c])) #publish the camera positions and the inlier set from the flydra calibraion MultiCalSelfCam.publish_calibration_points(self.flydra_calib, topic_base='/flydra') print '*'*80 print 'loaded original calibration from', self.flydra_calib print '*'*80 self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser = AllPointPickle() #load default laser points self.load_laser_points()
def calibrate(rerun_mcsc, calib_config, display_server_numbers): config = yaml.load(open(decode_url(calib_config))) for n in [int(i) for i in display_server_numbers]: src = DS_PKL + "/ds%d" % n ds = '/display_server%d' % n ids = [ds] + config['display_servers'][ds] a = AllPointPickle() a.initilize_from_directory(src) print "*" * 20 print src print "*" * 20 if rerun_mcsc: mcsc = MultiCalSelfCam(src) mcsc.create_from_cams(cam_ids=ids, cam_resolutions=a.resolutions.copy(), cam_points=a.results.copy(), cam_calibrations={}, num_cameras_fill=0) dest = mcsc.execute(blocking=True, copy_files=True, silent=False) else: dest = src + "/result" MultiCalSelfCam.publish_calibration_points(dest, topic_base='/ds%d' % n)
def __init__(self, flydra_calib, laser_pkl, undistort_flydra_points, visualize): self.flydra_calib = flydra_calib self.laser_pkl = laser_pkl self.undistort_flydra_points = undistort_flydra_points self.visualize = visualize # make sure the flydra cameras have associated calibration information. We might # not actually use the undistorted points (depends on the --undistort argument) name_map = MultiCalSelfCam.get_camera_names_map(self.flydra_calib) for c in MultiCalSelfCam.read_calibration_names(self.flydra_calib): camera_calibration_yaml_to_radfile( decode_url("package://flycave/calibration/cameras/%s.yaml" % c), os.path.join(self.flydra_calib, name_map[c]), ) # publish the camera positions and the inlier set from the flydra calibraion MultiCalSelfCam.publish_calibration_points(self.flydra_calib, topic_base="/flydra") print "*" * 80 print "loaded original calibration from", self.flydra_calib print "*" * 80 self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser = AllPointPickle() # load default laser points self.load_laser_points()
def calibrate(rerun_mcsc,calib_config,display_server_numbers): config = yaml.load(open(decode_url(calib_config))) for n in [int(i) for i in display_server_numbers]: src = DS_PKL + "/ds%d" % n ds = '/display_server%d' % n ids = [ds] + config['display_servers'][ds] a = AllPointPickle() a.initilize_from_directory(src) print "*"*20 print src print "*"*20 if rerun_mcsc: mcsc = MultiCalSelfCam(src) mcsc.create_from_cams( cam_ids=ids, cam_resolutions=a.resolutions.copy(), cam_points=a.results.copy(), cam_calibrations={}, num_cameras_fill=0) dest = mcsc.execute(blocking=True, copy_files=True, silent=False) else: dest = src + "/result" MultiCalSelfCam.publish_calibration_points( dest, topic_base='/ds%d' % n)
def load_laser_points(self, path=None): if not path: path = self.laser_pkl #we need all the laser points for cylinder fitting self.laser.initilize_from_directory(self.laser_pkl) print "number of laser points", self.laser.num_points #get all points visible in 2 or more cameras #and use the flydra calibration to get the 3d coords pts = self.laser.get_points_in_cameras(self.fly.get_cam_ids()) self.xyz = np.array([self.fly.find3d(pt,return_line_coords=False, undistort=True) for pt in pts]) create_point_cloud_message_publisher( self.xyz, topic_name='/flydracalib/points', publish_now=True, latch=True) create_pcd_file_from_points( decode_url('package://flycave/calibration/pcd/flydracyl.pcd'), self.xyz) if self.visualize: plt.figure() show_pointcloud_3d_plot(self.xyz) fig = plt.figure() show_pointcloud_2d_plots(self.xyz,fig=fig)
def test_decode_file(): PATH = '/testmotmotrosutils' DATA = '123' b = xmlrpclib.Binary(data=DATA) rospy.set_param(PATH,b) a = io.decode_file_from_parameter_server(PATH) b = io.decode_url('parameter://'+PATH) assert a == b d = open(a).read() assert d == DATA
import roslib roslib.load_manifest('freemoovr') roslib.load_manifest('motmot_ros_utils') import rospy from calib.io import MultiCalSelfCam, AllPointPickle from calib.visualization import create_pcd_file_from_points from rosutils.io import decode_url import flydra import flydra.reconstruct import flydra.align rospy.init_node('calibone', anonymous=True) DS1_PKL = decode_url('package://flycave/calibration/triplets/ds1') DS1_CALIB = decode_url('package://flycave/calibration/triplets/ds1/result') LASER_PKL = decode_url('package://flycave/calibration/laser') FLYDRA_CALIB = decode_url('package://flycave/calibration/flydra') #test mcsc wrapper if 0: a = AllPointPickle() a.initilize_from_directory(DS1_PKL) mcsc = MultiCalSelfCam(DS1_CALIB) mcsc.create_from_cams(cam_ids=[], cam_resolutions=a.resolutions.copy(), cam_points=a.results.copy(), cam_calibrations={}, num_cameras_fill=0)
import roslib; roslib.load_manifest('flyvr') roslib.load_manifest('motmot_ros_utils') import rospy from calib.io import MultiCalSelfCam, AllPointPickle from calib.visualization import create_pcd_file_from_points from rosutils.io import decode_url import flydra import flydra.reconstruct import flydra.align rospy.init_node('calibone', anonymous=True) DS1_PKL = decode_url('package://flycave/calibration/triplets/ds1') DS1_CALIB = decode_url('package://flycave/calibration/triplets/ds1/result') LASER_PKL = decode_url('package://flycave/calibration/laser') FLYDRA_CALIB = decode_url('package://flycave/calibration/flydra') #test mcsc wrapper if 0: a = AllPointPickle() a.initilize_from_directory(DS1_PKL) mcsc = MultiCalSelfCam(DS1_CALIB) mcsc.create_from_cams( cam_ids=[], cam_resolutions=a.resolutions.copy(), cam_points=a.results.copy(), cam_calibrations={},
rospy.sleep(0.1) if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument( '--calib-config', type=str, default='package://flycave/conf/calib-all.yaml', help='path to calibration configuration yaml file') argv = rospy.myargv() args = parser.parse_args(argv[1:]) rospy.init_node('generate_masks') config = {} conffile = decode_url(args.calib_config) with open(conffile, 'r') as f: config = yaml.load(f) for k in ("display_servers", "mask_dir", "tracking_cameras"): if not config.has_key(k): parser.error("malformed calibration config, missing %s" % k) mask_dir = decode_url(config["mask_dir"]) if not os.path.isdir(mask_dir): parser.error("mask dir not found") display_servers = config["display_servers"] all_cams = [] for cam in config["tracking_cameras"]: all_cams.append(cam)
def do_exr(self, interp_method, do_luminance, gamma, blend_curve): exrs = {ds:{} for ds in self.display_servers} do_xyz = ["x","y","z"] exrs_uvl = ["u","v","l","ui","vi","li"] comment = 'created from %s' % (", ".join(self.filenames),) if self.smoothed is not None: comment += '. MLS smoothed by %f' % self.smoothed if self.flydra_calib: comment += '. 3D reconstruction from %s' % self.flydra_calib comment += '. Interpolation method %s' % interp_method comment += '. Luminance blend %s (gamma: %.2f curve: %.2f)' % (do_luminance, gamma, blend_curve) rospy.loginfo(comment) if do_luminance: blender = blend.Blender( True or self.visualize, os.getcwd(), debug_exr=self.debug, exr_comments=comment ) def alloc_exr_mask(ds, name): dsc = self.dscs[ds] exrs[ds][name] = { "exr":np.zeros((768,1024)), } exrs[ds][name]["exr"].fill(np.nan) def update_mask(ds, name, val, vdispmask=None): valid_val = ~np.isnan(val) if vdispmask is not None: valid = np.logical_or(vdispmask,valid_val) else: valid = valid_val exrs[ds][name]["exr"][valid] = val[valid] all_3d = [] for ds in self.display_servers: dsc = self.dscs[ds] if self.visualize: cv2.namedWindow(ds) img = self.cvimgs[ds] for ax in do_xyz + exrs_uvl: alloc_exr_mask(ds, ax) ds_3d = [] for vdisp in self.data[ds]: vdispmask = dsc.get_virtual_display_mask(vdisp, squeeze=True) vdisp_3d = [] vdisp_2d = [] vdisp_lum = [] if self.visualize: arr = np.zeros((768,1024,4)) arr.fill(np.nan) for xyz,pixel,lum in self.data[ds][vdisp]: #just do one vdisp vdisp_2d.append(pixel) vdisp_3d.append(xyz) vdisp_lum.append(lum) if self.visualize: col = pixel[0] row = pixel[1] add_crosshairs_to_nparr(img, row=row, col=col, chan=2, sz=1) arr[row-2:row+2,col-2:col+2,X_INDEX] = xyz[0] arr[row-2:row+2,col-2:col+2,Y_INDEX] = xyz[1] arr[row-2:row+2,col-2:col+2,Z_INDEX] = xyz[2] arr[row-2:row+2,col-2:col+2,L_INDEX] = lum ds_3d.extend(vdisp_3d) vdisp_lum_arr = np.array(vdisp_lum, dtype=np.float) vdisp_2d_arr = np.array(vdisp_2d, dtype=np.float) vdisp_3d_arr = np.array(vdisp_3d, dtype=np.float) #construct the interpoolated geometry (uv) <-> 3d (xyz) mapping. ui,vi = self.interpolate_points( vdisp_3d_arr, vdisp_2d_arr, dsc, interp_method) #interpolate luminance li = interpolate_pixel_cords( points_2d=vdisp_2d_arr, values_1d=vdisp_lum_arr, img_width=dsc.width, img_height=dsc.height, method=interp_method) update_mask(ds, "ui", ui, vdispmask) update_mask(ds, "vi", vi, vdispmask) update_mask(ds, "li", li, vdispmask) #and keep an unterpolated copy u,v = self.interpolate_points( vdisp_3d_arr, vdisp_2d_arr, dsc, "none") update_mask(ds, "u", u, vdispmask) update_mask(ds, "v", v, vdispmask) if self.debug: for axnum,ax in enumerate(do_xyz): update_mask(ds, ax, interpolate_pixel_cords( points_2d=vdisp_2d_arr, values_1d=vdisp_3d_arr[:,axnum], img_width=dsc.width, img_height=dsc.height, method="none") ) if self.visualize: self.show_vdisp_points(arr, ds, ui, vi, vdisp) if do_luminance: blender.add_display_server( ds, dsc, exrs[ds]["u"]["exr"].astype(np.float32),exrs[ds]["v"]["exr"].astype(np.float32), exrs[ds]["ui"]["exr"].astype(np.float32),exrs[ds]["vi"]["exr"].astype(np.float32), ) if self.visualize: cv2.imshow(ds, img) if self.debug: create_pcd_file_from_points(decode_url('%s.pcd' % ds),ds_3d) all_3d.extend(ds_3d) if self.debug: create_pcd_file_from_points(decode_url('all.pcd'),all_3d) create_point_cloud_message_publisher(all_3d,'/calibration/points',publish_now=True, latch=True) if do_luminance: blended = blender.blend(gamma, blend_curve) for ds in self.display_servers: dsc = self.dscs[ds] if do_luminance: final_li = blended[ds] else: #set the luminance channel to 1 (no blending) inside the viewport, #0 (off) outside final_li = np.ones_like(exrs[ds]["ui"]["exr"]) final_li[np.isnan(exrs[ds]["ui"]["exr"])] = 0 #in our shader convention -1 means no data, not NaN exrs[ds]["ui"]["exr"][np.isnan(exrs[ds]["ui"]["exr"])] = -1 exrs[ds]["vi"]["exr"][np.isnan(exrs[ds]["vi"]["exr"])] = -1 final_ui = exrs[ds]["ui"]["exr"] final_vi = exrs[ds]["vi"]["exr"] if self.visualize: plt.figure() plt.imshow(final_ui) plt.colorbar() plt.title('%s U' % ds) plt.figure() plt.imshow(final_vi) plt.colorbar() plt.title('%s V' % ds) plt.figure() plt.imshow(final_li) plt.colorbar() plt.title('%s L' % ds) exrpath = "%s.exr" % ds exr.save_exr( exrpath, r=final_ui, g=final_vi, b=final_li, comments=comment) if self.debug: exrs[ds]["u"]["exr"][np.isnan(exrs[ds]["u"]["exr"])] = -1 exrs[ds]["v"]["exr"][np.isnan(exrs[ds]["v"]["exr"])] = -1 final_u = exrs[ds]["u"]["exr"] final_v = exrs[ds]["v"]["exr"] exr.save_exr( "%s.nointerp.exr" % ds, r=final_u, g=final_v, b=np.zeros_like(final_u), comments=comment) #save the resulting geometry to the parameter server if self.update_parameter_server: geom = self.geom.to_geom_dict() dsc.set_geometry(geom) dsc.set_binary_exr(exrpath) rospy.loginfo("updated parameter server") sh.rosnode("kill","/%s"%ds) rospy.loginfo("restarted server")
cal = Calibrator( visualize=args.visualize, debug=args.debug, new_reconstructor=args.reconstructor, mask_out=False, update_parameter_server=args.update) tmp=[] [tmp.extend(_) for _ in args.calibration] cal_files = [] [cal_files.extend(glob.glob(_)) for _ in tmp] rospy.loginfo('cal_files: %r'%cal_files) if len(cal_files): #multiple bag files for c in cal_files: fn = decode_url(c) assert os.path.exists(fn) cal.load(fn, args.no_wait_display_server) else: rospy.logfatal('No data. Specify inputs with --calibration <file.bag>') rospy.signal_shutdown('no data') sys.exit(1) if args.smooth: cal.smooth(args.smooth) cal.do_exr(args.interpolation, args.luminance, args.gamma, args.blend_curve) if cal.visualize: plt.show()
def generate_exrs( self, display_server_numbers, prefer_parameter_server_properties=False, filt_method="linear", mask_out=False, mask_fill_value=np.nan, ): # so, we can successfully recover a cylinder. Find the pixel coords of those points in the # projector cameras, and then, via the projector calibration, find the corresponding pixel # in the projector for dsnum in display_server_numbers: dsname = "ds%d" % dsnum # use the display server calibration to get the projector pixel coordinate ds = flydra.reconstruct.Reconstructor( cal_source=decode_url("package://flycave/calibration/triplets/%s/result" % dsname) ) flydra_cams = self.fly.get_cam_ids() ds_cams = [c for c in ds.get_cam_ids() if not c.startswith("display_server")] flydra_points_3d = [] ds_points_2d = [] ds_points_3d = [] arr = np.zeros((768, 1024, 2)) arr.fill(np.nan) dsc = display_client.DisplayServerProxy( "/display_server%d" % dsnum, wait=False, prefer_parameter_server_properties=prefer_parameter_server_properties, ) # all points visible in 2 or more cameras for pts in self.laser.get_points_in_cameras(): flydra_ds_points_2d = [] ds_ds_points_2d = [] for pt in pts: cam, _ = pt if cam in flydra_cams: flydra_ds_points_2d.append(pt) if cam in ds_cams: ds_ds_points_2d.append(pt) # need at least 2 of each for 3D reconstruction in each if len(flydra_ds_points_2d) >= 2 and len(ds_ds_points_2d) >= 2: # get the projector coord ds_3d = ds.find3d(ds_ds_points_2d, return_line_coords=False) ds_2d = ds.find2d("display_server%d" % dsnum, ds_3d) # check bounds are realistic # FIXME: have I swapped u/v here???? # FIXME: can I make this check better??? u, v = ds_2d if u > dsc.width or v > dsc.height or u < 0 or v < 0: continue try: # get the real 3D coord flydra_3d = self.fly.find3d( flydra_ds_points_2d, return_line_coords=False, undistort=self.undistort_flydra_points ) x, y, z = flydra_3d # this is just a debug image for asessing coverage arr[v - 2 : v + 2, u - 2 : u + 2, Y_INDEX] = y arr[v - 2 : v + 2, u - 2 : u + 2, X_INDEX] = x flydra_points_3d.append(flydra_3d) ds_points_3d.append(ds_3d) ds_points_2d.append(ds_2d) except IndexError: print "SHIT?" * 10 pass create_pcd_file_from_points( decode_url("package://flycave/calibration/pcd/%sflydra3d.pcd" % dsname), flydra_points_3d ) create_pcd_file_from_points(decode_url("package://flycave/calibration/pcd/%s3d.pcd" % dsname), ds_points_3d) flydra_points_3d_arr = np.array(flydra_points_3d) ds_points_2d_arr = np.array(ds_points_2d) ds_points_3d_arr = np.array(ds_points_3d) print "%s constructed from %d 2D coords mapping to %d 3D points" % ( dsname, len(ds_points_2d), len(flydra_points_3d_arr), ) # make EXR files via smoothing reprojected 3d points in the projecor view new = self.recon.move_cloud(flydra_points_3d_arr) create_point_cloud_message_publisher( new, topic_name="/flydracalib/%spoints2" % dsname, publish_now=True, latch=True ) uv = self.recon.in_cylinder_coordinates(new) u0 = interpolate_pixel_cords( ds_points_2d_arr, uv[:, 0], img_width=dsc.width, img_height=dsc.height, method=filt_method ) v0 = interpolate_pixel_cords( ds_points_2d_arr, uv[:, 1], img_width=dsc.width, img_height=dsc.height, method=filt_method ) u0nonan = np.nan_to_num(u0) v0nonan = np.nan_to_num(v0) if u0nonan.max() > 1 or u0nonan.min() > 0: # the cubic interpolate function is sensitive to step changes, and introduces quite large # errors in smoothing. Crudely replace those values with invalid (nan) print "replacing out of range errors in smoothed u" u0[u0 > 1] = np.nan u0[u0 < 0] = np.nan if v0nonan.max() > 1 or v0nonan.min() > 0: # the cubic interpolate function is sensitive to step changes, and introduces quite large # errors in smoothing. Crudely replace those values with invalid (nan) print "replacing out of range errors in smoothed u" v0[v0 > 1] = np.nan v0[v0 < 0] = np.nan if mask_out: # mask out invalid parts if the convex hull doesnt work.... # mask is usually used for *= with an image, but here we will use boolean indexing to fill # invalid areas with nan # ... so fiddle the mask a little mask = dsc.get_virtual_display_mask("vdisp").squeeze() mask = ~mask u0[mask] = mask_fill_value v0[mask] = mask_fill_value if self.visualize: plt.figure() plt.subplot(211) plt.imshow(arr[:, :, X_INDEX]) plt.colorbar() plt.title("%s X" % dsname) plt.subplot(212) plt.imshow(u0) plt.title("%s U" % dsname) plt.colorbar() plt.figure() plt.subplot(211) plt.imshow(arr[:, :, Y_INDEX]) plt.colorbar() plt.title("%s Y" % dsname) plt.subplot(212) plt.imshow(v0) plt.colorbar() plt.title("%s V" % dsname) # save the exr file, -1 means no data, not NaN u0[np.isnan(u0)] = -1 v0[np.isnan(v0)] = -1 exrpath = decode_url("package://flycave/calibration/exr/%s.exr" % dsname) exr.save_exr(exrpath, r=u0, g=v0, b=np.zeros_like(u0)) # save the resulting geometry to the parameter server geom = self.recon.get_geom_dict() dsc.set_geometry(geom) dsc.set_binary_exr(exrpath)
import yaml import argparse import os.path import roslib roslib.load_manifest('flyvr') roslib.load_manifest('motmot_ros_utils') import rospy from calib.io import MultiCalSelfCam, AllPointPickle from rosutils.io import decode_url #FIXME: should use the single pickle file from the collect points part... DS_PKL = decode_url('package://flycave/calibration/triplets/') def calibrate(rerun_mcsc, calib_config, display_server_numbers): config = yaml.load(open(decode_url(calib_config))) for n in [int(i) for i in display_server_numbers]: src = DS_PKL + "/ds%d" % n ds = '/display_server%d' % n ids = [ds] + config['display_servers'][ds] a = AllPointPickle() a.initilize_from_directory(src) print "*" * 20 print src print "*" * 20
self.xyz = np.array([self.fly.find3d(pt,return_line_coords=False, undistort=self.undistort_flydra_points) for pt in pts]) def export(self,fname): print 'saving to',fname create_pcd_file_from_points(fname, self.xyz) print 'done saving' if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument( '--laser-pkldir', type=str, default='package://flycave/calibration/laser', help='path to dir containing result.pkl, resolution.pkl, etc') parser.add_argument( '--flydra-calib', type=str, default='package://flycave/calibration/flydra', help='path to flydra multicamselfcal result dir') parser.add_argument( '--output', type=str, default=None) parser.add_argument('--undistort-radial', default=False, action='store_true') args = parser.parse_args() c = Exporter( decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial) if args.output is None: args.output = decode_url('package://flycave/calibration/pcd/flydracyl.pcd') c.export(args.output)
def test_io(): print io.decode_url('~/Documents') print io.decode_url('/tmp') print io.decode_url('file:///home/strawlab') print io.decode_url('package://vros_display/foo/bar') print io.decode_url('package://vros_display/calib/${NODE_NAME}.xml') print io.decode_url('${ROS_HOME}/foo')
def export(self, fname): print "saving to", fname create_pcd_file_from_points(fname, self.xyz) print "done saving" if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument( "--laser-pkldir", type=str, default="package://flycave/calibration/laser", help="path to dir containing result.pkl, resolution.pkl, etc", ) parser.add_argument( "--flydra-calib", type=str, default="package://flycave/calibration/flydra", help="path to flydra multicamselfcal result dir", ) parser.add_argument("--output", type=str, default=None) parser.add_argument("--undistort-radial", default=False, action="store_true") args = parser.parse_args() c = Exporter(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial) if args.output is None: args.output = decode_url("package://flycave/calibration/pcd/flydracyl.pcd") c.export(args.output)
help='path to flydra multicamselfcal result dir') parser.add_argument( '--inlier-dir', type=str, default=None, help='path to directory containing Xe.dat, Re.dat, Ce.dat') parser.add_argument( '--parameter-server-properties', default=False, action='store_true', help='get display server properties (height, width, etc) from the parameter server. '\ 'this means the display server does not need to be running.') parser.add_argument( '--display-server-numbers', type=str, default='0,1,3', help='comma separated list of display server numbers') parser.add_argument('--visualize', default=False, action='store_true') args = parser.parse_args() c = Calibrator( decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.visualize, args.inlier_dir) c.generate_exrs( [int(i) for i in args.display_server_numbers.split(',')], prefer_parameter_server_properties=args.parameter_server_properties, filt_method='linear', mask_out=False, mask_fill_value=np.nan) if args.visualize: plt.show() #print 'now spinning forever...'
def generate_exrs(self, display_server_numbers, prefer_parameter_server_properties=False, filt_method='linear', mask_out=False, mask_fill_value=np.nan): #so, we can successfully recover a cylinder. Find the pixel coords of those points in the #projector cameras, and then, via the projector calibration, find the corresponding pixel #in the projector for dsnum in display_server_numbers: dsname = "ds%d" % dsnum #use the display server calibration to get the projector pixel coordinate ds = flydra.reconstruct.Reconstructor( cal_source=decode_url('package://flycave/calibration/triplets/%s/result' % dsname)) flydra_cams = self.fly.get_cam_ids() ds_cams = [c for c in ds.get_cam_ids() if not c.startswith('display_server')] flydra_points_3d = [] ds_points_2d = [] ds_points_3d = [] arr = np.zeros((768,1024,2)) arr.fill(np.nan) dsc = display_client.DisplayServerProxy("/display_server%d" % dsnum, wait=False, prefer_parameter_server_properties=prefer_parameter_server_properties) #all points visible in 2 or more cameras for pts in self.laser.get_points_in_cameras(): flydra_ds_points_2d = [] ds_ds_points_2d = [] for pt in pts: cam, _ = pt if cam in flydra_cams: flydra_ds_points_2d.append(pt) if cam in ds_cams: ds_ds_points_2d.append(pt) #need at least 2 of each for 3D reconstruction in each if len(flydra_ds_points_2d) >= 2 and len(ds_ds_points_2d) >= 2: #get the projector coord ds_3d = ds.find3d(ds_ds_points_2d,return_line_coords=False) ds_2d = ds.find2d('display_server%d' % dsnum,ds_3d) #check bounds are realistic #FIXME: have I swapped u/v here???? #FIXME: can I make this check better??? u,v = ds_2d if u > dsc.width or v > dsc.height or u < 0 or v < 0: continue try: #get the real 3D coord flydra_3d = self.fly.find3d(flydra_ds_points_2d,return_line_coords=False, undistort=True) x,y,z = flydra_3d #this is just a debug image for asessing coverage arr[v-2:v+2,u-2:u+2,Y_INDEX] = y arr[v-2:v+2,u-2:u+2,X_INDEX] = x flydra_points_3d.append(flydra_3d) ds_points_3d.append(ds_3d) ds_points_2d.append(ds_2d) except IndexError: print "SHIT?"*10 pass create_pcd_file_from_points( decode_url('package://flycave/calibration/pcd/%sflydra3d.pcd' % dsname), flydra_points_3d) create_pcd_file_from_points( decode_url('package://flycave/calibration/pcd/%s3d.pcd' % dsname), ds_points_3d) flydra_points_3d_arr = np.array(flydra_points_3d) ds_points_2d_arr = np.array(ds_points_2d) ds_points_3d_arr = np.array(ds_points_3d) print "%s constructed from %d 2D coords mapping to %d 3D points" % (dsname, len(ds_points_2d), len(flydra_points_3d_arr)) create_point_cloud_message_publisher( flydra_points_3d_arr, topic_name='/flydracalib/%spoints2' % dsname, publish_now=True, latch=True) uv = self.geom.worldcoord2texcoord(flydra_points_3d_arr) u0 = interpolate_pixel_cords( ds_points_2d_arr, uv[:,0], img_width=dsc.width, img_height=dsc.height, method=filt_method) v0 = interpolate_pixel_cords( ds_points_2d_arr, uv[:,1], img_width=dsc.width, img_height=dsc.height, method=filt_method) u0nonan = np.nan_to_num(u0) v0nonan = np.nan_to_num(v0) if u0nonan.max() > 1 or u0nonan.min() > 0: #the cubic interpolate function is sensitive to step changes, and introduces quite large #errors in smoothing. Crudely replace those values with invalid (nan) print 'replacing out of range errors in smoothed u' u0[u0>1] = np.nan u0[u0<0] = np.nan if v0nonan.max() > 1 or v0nonan.min() > 0: #the cubic interpolate function is sensitive to step changes, and introduces quite large #errors in smoothing. Crudely replace those values with invalid (nan) print 'replacing out of range errors in smoothed u' v0[v0>1] = np.nan v0[v0<0] = np.nan if mask_out: #mask out invalid parts if the convex hull doesnt work.... #mask is usually used for *= with an image, but here we will use boolean indexing to fill #invalid areas with nan #... so fiddle the mask a little mask = dsc.get_virtual_display_mask('vdisp').squeeze() mask = ~mask u0[mask] = mask_fill_value v0[mask] = mask_fill_value if self.visualize: plt.figure() plt.subplot(211) plt.imshow(arr[:,:,X_INDEX]) plt.colorbar() plt.title('%s X' % dsname) plt.subplot(212) plt.imshow(u0) plt.title('%s U' % dsname) plt.colorbar() plt.figure() plt.subplot(211) plt.imshow(arr[:,:,Y_INDEX]) plt.colorbar() plt.title('%s Y' % dsname) plt.subplot(212) plt.imshow(v0) plt.colorbar() plt.title('%s V' % dsname) #save the exr file, -1 means no data, not NaN u0[np.isnan(u0)] = -1 v0[np.isnan(v0)] = -1 exrpath = decode_url('package://flycave/calibration/exr/%s.exr' % dsname) exr.save_exr(exrpath, r=u0, g=v0, b=np.zeros_like(u0)) #save the resulting geometry to the parameter server geom = self.geom.to_geom_dict() dsc.set_geometry(geom) dsc.set_binary_exr(exrpath)
"--inlier-dir", type=str, default=None, help="path to directory containing Xe.dat, Re.dat, Ce.dat" ) parser.add_argument( "--parameter-server-properties", default=False, action="store_true", help="get display server properties (height, width, etc) from the parameter server. " "this means the display server does not need to be running.", ) parser.add_argument( "--display-server-numbers", type=str, default="0,1,3", help="comma separated list of display server numbers" ) parser.add_argument("--visualize", default=False, action="store_true") args = parser.parse_args() c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.visualize, args.inlier_dir) c.generate_exrs( [int(i) for i in args.display_server_numbers.split(",")], prefer_parameter_server_properties=args.parameter_server_properties, filt_method="linear", mask_out=False, mask_fill_value=np.nan, ) if args.visualize: plt.show() # print 'now spinning forever...' # rospy.spin()
) parser.add_argument("--undistort-radial", default=False, action="store_true") parser.add_argument( "--parameter-server-properties", default=False, action="store_true", help="get display server properties (height, width, etc) from the parameter server. " "this means the display server does not need to be running.", ) parser.add_argument( "--display-server-numbers", type=str, default="0,1,3", help="comma separated list of display server numbers" ) parser.add_argument("--visualize", default=True, action="store_true") args = parser.parse_args() c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial, args.visualize) cx, cy, cz, ax, ay, az, radius = [ 0.331529438495636, 0.5152832865715027, 0.2756080627441406, -0.7905913591384888, 0.24592067301273346, -7.272743225097656, 2.241607666015625, ] c.set_cylinder_model(cx, cy, cz, ax, ay, az, radius) c.new_flydra_calib() c.generate_exrs(
'--inlier-dir', type=str, default=None, help='path to directory containing Xe.dat, Re.dat, Ce.dat') parser.add_argument( '--parameter-server-properties', default=False, action='store_true', help='get display server properties (height, width, etc) from the parameter server. '\ 'this means the display server does not need to be running.') parser.add_argument('--display-server-numbers', type=str, default='0,1,3', help='comma separated list of display server numbers') parser.add_argument('--visualize', default=False, action='store_true') args = parser.parse_args() c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.visualize, args.inlier_dir) c.generate_exrs( [int(i) for i in args.display_server_numbers.split(',')], prefer_parameter_server_properties=args.parameter_server_properties, filt_method='linear', mask_out=False, mask_fill_value=np.nan) if args.visualize: plt.show() #print 'now spinning forever...' #rospy.spin()
import yaml import argparse import os.path import roslib; roslib.load_manifest('freemovr_engine') roslib.load_manifest('motmot_ros_utils') import rospy from calib.io import MultiCalSelfCam, AllPointPickle from rosutils.io import decode_url #FIXME: should use the single pickle file from the collect points part... DS_PKL = decode_url('package://flycave/calibration/triplets/') def calibrate(rerun_mcsc,calib_config,display_server_numbers): config = yaml.load(open(decode_url(calib_config))) for n in [int(i) for i in display_server_numbers]: src = DS_PKL + "/ds%d" % n ds = '/display_server%d' % n ids = [ds] + config['display_servers'][ds] a = AllPointPickle() a.initilize_from_directory(src) print "*"*20 print src print "*"*20 if rerun_mcsc:
default='package://flycave/calibration/flydra', help='path to flydra multicamselfcal result dir') parser.add_argument('--undistort-radial', default=False, action='store_true') parser.add_argument('--parameter-server-properties', default=False, action='store_true', help='get display server properties (height, width, etc) from the parameter server. '\ 'this means the display server does not need to be running.') parser.add_argument('--display-server-numbers', type=str, default='0,1,3', help='comma separated list of display server numbers') parser.add_argument('--visualize', default=True, action='store_true') args = parser.parse_args() c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial, args.visualize) cx, cy, cz, ax, ay, az, radius = [ 0.331529438495636, 0.5152832865715027, 0.2756080627441406, -0.7905913591384888, 0.24592067301273346, -7.272743225097656, 2.241607666015625 ] c.set_cylinder_model(cx, cy, cz, ax, ay, az, radius) c.new_flydra_calib() c.generate_exrs( [int(i) for i in args.display_server_numbers.split(',')], prefer_parameter_server_properties=args.parameter_server_properties,