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 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)
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)
print "cameras", cams
pts = a.get_points_in_cameras(r.get_cam_ids())
pt = pts[0]
print "2d points", pt
X = r.find3d(pt, return_line_coords=False)
print "3d point", X
pt = r.find2d(cams[0], X)
print "2d point in %s" % cams[0], pt
print "2d points", [r.find2d(c, X) for c in cams]
#for all laser points, find their 3D coords, check it makes a cylinder
if 0:
#use the flydra calibration to get the 3d coords
r = flydra.reconstruct.Reconstructor(cal_source=FLYDRA_CALIB)
laser = AllPointPickle()
laser.initilize_from_directory(LASER_PKL)
print "number of laser points", laser.num_points
#all points visible in 2 or more cameras
pts = laser.get_points_in_cameras(r.get_cam_ids())
create_pcd_file_from_points(
'/tmp/cyl.pcd', [r.find3d(pt, return_line_coords=False) for pt in pts])
pts = a.get_points_in_cameras(r.get_cam_ids())
pt = pts[0]
print "2d points",pt
X = r.find3d(pt,return_line_coords=False)
print "3d point",X
pt = r.find2d(cams[0],X)
print "2d point in %s" % cams[0],pt
print "2d points",[r.find2d(c,X) for c in cams]
#for all laser points, find their 3D coords, check it makes a cylinder
if 0:
#use the flydra calibration to get the 3d coords
r = flydra.reconstruct.Reconstructor(cal_source=FLYDRA_CALIB)
laser = AllPointPickle()
laser.initilize_from_directory(LASER_PKL)
print "number of laser points",laser.num_points
#all points visible in 2 or more cameras
pts = laser.get_points_in_cameras(r.get_cam_ids())
create_pcd_file_from_points(
'/tmp/cyl.pcd',
[r.find3d(pt,return_line_coords=False) for pt in pts])
def export(self, fname):
print "saving to", fname
create_pcd_file_from_points(fname, self.xyz)
print "done saving"
def export(self,fname):
print 'saving to',fname
create_pcd_file_from_points(fname,
self.xyz)
print 'done saving'
