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