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 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 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 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 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 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)