def conv(elem, origin):
return visual_conversions.vox_to_pointcloud2_msg(
elem['known_occ'],
scale=self.scale,
frame='gpu_voxel_world',
origin=origin,
density_factor=3)
def get_gt(self, density_factor=3):
pts = visual_conversions.vox_to_pointcloud2_msg(
self.elem['gt_occ'],
scale=self.scale,
frame='gpu_voxel_world',
origin=self.origin,
density_factor=density_factor)
return pts
def get_segmented_points(self):
pts = visual_conversions.vox_to_pointcloud2_msg(
self.elem['known_occ'],
scale=self.scale,
frame='gpu_voxel_world',
origin=self.origin,
density_factor=3)
return [pts]
def publish(self, name, voxelgrid):
if tf.is_tensor(voxelgrid):
voxelgrid = voxelgrid.numpy()
msg = conversions.vox_to_pointcloud2_msg(voxelgrid,
frame=self.frame,
scale=self.scale,
origin=self.origin,
density_factor=3)
msg = self.transformer.transform_to_frame(msg, "gpu_voxel_world")
self.pubs[name].publish(msg)
self.last_pt_cloud = msg
def get_gt(self, density_factor=3):
vg = np.ones((8, 8, 11))
scale = 0.02
origin = visual_conversions.get_origin_in_voxel_coordinates(
(1.45, 1.92, 1.43), scale=scale)
pts = visual_conversions.vox_to_pointcloud2_msg(
vg,
scale=scale,
frame='gpu_voxel_world',
origin=origin,
density_factor=3)
return pts
def get_gt(self, density_factor=3):
# CHEEZITs
vg = np.ones((3, 8, 10))
scale = 0.02
# cheezit cheezit cheezit cheezit cheezit cheezit cheezit cheezit cheezit
origin = visual_conversions.get_origin_in_voxel_coordinates(
(1.55, 2.01, 1.17), scale=scale)
cheezit_pts = visual_conversions.vox_to_pointcloud2_msg(
vg,
scale=scale,
frame='gpu_voxel_world',
origin=origin,
density_factor=3)
# Pitcher Pitcher Pitcher Pitcher Pitcher Pitcher Pitcher Pitcher Pitcher
origin = get_origin_in_voxel_coordinates((1.43, 1.60, 1.05),
self.scale)
pitcher_pts = visual_conversions.vox_to_pointcloud2_msg(
self.elem['gt_occ'],
scale=self.scale,
frame='gpu_voxel_world',
origin=origin,
density_factor=density_factor)
return combine_pointcloud2s([cheezit_pts, pitcher_pts])
def transform_to_gpuvoxels(self, view, vg) -> PointCloud2:
# TODO: It is odd that I use visual_conversions here, since I used conversions (not visual, different package
# of mine) get the pointcloud in the first place. However, visual_conversions has this nice function which
# densifies the points
msg = visual_conversions.vox_to_pointcloud2_msg(
vg,
frame=view.target_frame,
scale=view.scale,
origin=-view.origin / view.scale,
density_factor=self.completion_density)
# pt = conversions.voxelgrid_to_pointcloud(vg, scale=self.robot_view.scale,
# origin=self.robot_view.origin)
msg = view.transform_pts_to_target(msg, target_frame="gpu_voxel_world")
return msg
