def smooth(self, amount):
rospy.loginfo("smoothing points with MLS filter %f" % amount)
self.smoothed = amount
all_3d = []
for ds in self.display_servers:
for vdisp in self.data[ds]:
for xyz,pixel,lum in self.data[ds][vdisp]:
all_3d.append(xyz)
create_point_cloud_message_publisher(all_3d,'/calibration/pre_smooth',
publish_now=True, latch=True)
p = pcl.PointCloud()
p.from_array(np.array(all_3d,dtype=np.float32))
smoothed = p.filter_mls(amount).to_list()
i = 0
all_3d = []
for ds in self.display_servers:
for vdisp in self.data[ds]:
for xyz,pixel,lum in self.data[ds][vdisp]:
sxyz = smoothed[i]
xyz[0] = sxyz[0]; xyz[1] = sxyz[1]; xyz[2] = sxyz[2]
all_3d.append(xyz)
i += 1
create_point_cloud_message_publisher(all_3d,'/calibration/post_smooth',
publish_now=True, latch=True)
def smooth(self, amount):
rospy.loginfo("smoothing points with MLS filter %f" % amount)
self.smoothed = amount
all_3d = []
for ds in self.display_servers:
for vdisp in self.data[ds]:
for xyz, pixel, lum in self.data[ds][vdisp]:
all_3d.append(xyz)
create_point_cloud_message_publisher(all_3d,
'/calibration/pre_smooth',
publish_now=True,
latch=True)
p = pcl.PointCloud()
p.from_array(np.array(all_3d, dtype=np.float32))
smoothed = p.filter_mls(amount).to_list()
i = 0
all_3d = []
for ds in self.display_servers:
for vdisp in self.data[ds]:
for xyz, pixel, lum in self.data[ds][vdisp]:
sxyz = smoothed[i]
xyz[0] = sxyz[0]
xyz[1] = sxyz[1]
xyz[2] = sxyz[2]
all_3d.append(xyz)
i += 1
create_point_cloud_message_publisher(all_3d,
'/calibration/post_smooth',
publish_now=True,
latch=True)
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")
