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
