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 __init__(self, flydra_calib, laser_pkl, undistort_flydra_points): self.flydra_calib = flydra_calib self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser_pkl = laser_pkl self.undistort_flydra_points = undistort_flydra_points self.laser = AllPointPickle() self.laser.initilize_from_directory(self.laser_pkl) 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=self.undistort_flydra_points) for pt in pts])
def __init__(self, flydra_calib, laser_pkl, visualize, inlier_dir): if inlier_dir is None: if not os.path.isdir(flydra_calib): raise RuntimeError('cannot guess inlier directory') self.inlier_dir = flydra_calib else: self.inlier_dir = inlier_dir self.flydra_calib = flydra_calib self.laser_pkl = laser_pkl self.visualize = visualize #publish the camera positions and the inlier set from the flydra calibraion MultiCalSelfCam.publish_calibration_points(self.flydra_calib, topic_base='/flydra', inlier_dir=self.inlier_dir) print '*' * 80 print 'loaded original calibration from', self.flydra_calib print '*' * 80 print '*' * 80 print 'loaded inliers from', self.inlier_dir print '*' * 80 self.fly = flydra.reconstruct.Reconstructor( cal_source=self.flydra_calib) self.laser = AllPointPickle() #load default laser points self.load_laser_points() if 1: # FIXME: we have hard-coded our geometry here. self.geom = simple_geom.Cylinder(base=dict(x=0, y=0, z=0), axis=dict(x=0, y=0, z=1), radius=0.5) print 'Using geometry -----------------------' print self.geom.to_geom_dict() print '--------------------------------------' if 0: # This is from John's original file 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)
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()
class Exporter(object): def __init__(self, flydra_calib, laser_pkl, undistort_flydra_points): self.flydra_calib = flydra_calib self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser_pkl = laser_pkl self.undistort_flydra_points = undistort_flydra_points self.laser = AllPointPickle() self.laser.initilize_from_directory(self.laser_pkl) 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=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'
def __init__(self, flydra_calib, laser_pkl, visualize, inlier_dir): if inlier_dir is None: if not os.path.isdir(flydra_calib): raise RuntimeError("cannot guess inlier directory") self.inlier_dir = flydra_calib else: self.inlier_dir = inlier_dir self.flydra_calib = flydra_calib self.laser_pkl = laser_pkl self.visualize = visualize # publish the camera positions and the inlier set from the flydra calibraion MultiCalSelfCam.publish_calibration_points(self.flydra_calib, topic_base="/flydra", inlier_dir=self.inlier_dir) print "*" * 80 print "loaded original calibration from", self.flydra_calib print "*" * 80 print "*" * 80 print "loaded inliers from", self.inlier_dir print "*" * 80 self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser = AllPointPickle() # load default laser points self.load_laser_points() if 1: # FIXME: we have hard-coded our geometry here. self.geom = simple_geom.Cylinder(base=dict(x=0, y=0, z=0), axis=dict(x=0, y=0, z=1), radius=0.5) print "Using geometry -----------------------" print self.geom.to_geom_dict() print "--------------------------------------" if 0: # This is from John's original file 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)
class Calibrator(object): 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 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=self.undistort_flydra_points) 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) def detect_clyinder(self, smooth=0): raise Exception( "NOT FINISHED YET. " "It is better to do this manually using the tune cyl application, then enter the results into __main__" ) # fit a cylinder to this cloud p = pcl.PointCloud() p.from_list(self.xyz) if smooth > 0: cloud = p.filter_mls(smooth) else: cloud = p create_point_cloud_message_publisher( p.to_list(), topic_name="/flydracalib/pointssmooth", publish_now=True, latch=True ) seg = cloud.make_segmenter_normals(searchRadius=0.2) seg.set_optimize_coefficients(True) seg.set_model_type(pcl.SACMODEL_CYLINDER) seg.set_method_type(pcl.SAC_RANSAC) seg.set_normal_distance_weight(0.01) seg.set_max_iterations(10000) seg.set_distance_threshold(0.5) # seg.set_radius_limits (1, 3) # seg.set_axis(0.41,1.894,2.733) # seg.set_eps_angle(0.1) indices, model = seg.segment() print "model", model print "inliers", len(indices) cx, cy, cz, ax, ay, az, radius = model return cx, cy, cz, ax, ay, az, radius def set_cylinder_model(self, cx, cy, cz, ax, ay, az, radius): self.recon = CylinderPointCloudTransformer(cx, cy, cz, ax, ay, az, radius, self.xyz) create_cylinder_publisher( cx, cy, cz, ax, ay, az, radius, topic_name="/flydracalib/cyl", publish_now=True, latch=True, length=3 * radius, color=(0, 1, 0, 0.3), ) create_point_publisher( cx, cy, cz, r=0.1, topic_name="/flydracalib/cylcenter", publish_now=True, latch=True, color=(1, 0, 0, 0.5) ) create_point_cloud_message_publisher( self.recon.move_cloud(self.xyz), topic_name="/flydracalib/points2", publish_now=True, latch=True ) def new_flydra_calib(self, path="/tmp/flydra.xml"): M = self.recon.get_transformation_matrix() newfly = self.fly.get_aligned_copy(M) newfly.save_to_xml_filename(path) pts2d = self.laser.get_points_in_cameras(newfly.get_cam_ids(), random_num_results=1) pts3d = newfly.find3d(pts2d[0], return_line_coords=False) cx, cy, cz = pts3d create_point_publisher( cx, cy, cz, r=0.1, topic_name="/flydracalib/testpoint", publish_now=True, latch=True, color=(0, 0, 1, 0.5) ) return path 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)
class Calibrator(object): def __init__(self, flydra_calib, laser_pkl, visualize, inlier_dir): if inlier_dir is None: if not os.path.isdir(flydra_calib): raise RuntimeError('cannot guess inlier directory') self.inlier_dir = flydra_calib else: self.inlier_dir = inlier_dir self.flydra_calib = flydra_calib self.laser_pkl = laser_pkl self.visualize = visualize #publish the camera positions and the inlier set from the flydra calibraion MultiCalSelfCam.publish_calibration_points(self.flydra_calib, topic_base='/flydra', inlier_dir=self.inlier_dir) print '*'*80 print 'loaded original calibration from', self.flydra_calib print '*'*80 print '*'*80 print 'loaded inliers from', self.inlier_dir print '*'*80 self.fly = flydra.reconstruct.Reconstructor(cal_source=self.flydra_calib) self.laser = AllPointPickle() #load default laser points self.load_laser_points() if 1: # FIXME: we have hard-coded our geometry here. self.geom = simple_geom.Cylinder( base=dict(x=0,y=0,z=0), axis=dict(x=0,y=0,z=1), radius=0.5) print 'Using geometry -----------------------' print self.geom.to_geom_dict() print '--------------------------------------' if 0: # This is from John's original file 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) 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 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)
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) dest = mcsc.execute(blocking=True, dest=tempfile.mkdtemp(prefix='mcsc'), silent=False) MultiCalSelfCam.publish_calibration_points(dest) MultiCalSelfCam.save_to_pcd(dest, "/tmp/ds1.pcd") #test roundtrip 2d -> 3d -> 2d
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) dest = mcsc.execute(blocking=True, dest=tempfile.mkdtemp(prefix='mcsc'), silent=False) MultiCalSelfCam.publish_calibration_points(dest) MultiCalSelfCam.save_to_pcd(dest, "/tmp/ds1.pcd") #test roundtrip 2d -> 3d -> 2d if 0:
class Calibrator(object): 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 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=self.undistort_flydra_points) 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) def detect_clyinder(self, smooth=0): raise Exception("NOT FINISHED YET. "\ "It is better to do this manually using the tune cyl application, then enter the results into __main__") #fit a cylinder to this cloud p = pcl.PointCloud() p.from_list(self.xyz) if smooth > 0: cloud = p.filter_mls(smooth) else: cloud = p create_point_cloud_message_publisher( p.to_list(), topic_name='/flydracalib/pointssmooth', publish_now=True, latch=True) seg = cloud.make_segmenter_normals(searchRadius=0.2) seg.set_optimize_coefficients (True); seg.set_model_type (pcl.SACMODEL_CYLINDER) seg.set_method_type (pcl.SAC_RANSAC) seg.set_normal_distance_weight (0.01) seg.set_max_iterations (10000) seg.set_distance_threshold (0.5) #seg.set_radius_limits (1, 3) #seg.set_axis(0.41,1.894,2.733) #seg.set_eps_angle(0.1) indices, model = seg.segment() print "model", model print "inliers", len(indices) cx,cy,cz,ax,ay,az,radius = model return cx,cy,cz,ax,ay,az,radius def set_cylinder_model(self, cx,cy,cz,ax,ay,az,radius): self.recon = CylinderPointCloudTransformer(cx,cy,cz,ax,ay,az,radius,self.xyz) create_cylinder_publisher( cx,cy,cz,ax,ay,az,radius, topic_name='/flydracalib/cyl', publish_now=True, latch=True, length=3*radius, color=(0,1,0,0.3)) create_point_publisher( cx,cy,cz, r=0.1, topic_name='/flydracalib/cylcenter', publish_now=True, latch=True, color=(1,0,0,0.5)) create_point_cloud_message_publisher( self.recon.move_cloud(self.xyz), topic_name='/flydracalib/points2', publish_now=True, latch=True) def new_flydra_calib(self, path='/tmp/flydra.xml'): M = self.recon.get_transformation_matrix() newfly = self.fly.get_aligned_copy(M) newfly.save_to_xml_filename(path) pts2d = self.laser.get_points_in_cameras(newfly.get_cam_ids(), random_num_results=1) pts3d = newfly.find3d(pts2d[0],return_line_coords=False) cx,cy,cz = pts3d create_point_publisher( cx,cy,cz, r=0.1, topic_name='/flydracalib/testpoint', publish_now=True, latch=True, color=(0,0,1,0.5)) return path 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)