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=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)
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)
print "cameras", cams pts = a.get_points_in_cameras(r.get_cam_ids()) pt = pts[0] print "2d points", pt X = r.find3d(pt, return_line_coords=False) print "3d point", X pt = r.find2d(cams[0], X) print "2d point in %s" % cams[0], pt print "2d points", [r.find2d(c, X) for c in cams] #for all laser points, find their 3D coords, check it makes a cylinder if 0: #use the flydra calibration to get the 3d coords r = flydra.reconstruct.Reconstructor(cal_source=FLYDRA_CALIB) laser = AllPointPickle() laser.initilize_from_directory(LASER_PKL) print "number of laser points", laser.num_points #all points visible in 2 or more cameras pts = laser.get_points_in_cameras(r.get_cam_ids()) create_pcd_file_from_points( '/tmp/cyl.pcd', [r.find3d(pt, return_line_coords=False) for pt in pts])
pts = a.get_points_in_cameras(r.get_cam_ids()) pt = pts[0] print "2d points",pt X = r.find3d(pt,return_line_coords=False) print "3d point",X pt = r.find2d(cams[0],X) print "2d point in %s" % cams[0],pt print "2d points",[r.find2d(c,X) for c in cams] #for all laser points, find their 3D coords, check it makes a cylinder if 0: #use the flydra calibration to get the 3d coords r = flydra.reconstruct.Reconstructor(cal_source=FLYDRA_CALIB) laser = AllPointPickle() laser.initilize_from_directory(LASER_PKL) print "number of laser points",laser.num_points #all points visible in 2 or more cameras pts = laser.get_points_in_cameras(r.get_cam_ids()) create_pcd_file_from_points( '/tmp/cyl.pcd', [r.find3d(pt,return_line_coords=False) for pt in pts])
def export(self, fname): print "saving to", fname create_pcd_file_from_points(fname, self.xyz) print "done saving"
def export(self,fname): print 'saving to',fname create_pcd_file_from_points(fname, self.xyz) print 'done saving'