def voxelize_point_cloud(self, pts):
xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pts)
occluding_xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(
pts)
if len(xyz_array) != 0:
self.origin = np.mean(xyz_array, axis=0) - np.array([
self.scale * 32 - self.voxelgrid_forward_shift,
self.scale * 32, self.scale * 32
])
vg = conversions.pointcloud_to_voxelgrid(xyz_array,
scale=self.scale,
origin=self.origin,
add_trailing_dim=True,
add_leading_dim=False)
occluding_vg = conversions.pointcloud_to_voxelgrid(
occluding_xyz_array,
scale=self.scale,
origin=self.origin,
add_trailing_dim=True,
add_leading_dim=False)
# vg = crop_vg(vg)
# vg = np.swapaxes(vg, 1,2)
elem = {'known_occ': vg}
ko, kf = data_tools.simulate_2_5D_input(vg)
_, kf = data_tools.simulate_2_5D_input(occluding_vg)
# if "YCB" in trial:
# if True:
# ko, kf = data_tools.simulate_slit_occlusion(ko, kf, x_bounds[0], x_bounds[1])
# nz = np.nonzero(ko)
# try:
# lb = min(nz[1])
# ub = max(nz[1]) - 1
# except ValueError:
# lb = 0
# ub = 63
# kf[:, 0:lb, :, 0] = 0
# kf[:, ub:, :, 0] = 0
elem['known_occ'] = ko
elem['known_free'] = kf
return elem
def test_voxelgrid_to_pointcloud_on_large_data(self):
d = load_test_files()
for i, vg_orig in enumerate(d):
pt_cloud = voxelgrid_to_pointcloud(vg_orig)
vg_new = pointcloud_to_voxelgrid(pt_cloud,
add_leading_dim=True,
add_trailing_dim=True)
self.assertTrue((vg_orig == vg_new).all(),
"Failed on point cloud {}".format(i))
def test_voxelgrid_to_pointcloud_on_simple(self):
vg_orig = np.zeros([3, 3, 3])
vg_orig[(1, 2), (0, 2), (1, 0)] = 1.0
pt_cloud = voxelgrid_to_pointcloud(vg_orig)
vg_new = pointcloud_to_voxelgrid(pt_cloud,
add_leading_dim=False,
add_trailing_dim=False,
shape=[3, 3, 3])
self.assertTrue((vg_orig == vg_new).all())
def test_voxelgrid_to_pointcloud_with_scaling(self):
vg_orig = load_test_files()[0]
for scale in [0.01, 1.0, 10., 10000.]:
pt_cloud = voxelgrid_to_pointcloud(vg_orig, scale=scale)
vg_new = pointcloud_to_voxelgrid(pt_cloud,
scale=scale,
add_leading_dim=True,
add_trailing_dim=True,
shape=(64, 64, 64))
self.assertTrue((vg_orig == vg_new).all(),
"Failed on scaling {}".format(scale))
def test_voxelgrid_to_pointcloud_with_scaling_on_simple(self):
vg_orig = np.zeros([3, 3, 3])
vg_orig[(1, 2), (0, 2), (1, 0)] = 1.0
for scale in [0.02, 1.0, 10., 10000.]:
pt_cloud = voxelgrid_to_pointcloud(vg_orig, scale=scale)
vg_new = pointcloud_to_voxelgrid(pt_cloud,
scale=scale,
add_leading_dim=False,
add_trailing_dim=False,
shape=[3, 3, 3])
self.assertTrue((vg_orig == vg_new).all())
def transform_from_gpuvoxels(view, pt_msg: PointCloud2):
transformed_cloud = view.transform_pts_to_target(pt_msg)
xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(
transformed_cloud)
# TODO: visual_conversions produces the wrong result cause the transforms are calculated differently.
# Look into this
vg = conversions.pointcloud_to_voxelgrid(
xyz_array,
scale=view.scale,
origin=view.origin,
add_trailing_dim=True,
add_leading_dim=False,
)
return vg
def voxelize_point_cloud(pts):
xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pts)
vg = conversions.pointcloud_to_voxelgrid(xyz_array, scale=scale, origin=origin, add_trailing_dim=True,
add_leading_dim=False)
vg = crop_vg(vg)
# vg = np.swapaxes(vg, 1,2)
elem = {'known_occ': vg}
ko, kf = data_tools.simulate_2_5D_input(vg)
if "YCB" in trial:
# ko, kf = data_tools.simulate_slit_occlusion(ko, kf, x_bounds[0], x_bounds[1])
kf[:, 0:x_bounds[0], :, 0] = 0
kf[:, x_bounds[1]:, :, 0] = 0
elem['known_occ'] = ko
elem['known_free'] = kf
return elem
def main():
# cloth setting
# origin = (-0.5, -0.5, 1.0) # "left most" point
# scale = 1.0/64.0 # how large the grid is
#rope setting
origin = (-0.7, -0.7, 2.0)
scale = 1.4 / 64.0
shape = (64, 64, 64) # how many grids there
shape_inp = (1, 64, 64, 64, 1)
rospy.init_node("cdcpd_shape_completion")
pub_incomp = rospy.Publisher('incomp', VoxelgridStamped, queue_size=1)
pub_comp = rospy.Publisher('comp', VoxelgridStamped, queue_size=1)
if not is_online:
in_rosbag_name = "/home/deformtrack/catkin_ws/src/cdcpd_test_blender/dataset/rope_winding_cylinder_5.bag"
out_rosbag_name = "/home/deformtrack/catkin_ws/src/cdcpd_test_blender/dataset/rope_winding_cylinder_5_comp.bag"
read_bagfile(in_rosbag_name)
mask_list = color_segmentation()
pc = imgs2pc(rgb_list[0], depth_list[0], camera_info_list[0],
mask_list[0])
voxelgrid = pointcloud_to_voxelgrid(pc,
scale=scale,
origin=origin,
shape=shape)
known_occ = np.zeros(voxelgrid.shape)
for x in range(64):
for y in range(64):
for z in range(64):
if voxelgrid[x, y, z] == 1:
known_occ[z, y, x] = 1
# known_occ = voxelgrid
voxel_inp = np.zeros(shape_inp)
free_inp = np.zeros(shape_inp)
known_free = 1.0 - known_occ
voxel_inp[0, :, :, :, 0] = known_occ
free_inp[0, :, :, :, 0] = known_free
inp = {
'known_occ': voxel_inp,
'known_free': free_inp,
}
completion = model_runner.model(inp)
# print("completion vg:")
comp_np_flip = (completion['predicted_occ'].numpy())[0, :, :, :, 0]
comp_np_flip[comp_np_flip < 0.5] = 0
comp_np_flip[comp_np_flip >= 0.5] = 1
comp_np = np.zeros(comp_np_flip.shape)
for x in range(64):
for y in range(64):
for z in range(64):
if comp_np_flip[x, y, z] == 1:
comp_np[z, y, x] = 1
# print(comp_np.shape)
# print(comp_np.sum())
# print(comp_np)
pub_incomp.publish(
conversions.vox_to_voxelgrid_stamped(
voxelgrid, # Numpy or Tensorflow
scale=scale, # Each voxel is a 1cm cube
frame_id='world', # In frame "world", same as rviz fixed frame
origin=origin)) # Bottom left corner
pub_comp.publish(
conversions.vox_to_voxelgrid_stamped(
comp_np, # Numpy or Tensorflow
scale=scale, # Each voxel is a 1cm cube
frame_id='world', # In frame "world", same as rviz fixed frame
origin=origin)) # Bottom left corner
verts, faces, normals, values = measure.marching_cubes_lewiner(
comp_np, 0)
verts = verts * scale + origin
verts_msg = numpy2multiarray(np.transpose(verts))
faces_msg = numpy2multiarray(np.transpose(faces))
normals_msg = numpy2multiarray(np.transpose(normals))
app_msgs = [verts_msg, faces_msg, normals_msg]
append_bagfile(in_rosbag_name, out_rosbag_name, app_msgs)
print("completion finished")
else:
read_live()
