def gen_exr(fname=None, width=None, height=None, luminance=None):
u = np.linspace(0.0, 1.0, width)
v = np.linspace(0.0, 1.0, height)
U, V = np.meshgrid(u, v)
B = luminance * np.ones_like(U)
save_exr(fname, r=U, g=V, b=B)
def save_calibration_exr(self,fname):
state_dict = self._get_state_as_dict()
obj = {'pinhole_wizard_input':state_dict}
tcs = np.zeros( (self.dsc.height,self.dsc.width,2))-1
dist = np.nan*np.ones( (self.dsc.height,self.dsc.width))
angle = np.nan*np.ones( (self.dsc.height,self.dsc.width))
allmask = np.zeros((self.dsc.height,self.dsc.width))
di = self.dsc.get_display_info()
for row in self.vdisp_store:
vdisp = row[VS_VDISP]
for d in di['virtualDisplays']:
if d['id'] != vdisp:
continue
else:
break
assert d['id'] == vdisp
polygon_verts = d['viewport']
maskarr = np.zeros( allmask.shape, dtype=np.uint8 )
fill_polygon.fill_polygon(polygon_verts, maskarr)
if np.max(maskarr)==0: # no mask
maskarr += 1
allmask += maskarr
mask = np.nonzero(maskarr)
camera = row[VS_CAMERA_OBJECT]
assert camera is not None
this_tcs = self.geom.compute_for_camera_view(camera,
what='texture_coords')
this_dist = self.geom.compute_for_camera_view(camera,
what='distance' )
this_angle = self.geom.compute_for_camera_view(camera,
what='incidence_angle' )
this_tcs[ np.isnan(this_tcs) ] = -1.0 # nan -> -1
# copy the important parts to the full display image
tcs[mask] = this_tcs[mask]
dist[mask] = this_dist[mask]
angle[mask] = this_angle[mask]
r=tcs[:,:,0]
g=tcs[:,:,1]
if 0:
# Replace this code with something that calculates a real
# blending value here based on distance. Probably need to
# normalize by the maximum distance.
raise NotImplementedError("b = f(dist,angle)")
else:
b=np.ones_like(tcs[:,:,1])
exr.save_exr( fname, r=r, g=g, b=b)
if 1:
# save low dynamic range .png image
ri = np.array( r*255, dtype=np.uint8 )
gi = np.array( g*255, dtype=np.uint8 )
bi = np.array( b*255, dtype=np.uint8 )
h,w = r.shape[:2]
imi = np.empty( (h,w,3), dtype=np.uint8 )
imi[:,:,0] = ri
imi[:,:,1] = gi
imi[:,:,2] = bi
scipy.misc.imsave(fname+'.png', imi)
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")
subprocess.call(["rosnode", "kill", "/%s" % ds])
rospy.loginfo("restarted server")
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")
subprocess.call(["rosnode", "kill", "/%s"%ds])
rospy.loginfo("restarted server")
def save_calibration_exr(self, fname):
state_dict = self._get_state_as_dict()
obj = {'pinhole_wizard_input': state_dict}
tcs = np.zeros((self.dsc.height, self.dsc.width, 2)) - 1
dist = np.nan * np.ones((self.dsc.height, self.dsc.width))
angle = np.nan * np.ones((self.dsc.height, self.dsc.width))
allmask = np.zeros((self.dsc.height, self.dsc.width))
di = self.dsc.get_display_info()
for row in self.vdisp_store:
vdisp = row[VS_VDISP]
for d in di['virtualDisplays']:
if d['id'] != vdisp:
continue
else:
break
assert d['id'] == vdisp
polygon_verts = d['viewport']
maskarr = np.zeros(allmask.shape, dtype=np.uint8)
fill_polygon.fill_polygon(polygon_verts, maskarr)
if np.max(maskarr) == 0: # no mask
maskarr += 1
allmask += maskarr
mask = np.nonzero(maskarr)
camera = row[VS_CAMERA_OBJECT]
assert camera is not None
this_tcs = self.geom.compute_for_camera_view(camera,
what='texture_coords')
this_dist = self.geom.compute_for_camera_view(camera,
what='distance')
this_angle = self.geom.compute_for_camera_view(
camera, what='incidence_angle')
this_tcs[np.isnan(this_tcs)] = -1.0 # nan -> -1
# copy the important parts to the full display image
tcs[mask] = this_tcs[mask]
dist[mask] = this_dist[mask]
angle[mask] = this_angle[mask]
r = tcs[:, :, 0]
g = tcs[:, :, 1]
if 0:
# Replace this code with something that calculates a real
# blending value here based on distance. Probably need to
# normalize by the maximum distance.
raise NotImplementedError("b = f(dist,angle)")
else:
b = np.ones_like(tcs[:, :, 1])
exr.save_exr(fname, r=r, g=g, b=b)
if 1:
# save low dynamic range .png image
ri = np.array(r * 255, dtype=np.uint8)
gi = np.array(g * 255, dtype=np.uint8)
bi = np.array(b * 255, dtype=np.uint8)
h, w = r.shape[:2]
imi = np.empty((h, w, 3), dtype=np.uint8)
imi[:, :, 0] = ri
imi[:, :, 1] = gi
imi[:, :, 2] = bi
scipy.misc.imsave(fname + '.png', imi)
