def as_boxes(self):
geom = trimesh.voxel.VoxelGrid(
self.matrix, ttf.scale_and_translate(self.pitch, self.origin))
geom = geom.as_boxes()
I, J, K = zip(*np.argwhere(self.matrix))
geom.visual.face_colors = self.values[I, J, K].repeat(12, axis=0)
return geom
def check_average_voxelization_3d(origin, pitch, points, values, gpu,
**kwargs):
batch_indices = np.zeros((points.shape[0], ), dtype=np.int32)
if gpu >= 0:
cuda.get_device_from_id(gpu).use()
values = cuda.to_gpu(values)
points = cuda.to_gpu(points)
batch_indices = cuda.to_gpu(batch_indices)
y = morefusion.functions.average_voxelization_3d(
values,
points,
batch_indices,
batch_size=1,
origin=origin,
pitch=pitch,
dimensions=(32, 32, 32),
)[0]
y = y.transpose(1, 2, 3, 0)
matrix_values = cuda.to_cpu(y.array)
matrix_filled = (matrix_values != 0).any(axis=3)
scene = trimesh.Scene()
scene.angles = np.zeros(3)
geom = trimesh.voxel.VoxelGrid(matrix_filled,
ttf.scale_and_translate(pitch,
origin)).as_boxes()
I, J, K = zip(*np.argwhere(matrix_filled))
geom.visual.face_colors = matrix_values[I, J, K].repeat(12, axis=0)
scene.add_geometry(geom)
def callback(scene):
scene.set_camera(angles=scene.angles)
scene.angles += [0, np.deg2rad(1), 0]
trimesh.viewer.SceneViewer(scene=scene, callback=callback, **kwargs)
def visualize_data():
data = contrib.get_data()
models = morefusion.datasets.YCBVideoModels()
colormap = imgviz.label_colormap()
scenes = {
"pcd": trimesh.Scene(),
"grid_target": trimesh.Scene(),
"grid_nontarget_empty": trimesh.Scene(),
"cad": trimesh.Scene(),
}
rgb = data["rgb"]
depth = data["depth"]
K = data["intrinsic_matrix"]
pcd = morefusion.geometry.pointcloud_from_depth(depth,
fx=K[0, 0],
fy=K[1, 1],
cx=K[0, 2],
cy=K[1, 2])
nonnan = ~np.isnan(depth)
geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan])
scenes["pcd"].add_geometry(geom)
# scenes["cad"].add_geometry(geom)
T_world2cam = None
for instance in data["instances"]:
if T_world2cam is None:
T_world2cam = np.linalg.inv(instance["T_cam2world"])
class_id = instance["class_id"]
transform = instance["transform_init"]
grid_target = instance["grid_target"]
grid_nontarget_empty = instance["grid_nontarget_empty_noground"]
cad = models.get_cad(class_id=class_id)
# if hasattr(cad.visual, "to_color"):
# cad.visual = cad.visual.to_color()
scenes["pcd"].add_geometry(
cad,
node_name=str(instance["id"]),
geom_name=str(instance["id"]),
transform=transform,
)
scenes["cad"].add_geometry(
cad,
node_name=str(instance["id"]),
geom_name=str(instance["id"]),
transform=transform,
)
transform_vg = ttf.scale_and_translate(scale=instance["pitch"],
translate=instance["origin"])
if instance["id"] == 0:
color_id = 3
elif instance["id"] == 1:
color_id = 1
elif instance["id"] == 2:
color_id = 4
else:
raise ValueError
geom = trimesh.voxel.VoxelGrid(
grid_target,
transform=transform_vg).as_boxes(colors=colormap[color_id])
scenes["grid_target"].add_geometry(geom)
points = np.argwhere(grid_nontarget_empty)
points = points * instance["pitch"] + instance["origin"]
geom = trimesh.PointCloud(vertices=points, colors=colormap[color_id])
# geom = trimesh.voxel.VoxelGrid(
# grid_nontarget_empty, transform=transform_vg
# ).as_boxes(colors=colormap[instance["id"] + 1])
scenes["grid_nontarget_empty"].add_geometry(geom)
camera_transform = morefusion.extra.trimesh.to_opengl_transform()
for scene in scenes.values():
# scene.add_geometry(contrib.grid(scale=0.1), transform=T_world2cam)
scene.camera_transform = camera_transform
return scenes
def get_scene(dataset):
camera = trimesh.scene.Camera(fov=(30, 22.5))
index = 0
frame = dataset.get_frame(index)
examples = dataset.get_example(index)
scenes = {
"rgb": None,
}
camera_transform = morefusion.extra.trimesh.to_opengl_transform()
vizs = [frame["rgb"]]
for i, example in enumerate(examples):
viz = imgviz.tile(
[
example["rgb"],
imgviz.depth2rgb(example["pcd"][:, :, 0]),
imgviz.depth2rgb(example["pcd"][:, :, 1]),
imgviz.depth2rgb(example["pcd"][:, :, 2]),
],
border=(255, 255, 255),
)
viz = imgviz.draw.text_in_rectangle(
viz,
"lt",
f"visibility: {example['visibility']:.0%}",
size=30,
background=(0, 255, 0),
color=(0, 0, 0),
)
vizs.append(viz)
geom = trimesh.voxel.VoxelGrid(
example["grid_target"],
ttf.scale_and_translate(example["pitch"], example["origin"]),
).as_boxes(colors=(1.0, 0, 0, 0.5))
scenes[f"occupied_{i:04d}"] = trimesh.Scene(
geom, camera=camera, camera_transform=camera_transform)
geom = trimesh.voxel.VoxelGrid(
example["grid_nontarget"],
ttf.scale_and_translate(example["pitch"], example["origin"]),
).as_boxes(colors=(0, 1.0, 0, 0.5))
scenes[f"occupied_{i:04d}"].add_geometry(geom)
geom = trimesh.voxel.VoxelGrid(
example["grid_empty"],
ttf.scale_and_translate(example["pitch"], example["origin"]),
).as_boxes(colors=(0.5, 0.5, 0.5, 0.5))
scenes[f"empty_{i:04d}"] = trimesh.Scene(
geom, camera=camera, camera_transform=camera_transform)
scenes[f"full_occupied_{i:04d}"] = trimesh.Scene(
camera=camera, camera_transform=camera_transform)
if (example["grid_target_full"] > 0).any():
geom = trimesh.voxel.VoxelGrid(
example["grid_target_full"],
ttf.scale_and_translate(example["pitch"], example["origin"]),
).as_boxes(colors=(1.0, 0, 0, 0.5))
scenes[f"full_occupied_{i:04d}"].add_geometry(geom)
if (example["grid_nontarget_full"] > 0).any():
colors = imgviz.label2rgb(
example["grid_nontarget_full"].reshape(1, -1) +
1).reshape(example["grid_nontarget_full"].shape + (3, ))
geom = trimesh.voxel.VoxelGrid(
example["grid_nontarget_full"],
ttf.scale_and_translate(example["pitch"], example["origin"]),
).as_boxes(colors=colors)
scenes[f"full_occupied_{i:04d}"].add_geometry(geom)
dim = example["grid_target"].shape[0]
extents = np.array([dim, dim, dim]) * example["pitch"]
geom = trimesh.path.creation.box_outline(extents)
geom.apply_translation(example["origin"] +
(dim / 2 - 0.5) * example["pitch"])
scenes[f"occupied_{i:04d}"].add_geometry(geom)
scenes[f"empty_{i:04d}"].add_geometry(geom)
viz = imgviz.tile(vizs)
scenes["rgb"] = viz
return scenes
dim = 16
pitch = max(geom.extents) / dim * 1.1
origin = (-pitch * dim / 2, ) * 3
sdf = models.get_cad(class_id=2).nearest.signed_distance(points)
print(f"[{name}] pitch: {pitch}")
print(f"[{name}] dim: {dim}")
grid, _, _ = morefusion.functions.pseudo_occupancy_voxelization(
points=cuda.to_gpu(points),
sdf=cuda.to_gpu(sdf),
pitch=pitch,
origin=origin,
dims=(dim, ) * 3,
threshold=2,
)
grid = cuda.to_cpu(grid.array)
colors = imgviz.depth2rgb(grid.reshape(1, -1), min_value=0, max_value=1)
colors = colors.reshape(dim, dim, dim, 3)
colors = np.concatenate((colors, np.full((dim, dim, dim, 1), 127)), axis=3)
voxel = trimesh.voxel.VoxelGrid(grid,
ttf.scale_and_translate(pitch, origin))
geom = voxel.as_boxes()
I, J, K = zip(*np.argwhere(grid))
geom.visual.face_colors = colors[I, J, K].repeat(12, axis=0)
scene.add_geometry(geom)
scenes[name] = scene
morefusion.extra.trimesh.display_scenes(scenes, tile=(1, 2))
def visualize_grids(
camera,
camera_transform,
rgb,
pcd,
pitch,
origin,
grid_target,
grid_nontarget,
grid_empty,
):
scenes = {}
scene_common = trimesh.Scene(camera=camera)
# point_cloud
nonnan = ~np.isnan(pcd).any(axis=2)
geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan])
scene_common.add_geometry(geom, geom_name="point_cloud")
# grid_aabb
dimensions = np.array(grid_target.shape)
center = origin + dimensions / 2 * pitch
geom = trimesh.path.creation.box_outline(
extents=dimensions * pitch,
transform=ttf.translation_matrix(center),
)
scene_common.add_geometry(geom, geom_name="grid_aabb")
# camera
camera_marker = camera.copy()
camera_marker.transform = morefusion.extra.trimesh.from_opengl_transform(
camera_transform)
geom = trimesh.creation.camera_marker(camera_marker, marker_height=0.1)
scene_common.add_geometry(geom, geom_name="camera_marker")
scenes["occupied"] = trimesh.Scene(
camera=scene_common.camera,
geometry=scene_common.geometry,
camera_transform=camera_transform,
)
# grid_target
voxel = trimesh.voxel.VoxelGrid(
grid_target,
ttf.scale_and_translate(pitch, origin),
)
geom = voxel.as_boxes(colors=(1.0, 0, 0, 0.5))
scenes["occupied"].add_geometry(geom, geom_name="grid_target")
# grid_nontarget
voxel = trimesh.voxel.VoxelGrid(
grid_nontarget,
ttf.scale_and_translate(pitch, origin),
)
geom = voxel.as_boxes(colors=(0, 1.0, 0, 0.5))
scenes["occupied"].add_geometry(geom, geom_name="grid_nontarget")
scenes["empty"] = trimesh.Scene(
camera=scene_common.camera,
geometry=scene_common.geometry,
camera_transform=camera_transform,
)
# grid_empty
voxel = trimesh.voxel.VoxelGrid(grid_empty,
ttf.scale_and_translate(pitch, origin))
geom = voxel.as_boxes(colors=(0.5, 0.5, 0.5, 0.5))
scenes["empty"].add_geometry(geom, geom_name="grid_empty")
return scenes
def visualize(self): # NOQA
# scene-level
rgb = self._rgb
pcd = self._pcd
scenes = {}
# ---------------------------------------------------------------------
# instance-level
instance_id = self._instance_id
cad = self._cads[instance_id]
T_cad2cam_true = self._Ts_cad2cam_true[instance_id]
T_cad2cam_pred = self._Ts_cad2cam_pred[instance_id]
grid_target = self._grids
pitch = self._pitch
origin = self._origin
scene = trimesh.Scene()
# occupied target/untarget
voxel = trimesh.voxel.VoxelGrid(grid_target[0],
ttf.scale_and_translate(pitch, origin))
geom = voxel.as_boxes((1.0, 0, 0, 0.5))
scene.add_geometry(geom, geom_name="occupied_target")
voxel = trimesh.voxel.VoxelGrid(
grid_target[1],
ttf.scale_and_translate(pitch, origin),
)
geom = voxel.as_boxes((0, 1.0, 0, 0.5))
scene.add_geometry(geom, geom_name="occupied_untarget")
scenes["instance_occupied"] = scene
# empty
scene = trimesh.Scene()
voxel = trimesh.voxel.VoxelGrid(grid_target[2],
ttf.scale_and_translate(pitch, origin))
geom = voxel.as_boxes((0.5, 0.5, 0.5, 0.5))
scene.add_geometry(geom, geom_name="empty")
scenes["instance_empty"] = scene
scene = trimesh.Scene()
# cad_true
cad_trans = cad.copy()
cad_trans.visual.vertex_colors[:, 3] = 127
scene.add_geometry(
cad_trans,
transform=T_cad2cam_true,
geom_name="cad_true",
node_name="cad_true",
)
scenes["instance_cad"] = scene
# cad_pred
for scene in scenes.values():
scene.add_geometry(
cad,
transform=T_cad2cam_pred,
geom_name="cad_pred",
node_name="cad_pred",
)
# bbox
aabb_min = origin - pitch / 2
aabb_max = aabb_min + pitch * np.array(grid_target.shape[1:])
geom = trimesh.path.creation.box_outline(aabb_max - aabb_min)
geom.apply_translation((aabb_min + aabb_max) / 2)
for scene in scenes.values():
scene.add_geometry(geom, geom_name="bbox")
# ---------------------------------------------------------------------
# scene_pcd
scenes["scene_pcd_only"] = trimesh.Scene()
scenes["scene_cad"] = trimesh.Scene()
scenes["scene_pcd"] = trimesh.Scene()
nonnan = ~np.isnan(pcd).any(axis=2)
geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan])
scenes["scene_pcd_only"].add_geometry(geom, geom_name="pcd")
scenes["scene_pcd"].add_geometry(geom, geom_name="pcd")
for instance_id in self._instance_ids:
if instance_id not in self._cads:
continue
cad = self._cads[instance_id]
T_cad2cam_pred = self._Ts_cad2cam_pred[instance_id]
if cad:
for key in ["scene_cad", "scene_pcd"]:
scenes[key].add_geometry(
cad,
transform=T_cad2cam_pred,
geom_name=f"cad_pred_{instance_id}",
node_name=f"cad_pred_{instance_id}",
)
# scene_occupancy
colormap = imgviz.label_colormap()
scenes["scene_occupied"] = trimesh.Scene()
scenes["scene_empty"] = trimesh.Scene()
for instance_id in self._mapping.instance_ids:
occupied, empty = self._mapping.get_target_pcds(instance_id)
color = trimesh.visual.to_rgba(colormap[instance_id])
color[3] = 127
geom = trimesh.voxel.ops.multibox(occupied,
pitch=0.01,
colors=color)
scenes["scene_occupied"].add_geometry(
geom, geom_name=f"occupied_{instance_id}")
geom = trimesh.PointCloud(vertices=empty, colors=[0.5, 0.5, 0.5])
scenes["scene_empty"].add_geometry(
geom, geom_name=f"empty_{instance_id}")
# set camera
camera = trimesh.scene.Camera(
resolution=(640, 480),
fov=(60 * 0.7, 45 * 0.7),
)
camera_transform = morefusion.extra.trimesh.to_opengl_transform()
for scene in scenes.values():
scene.camera = camera
scene.camera_transform = camera_transform
return scenes