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
