def __init__(self, display_servers, cams, mask_dir, trigger):
self.mask_dir = mask_dir
cam_handlers = []
#turn on projectors
for d in display_servers:
dsc = display_client.DisplayServerProxy(d,wait=True)
dsc.enter_2dblit_mode()
dsc.show_pixels(dsc.new_image(dsc.IMAGE_COLOR_WHITE))
for cam in cams:
rospy.loginfo("Generating mask for cam %s" % cam)
cam_handlers.append(CameraHandler(cam,debug=False))
self.trigger_proxy_rate = rospy.ServiceProxy(trigger+'/set_framerate', camera_trigger.srv.SetFramerate)
self.trigger_proxy_rate(1.0)
rospy.loginfo("Set framerate to 1fps")
self.runner = SimultaneousCameraRunner(cam_handlers)
s = rospy.Service('~mask_start', std_srvs.srv.Empty, self._change_mode_start)
s = rospy.Service('~mask_finish', std_srvs.srv.Empty, self._change_mode_finish)
self.mode = self.MODE_WAIT
def show_image(ds,viewport,fname,white,black,rgb,pixel, ptsize, scale=False):
rospy.init_node('show_image')
dsc = display_client.DisplayServerProxy(ds,wait=True)
dsc.enter_2dblit_mode()
if viewport:
mask = dsc.get_virtual_display_mask(viewport)
else:
mask = None
if rgb != (-1,-1,-1):
arr = dsc.new_image(rgb, mask)
elif white:
arr = dsc.new_image(dsc.IMAGE_COLOR_WHITE, mask)
elif black:
arr = dsc.new_image(dsc.IMAGE_COLOR_BLACK, mask)
else:
arr = scipy.misc.imread(fname)
if arr.shape!=(dsc.height,dsc.width):
arr = arr[0:min(dsc.height,arr.shape[0]),0:min(dsc.width,arr.shape[1]),:]
if mask != None:
masks = np.dstack([mask for i in range(0,arr.shape[-1])])
if arr.shape != masks.shape:
arr = np.resize(arr, masks.shape)
arr *= masks
if pixel and (white or black or (rgb != (-1,-1,-1))):
col,row = map(int,pixel.split(','))
if white:
arr = dsc.new_image(dsc.IMAGE_COLOR_BLACK, mask)
rgb = (dsc.IMAGE_COLOR_WHITE, dsc.IMAGE_COLOR_WHITE, dsc.IMAGE_COLOR_WHITE)
elif black:
arr = dsc.new_image(dsc.IMAGE_COLOR_WHITE, mask)
rgb = (dsc.IMAGE_COLOR_BLACK, dsc.IMAGE_COLOR_BLACK, dsc.IMAGE_COLOR_BLACK)
else:
arr = dsc.new_image(dsc.IMAGE_COLOR_BLACK, mask)
for i,c in enumerate(rgb):
arr[row-ptsize:row+ptsize,col-ptsize:col+ptsize,i] = c
if scale:
orig_aspect = arr.shape[1]/float(arr.shape[0]) # w/h
native_aspect = dsc.width/float(dsc.height)
if native_aspect >= orig_aspect:
# display is wider than image
new_shape_height_h = int(dsc.width/float(orig_aspect))
new_shape_full = new_shape_height_h, dsc.width
else:
# display is taller than image
new_shape_wide_w = int(orig_aspect*dsc.height)
new_shape_full = dsc.height, new_shape_wide_w
new_image = scipy.misc.imresize( arr, new_shape_full )
arr = new_image[:dsc.height, :dsc.width]
dsc.show_pixels(arr)
def on_connect_to_display_server(self,*args):
e = self._ui.get_object('display_server_name_entry')
ds_name = e.get_text()
self.dsc.proxy_set_dsc(display_client.DisplayServerProxy(ds_name))
self.update_dsc_sensitivity(True)
self.update_bg_image()
if self._ui.get_object('display_server_populate_cb').get_active():
gi = self.dsc.get_geometry_info()
di = self.dsc.get_display_info()
fname = "running display server %s" % ds_name
self.load_display({"display":di},fname)
self.load_geom({"geom":gi},fname)
def load(self, b, nowait_display_server):
with rosbag.Bag(b, 'r') as bag:
rospy.loginfo("processing %s" % b)
self.filenames.append(b)
for topic, msg, t in bag.read_messages(topics=[CALIB_MAPPING_TOPIC]):
key = msg.display_server
try:
self.data[key]
except KeyError:
self.data[key] = {}
dsc = display_client.DisplayServerProxy(key,
wait=not nowait_display_server,
prefer_parameter_server_properties=nowait_display_server)
mask = dsc.get_display_mask()
cvimg = dsc.new_image(color=255, mask=~mask, nchan=3, dtype=np.uint8)
self.dscs[key] = dsc
self.cvimgs[key] = cvimg
self.masks[key] = mask
if self.visualize:
cv2.namedWindow(key)
finally:
#support old bag files that lacked luminace information
try:
luminance = msg.pixel_ptc_projector_luminance
except AttributeError:
luminance = 255
pixel = np.array((msg.pixel_projector.x, msg.pixel_projector.y))
#recompute 3D position
if self.flydra:
pts = []
for projpt in msg.points:
pts.append( (projpt.camera,(projpt.pixel.x,projpt.pixel.y)) )
xyz = self.flydra.find3d(pts,return_line_coords=False, undistort=True)
else:
xyz = np.array((msg.position.x,msg.position.y,msg.position.z))
try:
self.data[msg.display_server][msg.vdisp].append( [xyz,pixel,luminance] )
except KeyError:
self.data[msg.display_server][msg.vdisp] = [ [xyz,pixel,luminance] ]
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
'--display-server', type=str, metavar='/display_server', required=True, help=\
'the path of the display server to configure')
parser.add_argument(
'--viewport', type=str, metavar='vdisp', required=True, help=\
'the id of the virtual display on the display server')
parser.add_argument(
'--wait', action='store_true', default=False, help=\
'wait for display server to start (useful when roslaunched)')
parser.add_argument(
'--show-grid', action='store_true', default=False, help=\
'show red and blue grid in the viewports')
# use argparse, but only after ROS did its thing
argv = rospy.myargv()
args = parser.parse_args(argv[1:])
rospy.init_node('viewport_definer')
display_server = display_client.DisplayServerProxy(args.display_server,
args.wait)
display_server.enter_standby_mode()
display_server.set_mode('display2d')
display_info = display_server.get_display_info()
demo = ViewportDefiner(display_server=display_server,
display_info=display_info,
width=display_info['width'],
height=display_info['height'],
display_name=args.display_server,
viewport_id=args.viewport,
show_grid=args.show_grid)
tmp = demo.linedraw # trigger default value to be initialized. (XXX how else to do this?)
demo.configure_traits()
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)
import flyvr.display_client as display_client
import calib.acquire
if __name__ == "__main__":
dsn = "/display_server3"
camn = "/Basler_21017520"
nimgs = 20
greys = range(0, 255 + 5, 5) + [64, 127, 191]
col, row = 519, 390
ptsize = 3
rospy.init_node('show_foo')
dsc = display_client.DisplayServerProxy(dsn)
dsc.enter_2dblit_mode()
runner = calib.acquire.SequentialCameraRunner(
(calib.acquire.CameraHandler(camn), ), queue_depth=nimgs)
for g in greys:
print "GRAY %d" % g
rgb = (g, g, g)
arr = dsc.new_image(dsc.IMAGE_COLOR_BLACK, None)
for i, c in enumerate(rgb):
arr[row - ptsize:row + ptsize, col - ptsize:col + ptsize, i] = c
dsc.show_pixels(arr)