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 __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 run(self): # make some initial guesses. mean_v = np.mean(self.verts, axis=0) std_v = np.std(self.verts, axis=0) # FIXME: this is hard-coded to a unit cylinder. target_center = np.array([0, 0, 0.5]) translate = target_center - mean_v scale = np.mean(0.5 / std_v) # this is hacky... quat = tf.transformations.quaternion_from_euler(0, 0, 0) p0 = np.array([scale] + translate.tolist() + quat.tolist()) print 'p0' print p0 self.orig_scale = scale if 1: recon0 = self.get_recon_from_param_vec(p0) M0 = recon0.get_transformation_matrix() recon0.save_as_json('recon0.json') if 1: # debug xform before opt xe, ce, re = self.orig_XCR_e MultiCalSelfCam.transform_and_save(xe, ce, re, '/tmp', recon0) start_err = self.calc_error(p0) print 'start_err', start_err p01 = np.asfarray(p0) p02 = p01.flatten() all_out = fmin(self.calc_error, p0, full_output=True) popt = all_out[0] final_err = self.calc_error(popt) print 'final_err', final_err print popt print repr(popt) recon_opt = self.get_recon_from_param_vec(popt) if 1: recon_opt.save_as_json('recon_opt.json') aligned_verts = recon_opt.move_cloud(self.verts) transformed_points = pcl.PointCloud() transformed_points.from_array(aligned_verts.astype(np.float32)) transformed_points.to_file(self.out_fname) print 'saved transformed points to', self.out_fname M_opt = recon_opt.get_transformation_matrix() if 1: alignedR = self.srcR.get_aligned_copy(M_opt) alignedR.save_to_files_in_new_directory(self.out_flydra_R) xe, ce, re = self.orig_XCR_e MultiCalSelfCam.transform_and_save(xe, ce, re, self.out_flydra_R, recon_opt)
def run(self): # make some initial guesses. mean_v = np.mean(self.verts,axis=0) std_v = np.std(self.verts,axis=0) # FIXME: this is hard-coded to a unit cylinder. target_center = np.array([0,0,0.5]) translate = target_center - mean_v scale = np.mean(0.5/std_v) # this is hacky... quat = tf.transformations.quaternion_from_euler(0,0,0) p0 = np.array([scale] + translate.tolist() + quat.tolist()) print 'p0' print p0 self.orig_scale = scale if 1: recon0 = self.get_recon_from_param_vec(p0) M0 = recon0.get_transformation_matrix() recon0.save_as_json('recon0.json') if 1: # debug xform before opt xe, ce, re = self.orig_XCR_e MultiCalSelfCam.transform_and_save( xe, ce, re, '/tmp', recon0 ) start_err = self.calc_error( p0 ) print 'start_err',start_err p01 = np.asfarray(p0) p02 = p01.flatten() all_out = fmin( self.calc_error, p0, full_output=True) popt = all_out[0] final_err = self.calc_error( popt ) print 'final_err',final_err print popt print repr(popt) recon_opt = self.get_recon_from_param_vec(popt) if 1: recon_opt.save_as_json('recon_opt.json') aligned_verts = recon_opt.move_cloud(self.verts) transformed_points = pcl.PointCloud() transformed_points.from_array(aligned_verts.astype(np.float32)) transformed_points.to_file( self.out_fname ) print 'saved transformed points to', self.out_fname M_opt = recon_opt.get_transformation_matrix() if 1: alignedR = self.srcR.get_aligned_copy(M_opt) alignedR.save_to_files_in_new_directory(self.out_flydra_R) xe, ce, re = self.orig_XCR_e MultiCalSelfCam.transform_and_save( xe, ce, re, self.out_flydra_R, recon_opt )
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, 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, filename, orig_flydra_R, out_fname=None): self.srcR = flydra.reconstruct.Reconstructor(orig_flydra_R) self.orig_XCR_e = MultiCalSelfCam.read_calibration_result( orig_flydra_R) self.out_flydra_R = orig_flydra_R + '.aligned' print self.out_flydra_R assert not os.path.exists(self.out_flydra_R) pcd = pcl.PointCloud() pcd.from_file(filename) self.verts = pcd.to_array() height = 1.0 self.cyl = Cylinder(base=dict(x=0, y=0, z=0), axis=dict(x=0, y=0, z=height), radius=0.5) self._centers = np.zeros_like(self.verts) self._centers[:, 2] = height / 2.0 if out_fname is None: base = os.path.splitext(filename)[0] self.out_fname = base + '-cyl.pcd' else: self.out_fname = out_fname assert self.out_fname.endswith('.pcd')
def __init__(self, filename, orig_flydra_R, out_fname=None): self.srcR = flydra.reconstruct.Reconstructor( orig_flydra_R ) self.orig_XCR_e = MultiCalSelfCam.read_calibration_result( orig_flydra_R ) self.out_flydra_R = orig_flydra_R + '.aligned' print self.out_flydra_R assert not os.path.exists(self.out_flydra_R) pcd = pcl.PointCloud() pcd.from_file( filename ) self.verts = pcd.to_array() height = 1.0 self.cyl = Cylinder( base=dict(x=0,y=0,z=0), axis=dict(x=0,y=0,z=height), radius=0.5) self._centers = np.zeros_like(self.verts) self._centers[:,2] = height/2.0 if out_fname is None: base = os.path.splitext(filename)[0] self.out_fname = base + '-cyl.pcd' else: self.out_fname = out_fname assert self.out_fname.endswith('.pcd')
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 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)
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: r = flydra.reconstruct.Reconstructor(cal_source=DS1_CALIB)
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: r = flydra.reconstruct.Reconstructor(cal_source=DS1_CALIB) cams = r.get_cam_ids()
def run_mcsc(path): mcsc = MultiCalSelfCam(path) dest = mcsc.execute(blocking=True, dest=tempfile.mkdtemp(),silent=False) pcd = '/tmp/mcscpoints.pcd' MultiCalSelfCam.save_to_pcd(dirname=dest, fname=pcd) print 'saved to pcd file', pcd
def run_mcsc(path): mcsc = MultiCalSelfCam(path) dest = mcsc.execute(blocking=True, dest=tempfile.mkdtemp(), silent=False) pcd = '/tmp/mcscpoints.pcd' MultiCalSelfCam.save_to_pcd(dirname=dest, fname=pcd) print 'saved to pcd file', pcd