def test_formats():
urlyaml = io.decode_url('package://motmot_ros_utils/tests/Basler_21029383.yaml')
urlrad = io.decode_url('package://motmot_ros_utils/tests/Basler_21029383.rad')
rad = '/tmp/test.rad'
formats.camera_calibration_yaml_to_radfile(urlyaml, rad)
assert open(urlrad,'r').read() == open(rad,'r').read()
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 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 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 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 test_decode_file():
PATH = '/testmotmotrosutils'
DATA = '123'
b = xmlrpclib.Binary(data=DATA)
rospy.set_param(PATH,b)
a = io.decode_file_from_parameter_server(PATH)
b = io.decode_url('parameter://'+PATH)
assert a == b
d = open(a).read()
assert d == DATA
import roslib
roslib.load_manifest('freemoovr')
roslib.load_manifest('motmot_ros_utils')
import rospy
from calib.io import MultiCalSelfCam, AllPointPickle
from calib.visualization import create_pcd_file_from_points
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)
import roslib;
roslib.load_manifest('flyvr')
roslib.load_manifest('motmot_ros_utils')
import rospy
from calib.io import MultiCalSelfCam, AllPointPickle
from calib.visualization import create_pcd_file_from_points
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={},
rospy.sleep(0.1)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument(
'--calib-config', type=str, default='package://flycave/conf/calib-all.yaml',
help='path to calibration configuration yaml file')
argv = rospy.myargv()
args = parser.parse_args(argv[1:])
rospy.init_node('generate_masks')
config = {}
conffile = decode_url(args.calib_config)
with open(conffile, 'r') as f:
config = yaml.load(f)
for k in ("display_servers", "mask_dir", "tracking_cameras"):
if not config.has_key(k):
parser.error("malformed calibration config, missing %s" % k)
mask_dir = decode_url(config["mask_dir"])
if not os.path.isdir(mask_dir):
parser.error("mask dir not found")
display_servers = config["display_servers"]
all_cams = []
for cam in config["tracking_cameras"]:
all_cams.append(cam)
def do_exr(self, interp_method, do_luminance, gamma, blend_curve):
exrs = {ds:{} for ds in self.display_servers}
do_xyz = ["x","y","z"]
exrs_uvl = ["u","v","l","ui","vi","li"]
comment = 'created from %s' % (", ".join(self.filenames),)
if self.smoothed is not None:
comment += '. MLS smoothed by %f' % self.smoothed
if self.flydra_calib:
comment += '. 3D reconstruction from %s' % self.flydra_calib
comment += '. Interpolation method %s' % interp_method
comment += '. Luminance blend %s (gamma: %.2f curve: %.2f)' % (do_luminance, gamma, blend_curve)
rospy.loginfo(comment)
if do_luminance:
blender = blend.Blender(
True or self.visualize,
os.getcwd(),
debug_exr=self.debug,
exr_comments=comment
)
def alloc_exr_mask(ds, name):
dsc = self.dscs[ds]
exrs[ds][name] = {
"exr":np.zeros((768,1024)),
}
exrs[ds][name]["exr"].fill(np.nan)
def update_mask(ds, name, val, vdispmask=None):
valid_val = ~np.isnan(val)
if vdispmask is not None:
valid = np.logical_or(vdispmask,valid_val)
else:
valid = valid_val
exrs[ds][name]["exr"][valid] = val[valid]
all_3d = []
for ds in self.display_servers:
dsc = self.dscs[ds]
if self.visualize:
cv2.namedWindow(ds)
img = self.cvimgs[ds]
for ax in do_xyz + exrs_uvl:
alloc_exr_mask(ds, ax)
ds_3d = []
for vdisp in self.data[ds]:
vdispmask = dsc.get_virtual_display_mask(vdisp, squeeze=True)
vdisp_3d = []
vdisp_2d = []
vdisp_lum = []
if self.visualize:
arr = np.zeros((768,1024,4))
arr.fill(np.nan)
for xyz,pixel,lum in self.data[ds][vdisp]: #just do one vdisp
vdisp_2d.append(pixel)
vdisp_3d.append(xyz)
vdisp_lum.append(lum)
if self.visualize:
col = pixel[0]
row = pixel[1]
add_crosshairs_to_nparr(img, row=row, col=col, chan=2, sz=1)
arr[row-2:row+2,col-2:col+2,X_INDEX] = xyz[0]
arr[row-2:row+2,col-2:col+2,Y_INDEX] = xyz[1]
arr[row-2:row+2,col-2:col+2,Z_INDEX] = xyz[2]
arr[row-2:row+2,col-2:col+2,L_INDEX] = lum
ds_3d.extend(vdisp_3d)
vdisp_lum_arr = np.array(vdisp_lum, dtype=np.float)
vdisp_2d_arr = np.array(vdisp_2d, dtype=np.float)
vdisp_3d_arr = np.array(vdisp_3d, dtype=np.float)
#construct the interpoolated geometry (uv) <-> 3d (xyz) mapping.
ui,vi = self.interpolate_points(
vdisp_3d_arr,
vdisp_2d_arr,
dsc,
interp_method)
#interpolate luminance
li = interpolate_pixel_cords(
points_2d=vdisp_2d_arr,
values_1d=vdisp_lum_arr,
img_width=dsc.width,
img_height=dsc.height,
method=interp_method)
update_mask(ds, "ui", ui, vdispmask)
update_mask(ds, "vi", vi, vdispmask)
update_mask(ds, "li", li, vdispmask)
#and keep an unterpolated copy
u,v = self.interpolate_points(
vdisp_3d_arr,
vdisp_2d_arr,
dsc,
"none")
update_mask(ds, "u", u, vdispmask)
update_mask(ds, "v", v, vdispmask)
if self.debug:
for axnum,ax in enumerate(do_xyz):
update_mask(ds, ax,
interpolate_pixel_cords(
points_2d=vdisp_2d_arr,
values_1d=vdisp_3d_arr[:,axnum],
img_width=dsc.width,
img_height=dsc.height,
method="none")
)
if self.visualize:
self.show_vdisp_points(arr, ds, ui, vi, vdisp)
if do_luminance:
blender.add_display_server(
ds,
dsc,
exrs[ds]["u"]["exr"].astype(np.float32),exrs[ds]["v"]["exr"].astype(np.float32),
exrs[ds]["ui"]["exr"].astype(np.float32),exrs[ds]["vi"]["exr"].astype(np.float32),
)
if self.visualize:
cv2.imshow(ds, img)
if self.debug:
create_pcd_file_from_points(decode_url('%s.pcd' % ds),ds_3d)
all_3d.extend(ds_3d)
if self.debug:
create_pcd_file_from_points(decode_url('all.pcd'),all_3d)
create_point_cloud_message_publisher(all_3d,'/calibration/points',publish_now=True, latch=True)
if do_luminance:
blended = blender.blend(gamma, blend_curve)
for ds in self.display_servers:
dsc = self.dscs[ds]
if do_luminance:
final_li = blended[ds]
else:
#set the luminance channel to 1 (no blending) inside the viewport,
#0 (off) outside
final_li = np.ones_like(exrs[ds]["ui"]["exr"])
final_li[np.isnan(exrs[ds]["ui"]["exr"])] = 0
#in our shader convention -1 means no data, not NaN
exrs[ds]["ui"]["exr"][np.isnan(exrs[ds]["ui"]["exr"])] = -1
exrs[ds]["vi"]["exr"][np.isnan(exrs[ds]["vi"]["exr"])] = -1
final_ui = exrs[ds]["ui"]["exr"]
final_vi = exrs[ds]["vi"]["exr"]
if self.visualize:
plt.figure()
plt.imshow(final_ui)
plt.colorbar()
plt.title('%s U' % ds)
plt.figure()
plt.imshow(final_vi)
plt.colorbar()
plt.title('%s V' % ds)
plt.figure()
plt.imshow(final_li)
plt.colorbar()
plt.title('%s L' % ds)
exrpath = "%s.exr" % ds
exr.save_exr(
exrpath,
r=final_ui,
g=final_vi,
b=final_li,
comments=comment)
if self.debug:
exrs[ds]["u"]["exr"][np.isnan(exrs[ds]["u"]["exr"])] = -1
exrs[ds]["v"]["exr"][np.isnan(exrs[ds]["v"]["exr"])] = -1
final_u = exrs[ds]["u"]["exr"]
final_v = exrs[ds]["v"]["exr"]
exr.save_exr(
"%s.nointerp.exr" % ds,
r=final_u,
g=final_v,
b=np.zeros_like(final_u),
comments=comment)
#save the resulting geometry to the parameter server
if self.update_parameter_server:
geom = self.geom.to_geom_dict()
dsc.set_geometry(geom)
dsc.set_binary_exr(exrpath)
rospy.loginfo("updated parameter server")
sh.rosnode("kill","/%s"%ds)
rospy.loginfo("restarted server")
cal = Calibrator(
visualize=args.visualize,
debug=args.debug,
new_reconstructor=args.reconstructor,
mask_out=False,
update_parameter_server=args.update)
tmp=[]
[tmp.extend(_) for _ in args.calibration]
cal_files = []
[cal_files.extend(glob.glob(_)) for _ in tmp]
rospy.loginfo('cal_files: %r'%cal_files)
if len(cal_files):
#multiple bag files
for c in cal_files:
fn = decode_url(c)
assert os.path.exists(fn)
cal.load(fn, args.no_wait_display_server)
else:
rospy.logfatal('No data. Specify inputs with --calibration <file.bag>')
rospy.signal_shutdown('no data')
sys.exit(1)
if args.smooth:
cal.smooth(args.smooth)
cal.do_exr(args.interpolation, args.luminance, args.gamma, args.blend_curve)
if cal.visualize:
plt.show()
rospy.sleep(0.1)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument(
'--calib-config', type=str, default='package://flycave/conf/calib-all.yaml',
help='path to calibration configuration yaml file')
argv = rospy.myargv()
args = parser.parse_args(argv[1:])
rospy.init_node('generate_masks')
config = {}
conffile = decode_url(args.calib_config)
with open(conffile, 'r') as f:
config = yaml.load(f)
for k in ("display_servers", "mask_dir", "tracking_cameras"):
if not config.has_key(k):
parser.error("malformed calibration config, missing %s" % k)
mask_dir = decode_url(config["mask_dir"])
if not os.path.isdir(mask_dir):
parser.error("mask dir not found")
display_servers = config["display_servers"]
all_cams = []
for cam in config["tracking_cameras"]:
all_cams.append(cam)
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)
import yaml
import argparse
import os.path
import roslib
roslib.load_manifest('flyvr')
roslib.load_manifest('motmot_ros_utils')
import rospy
from calib.io import MultiCalSelfCam, AllPointPickle
from rosutils.io import decode_url
#FIXME: should use the single pickle file from the collect points part...
DS_PKL = decode_url('package://flycave/calibration/triplets/')
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
self.xyz = np.array([self.fly.find3d(pt,return_line_coords=False, undistort=self.undistort_flydra_points) for pt in pts])
def export(self,fname):
print 'saving to',fname
create_pcd_file_from_points(fname,
self.xyz)
print 'done saving'
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
'--laser-pkldir', type=str, default='package://flycave/calibration/laser',
help='path to dir containing result.pkl, resolution.pkl, etc')
parser.add_argument(
'--flydra-calib', type=str, default='package://flycave/calibration/flydra',
help='path to flydra multicamselfcal result dir')
parser.add_argument(
'--output', type=str, default=None)
parser.add_argument('--undistort-radial', default=False, action='store_true')
args = parser.parse_args()
c = Exporter(
decode_url(args.flydra_calib),
decode_url(args.laser_pkldir),
args.undistort_radial)
if args.output is None:
args.output = decode_url('package://flycave/calibration/pcd/flydracyl.pcd')
c.export(args.output)
def test_io():
print io.decode_url('~/Documents')
print io.decode_url('/tmp')
print io.decode_url('file:///home/strawlab')
print io.decode_url('package://vros_display/foo/bar')
print io.decode_url('package://vros_display/calib/${NODE_NAME}.xml')
print io.decode_url('${ROS_HOME}/foo')
def export(self, fname):
print "saving to", fname
create_pcd_file_from_points(fname, self.xyz)
print "done saving"
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
"--laser-pkldir",
type=str,
default="package://flycave/calibration/laser",
help="path to dir containing result.pkl, resolution.pkl, etc",
)
parser.add_argument(
"--flydra-calib",
type=str,
default="package://flycave/calibration/flydra",
help="path to flydra multicamselfcal result dir",
)
parser.add_argument("--output", type=str, default=None)
parser.add_argument("--undistort-radial", default=False, action="store_true")
args = parser.parse_args()
c = Exporter(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial)
if args.output is None:
args.output = decode_url("package://flycave/calibration/pcd/flydracyl.pcd")
c.export(args.output)
help='path to flydra multicamselfcal result dir')
parser.add_argument(
'--inlier-dir', type=str, default=None,
help='path to directory containing Xe.dat, Re.dat, Ce.dat')
parser.add_argument(
'--parameter-server-properties', default=False, action='store_true',
help='get display server properties (height, width, etc) from the parameter server. '\
'this means the display server does not need to be running.')
parser.add_argument(
'--display-server-numbers', type=str, default='0,1,3',
help='comma separated list of display server numbers')
parser.add_argument('--visualize', default=False, action='store_true')
args = parser.parse_args()
c = Calibrator(
decode_url(args.flydra_calib),
decode_url(args.laser_pkldir),
args.visualize,
args.inlier_dir)
c.generate_exrs(
[int(i) for i in args.display_server_numbers.split(',')],
prefer_parameter_server_properties=args.parameter_server_properties,
filt_method='linear',
mask_out=False,
mask_fill_value=np.nan)
if args.visualize:
plt.show()
#print 'now spinning forever...'
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)
"--inlier-dir", type=str, default=None, help="path to directory containing Xe.dat, Re.dat, Ce.dat"
)
parser.add_argument(
"--parameter-server-properties",
default=False,
action="store_true",
help="get display server properties (height, width, etc) from the parameter server. "
"this means the display server does not need to be running.",
)
parser.add_argument(
"--display-server-numbers", type=str, default="0,1,3", help="comma separated list of display server numbers"
)
parser.add_argument("--visualize", default=False, action="store_true")
args = parser.parse_args()
c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.visualize, args.inlier_dir)
c.generate_exrs(
[int(i) for i in args.display_server_numbers.split(",")],
prefer_parameter_server_properties=args.parameter_server_properties,
filt_method="linear",
mask_out=False,
mask_fill_value=np.nan,
)
if args.visualize:
plt.show()
# print 'now spinning forever...'
# rospy.spin()
)
parser.add_argument("--undistort-radial", default=False, action="store_true")
parser.add_argument(
"--parameter-server-properties",
default=False,
action="store_true",
help="get display server properties (height, width, etc) from the parameter server. "
"this means the display server does not need to be running.",
)
parser.add_argument(
"--display-server-numbers", type=str, default="0,1,3", help="comma separated list of display server numbers"
)
parser.add_argument("--visualize", default=True, action="store_true")
args = parser.parse_args()
c = Calibrator(decode_url(args.flydra_calib), decode_url(args.laser_pkldir), args.undistort_radial, args.visualize)
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)
c.new_flydra_calib()
c.generate_exrs(
'--inlier-dir',
type=str,
default=None,
help='path to directory containing Xe.dat, Re.dat, Ce.dat')
parser.add_argument(
'--parameter-server-properties', default=False, action='store_true',
help='get display server properties (height, width, etc) from the parameter server. '\
'this means the display server does not need to be running.')
parser.add_argument('--display-server-numbers',
type=str,
default='0,1,3',
help='comma separated list of display server numbers')
parser.add_argument('--visualize', default=False, action='store_true')
args = parser.parse_args()
c = Calibrator(decode_url(args.flydra_calib),
decode_url(args.laser_pkldir), args.visualize,
args.inlier_dir)
c.generate_exrs(
[int(i) for i in args.display_server_numbers.split(',')],
prefer_parameter_server_properties=args.parameter_server_properties,
filt_method='linear',
mask_out=False,
mask_fill_value=np.nan)
if args.visualize:
plt.show()
#print 'now spinning forever...'
#rospy.spin()
import yaml
import argparse
import os.path
import roslib;
roslib.load_manifest('freemovr_engine')
roslib.load_manifest('motmot_ros_utils')
import rospy
from calib.io import MultiCalSelfCam, AllPointPickle
from rosutils.io import decode_url
#FIXME: should use the single pickle file from the collect points part...
DS_PKL = decode_url('package://flycave/calibration/triplets/')
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:
default='package://flycave/calibration/flydra',
help='path to flydra multicamselfcal result dir')
parser.add_argument('--undistort-radial',
default=False,
action='store_true')
parser.add_argument('--parameter-server-properties', default=False, action='store_true',
help='get display server properties (height, width, etc) from the parameter server. '\
'this means the display server does not need to be running.')
parser.add_argument('--display-server-numbers',
type=str,
default='0,1,3',
help='comma separated list of display server numbers')
parser.add_argument('--visualize', default=True, action='store_true')
args = parser.parse_args()
c = Calibrator(decode_url(args.flydra_calib),
decode_url(args.laser_pkldir), args.undistort_radial,
args.visualize)
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)
c.new_flydra_calib()
c.generate_exrs(
[int(i) for i in args.display_server_numbers.split(',')],
prefer_parameter_server_properties=args.parameter_server_properties,
