def vis(coords, node_indices, point_indices=None):
import trimesh
depth = len(coords)
scale_factor = np.max(np.linalg.norm(coords[0], axis=-1))
for c in coords:
c /= scale_factor
if point_indices is None:
point_indices = [0] * depth
for i, point_index in enumerate(point_indices):
ci = coords[i]
center = ci[point_index]
for j in range(i + 1):
cj = coords[j]
ni = node_indices[i][j]
neighbors = ni[point_index]
print(i, j, ci.shape, cj.shape, neighbors.shape)
print(np.max(np.linalg.norm(cj[neighbors] - center, axis=-1)))
scene = trimesh.Scene()
scene.add_geometry(trimesh.PointCloud(cj, color=(0, 255, 0)))
scene.add_geometry(trimesh.PointCloud(ci, color=(0, 0, 255)))
scene.add_geometry(
trimesh.PointCloud(cj[neighbors], color=(255, 0, 0)))
scene.add_geometry(
trimesh.primitives.Sphere(center=center, radius=0.01))
scene.show(background=(0, 0, 0))
def algorithm():
gpu = 0
if gpu >= 0:
cuda.get_device_from_id(gpu).use()
instance_id = 3 # == class_id
models = morefusion.datasets.YCBVideoModels()
pcd_cad = models.get_pcd(class_id=instance_id)
dataset = morefusion.datasets.YCBVideoDataset("train")
example = dataset.get_example(1000)
depth = example["depth"]
instance_label = example["label"]
K = example["meta"]["intrinsic_matrix"]
pcd_depth = 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(pcd_depth).any(axis=2)
mask = (instance_label == instance_id) & nonnan
pcd_depth_target = pcd_depth[mask]
# -------------------------------------------------------------------------
quaternion_init = np.array([1, 0, 0, 0], dtype=np.float32)
translation_init = np.median(pcd_depth_target, axis=0)
transform_init = morefusion.functions.transformation_matrix(
quaternion_init, translation_init).array
pcd_cad = morefusion.extra.open3d.voxel_down_sample(pcd_cad,
voxel_size=0.01)
pcd_depth_target = morefusion.extra.open3d.voxel_down_sample(
pcd_depth_target, voxel_size=0.01)
registration = ICPRegistration(pcd_depth_target, pcd_cad, transform_init)
for T_cad2cam in registration.register_iterative():
scene = trimesh.Scene()
geom = trimesh.PointCloud(pcd_depth_target, colors=[1.0, 0, 0])
scene.add_geometry(geom, geom_name="a", node_name="a")
geom = trimesh.PointCloud(pcd_cad, colors=[0, 1.0, 0])
scene.add_geometry(geom,
geom_name="b",
node_name="b",
transform=T_cad2cam)
scene.camera_transform = morefusion.extra.trimesh.to_opengl_transform()
yield scene
def vis_clouds(in_coords, out_coords, target_index, neighbors):
out_colors = np.ones((out_coords.shape[0], 3), dtype=np.uint8)
out_colors *= 128
out_colors[target_index] = [0, 255, 0]
out_cloud = trimesh.PointCloud(out_coords, out_colors)
in_colors = np.zeros((in_coords.shape[0], 3), dtype=np.uint8)
in_colors[:, 2] = 255
neigh_indices = neighbors[neighbors[:, 0] == target_index, 1]
in_colors[neigh_indices] = [255, 0, 0]
in_cloud = trimesh.PointCloud(in_coords, in_colors)
scene = in_cloud.scene()
scene.add_geometry(out_cloud)
scene.show(background=(0, 0, 0))
def visualize(occupied, empty, K, width, height, rgb, pcd, mask, resolution,
aabb):
window = pyglet.window.Window(width=int(640 * 0.9 * 3),
height=int(480 * 0.9))
@window.event
def on_key_press(symbol, modifiers):
if modifiers == 0:
if symbol == pyglet.window.key.Q:
window.on_close()
gui = glooey.Gui(window)
hbox = glooey.HBox()
hbox.set_padding(5)
camera = trimesh.scene.Camera(
resolution=(width, height),
focal=(K[0, 0], K[1, 1]),
transform=np.eye(4),
)
camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1)
# initial camera pose
camera.transform = np.array(
[[0.73256052, -0.28776419, 0.6168848, 0.66972396],
[-0.26470017, -0.95534823, -0.13131483, -0.12390466],
[0.62712751, -0.06709345, -0.77602162, -0.28781298], [
0.,
0.,
0.,
1.,
]], )
aabb_min, aabb_max = aabb
bbox = trimesh.path.creation.box_outline(
aabb_max - aabb_min,
tf.translation_matrix((aabb_min + aabb_max) / 2),
)
geom = trimesh.PointCloud(vertices=pcd[mask], colors=rgb[mask])
scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker])
hbox.add(labeled_scene_widget(scene, label='pointcloud'))
geom = trimesh.voxel.multibox(occupied,
pitch=resolution,
colors=[1., 0, 0, 0.5])
scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker])
hbox.add(labeled_scene_widget(scene, label='occupied'))
geom = trimesh.voxel.multibox(empty,
pitch=resolution,
colors=[0.5, 0.5, 0.5, 0.5])
scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker])
hbox.add(labeled_scene_widget(scene, label='empty'))
gui.add(hbox)
pyglet.app.run()
def main():
models = morefusion.datasets.YCBVideoModels()
points = models.get_pcd(class_id=2)
quaternion_true = tf.random_quaternion()
quaternion_pred = quaternion_true + [0.1, 0, 0, 0]
transform_true = tf.quaternion_matrix(quaternion_true)
transform_pred = tf.quaternion_matrix(quaternion_pred)
scenes = {}
for use_translation in [False, True]:
if use_translation:
translation_true = np.random.uniform(-0.02, 0.02, (3,))
translation_pred = np.random.uniform(-0.02, 0.02, (3,))
transform_true[:3, 3] = translation_true
transform_pred[:3, 3] = translation_pred
add = morefusion.metrics.average_distance(
[points], [transform_true], [transform_pred]
)[0][0]
# ---------------------------------------------------------------------
scene = trimesh.Scene()
points_true = tf.transform_points(points, transform_true)
colors = np.full((points_true.shape[0], 3), [1.0, 0, 0])
geom = trimesh.PointCloud(vertices=points_true, color=colors)
scene.add_geometry(geom)
points_pred = tf.transform_points(points, transform_pred)
colors = np.full((points_true.shape[0], 3), [0, 0, 1.0])
geom = trimesh.PointCloud(vertices=points_pred, color=colors)
scene.add_geometry(geom)
scenes[f"use_translation: {use_translation}, add: {add}"] = scene
if scenes:
camera_transform = list(scenes.values())[0].camera_transform
scene.camera_transform = camera_transform
morefusion.extra.trimesh.display_scenes(scenes)
def visual_weights(bone_obj, bone_pts_weights, vis_label=True):
'''
visulize_weight
Parameters:
bone_obj: trimesh with N vertices / np.array [N, 3]
bone_pts_weights: np.array [N,k]
vis_label: argmax weight as color if true
Return:
colored bone obj (trimesh.mesh or trimesh.pointcloud)
'''
if vis_label:
label_list = np.argmax(bone_pts_weights, axis=1)
empty_weight_mask = np.max(bone_pts_weights, axis=1) == 0
bone_num = bone_pts_weights.shape[-1]
if isinstance(bone_obj, np.ndarray):
rgb = np.stack([
colorsys.hsv_to_rgb(i * 1.0 / bone_num, 0.8, 0.8)
for i in label_list
])
rgb[empty_weight_mask] = [0, 0, 0]
a = np.ones([rgb.shape[0], 1])
rgba = np.hstack([rgb, a])
bone_pts = trimesh.PointCloud(vertices=bone_obj.reshape(-1, 3),
colors=rgba)
return bone_pts
else:
if isinstance(bone_obj.visual, trimesh.visual.TextureVisuals):
bone_obj.visual = bone_obj.visual.to_color()
bone_obj.visual.vertex_colors = np.zeros(
[len(bone_obj.vertices), 4])
if vis_label:
for i in range(len(label_list)):
(r, g, b) = colorsys.hsv_to_rgb(label_list[i] * 1.0 / bone_num,
0.8, 0.8)
bone_obj.visual.vertex_colors[i] = (r * 255, g * 255, b * 255,
255)
else:
for i in range(bone_pts_weights.shape[0]):
weights = bone_pts_weights[i].reshape(-1)
color_map = np.arange(len(weights)) * 1.0 / bone_num
h = np.dot(weights, color_map)
(r, g, b) = colorsys.hsv_to_rgb(h, 0.8, 0.8)
bone_obj.visual.vertex_colors[i] = (r * 255, g * 255, b * 255,
255)
return bone_obj
def vis(coords, node_indices, row_splits, outer_row_splits):
import trimesh
depth = len(node_indices)
for i in range(depth):
scene = trimesh.scene.Scene()
p0 = trimesh.PointCloud(coords[i], color=(255, 255, 255))
p1 = trimesh.PointCloud(coords[i + 1], color=(0, 0, 255))
geometries = [p0, p1]
rs = row_splits[i]
ni = node_indices[i]
for j in outer_row_splits[i][:-1]:
geometries.append(
trimesh.primitives.Sphere(center=coords[i + 1][j],
radius=0.02,
color=(255, 255, 255)))
geometries.append(
trimesh.PointCloud(coords[i][ni[rs[j]:rs[j + 1]]],
color=(255, 0, 0)))
for g in geometries:
scene.add_geometry(g)
scene.show(background=(0, 0, 0))
def save_mesh(verts, savename, normals=None, faces=None, verts_color=None):
if faces is None:
if verts_color is None:
verts_color = np.zeros([verts.shape[0], 4])
verts_pts = trimesh.PointCloud(vertices=verts, colors=verts_color)
verts_pts.export(savename)
return verts_pts
else:
verts_mesh = trimesh.Trimesh(vertices=verts,
faces=faces,
normals=normals,
process=False)
verts_mesh.export(savename)
return verts_mesh
def vis_cloud(dataset: tf.data.Dataset,
augment_func: Optional[Callable] = None):
if augment_func is not None:
dataset = dataset.map(augment_func)
for coords, label in dataset:
if isinstance(coords, dict):
coords = coords["positions"]
coords = coords.numpy()
pc = trimesh.PointCloud(coords)
print(f"label = {label.numpy()}")
print(
f"max_r = {tf.reduce_max(tf.linalg.norm(coords, axis=-1)).numpy()}"
)
pc.show()
def get_scenes():
dataset = morefusion.datasets.YCBVideoDataset("train")
index = 0
video_id_prev = None
scene = trimesh.Scene()
while index < len(dataset):
example = dataset[index]
video_id, frame_id = dataset.ids[index].split("/")
clear = False
if video_id_prev is not None and video_id_prev != video_id:
clear = True
for node_name in scene.graph.nodes_geometry:
scene.graph.transforms.remove_node(node_name)
video_id_prev = video_id
rgb = example["color"]
depth = example["depth"]
K = example["meta"]["intrinsic_matrix"]
T_world2cam = np.r_[example["meta"]["rotation_translation_matrix"],
[[0, 0, 0, 1]]]
T_cam2world = np.linalg.inv(T_world2cam)
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])
scene.add_geometry(geom, transform=T_cam2world)
# A kind of current camera view, but a bit far away to see whole scene.
scene.camera.resolution = (rgb.shape[1], rgb.shape[0])
scene.camera.focal = (K[0, 0], K[1, 1])
scene.camera_transform = morefusion.extra.trimesh.to_opengl_transform(
T_cam2world @ tf.translation_matrix([0, 0, -0.5]))
# scene.set_camera()
geom = trimesh.creation.camera_marker(scene.camera,
marker_height=0.05)[1]
geom.colors = [(0, 1.0, 0)] * len(geom.entities)
scene.add_geometry(geom, transform=T_cam2world)
index += 15
print(f"[{index:08d}] video_id={video_id}")
yield {"__clear__": clear, "rgb": rgb, "scene": scene}
def vis_example(image, cloud):
import numpy as np
import matplotlib.pyplot as plt
import trimesh
if hasattr(image, 'numpy'):
image = image.numpy()
if hasattr(cloud, 'numpy'):
cloud = cloud.numpy()
image -= np.min(image)
image /= np.max(image)
plt.imshow(image)
plt.show()
colors = np.empty((cloud.shape[0], 4), dtype=np.uint8)
colors[:] = [0, 0, 255, 255]
trimesh.PointCloud(cloud, colors=colors).show()
def show_texture(texture, wt):
points = []
colors = []
for i in range(mesh.faces.shape[0]):
if wt[i] == 0:
continue
temp = texture[i] / wt[i]
A, B, C = vertices[i]
for u in range(reso):
for v in range(reso - u):
X = A + u * (B - A) + v * (C - A)
points.append(X)
colors.append(temp[u * reso + v])
pcd = trimesh.PointCloud(vertices=points, colors=colors)
print("Time taken:", time() - start)
pcd.scene().show()
def create_scene(graph, fov=(640, 480)):
"""
Visualise the beliefs of the camera poses and landmarks over
the GBP iterations.
"""
cam_params, landmarks = [], []
for cam in graph.cam_nodes:
cam_params.append(list(cam.mu))
for lmk in graph.lmk_nodes:
landmarks.append(list(lmk.mu))
K = graph.factors[0].args[0]
focal = [K[0, 0], K[1, 1]]
angle_fov = np.array([0., 0.])
angle_fov[0] = np.arctan(fov[0] / focal[0]) # in radians
angle_fov[1] = np.arctan(fov[1] / focal[1])
angle_fov_deg = angle_fov * (180 / np.pi) # in degrees
scene = trimesh.Scene()
for i, params in enumerate(cam_params):
Twc = np.linalg.inv(transformations.getT_axisangle(params))
cam_name = f'cam_{i}'
cam = trimesh.scene.Camera(fov=angle_fov_deg)
geom = trimesh.creation.camera_marker(cam, marker_height=0.1)
# geom[1].colors = [(0., 0., 1.)] * 16
scene.add_geometry(geom[1], transform=Twc, node_name=cam_name)
if i == 0:
# Set initial viewpoint behind this keyframe
cam_pose = Twc.copy()
cam_pose[:3, 3] -= 2.0 * cam_pose[:3, 2] / np.linalg.norm(cam_pose[:3, 2])
scene.camera.transform = to_opengl_transform(transform=cam_pose)
landmarks_pcd = trimesh.PointCloud(landmarks)
landmarks_pcd.colors = [[0., 0., 0.9]] * len(landmarks)
scene.add_geometry(landmarks_pcd, node_name='landmarks_pcd')
scene.camera.resolution = [1200, 900]
scene.camera.fov = 60 * (scene.camera.resolution / scene.camera.resolution.max())
return scene
def main(_):
import tensorflow as tf
tf.compat.v1.enable_eager_execution()
import tensorflow_datasets as tfds
FLAGS = flags.FLAGS
if FLAGS.version == "2":
from shape_tfds.shape.modelnet import Pointnet2, get_pointnet2_config
builder = Pointnet2(config=get_pointnet2_config(num_classes=40))
elif FLAGS.version == "2h":
from shape_tfds.shape.modelnet import Pointnet2H5
builder = Pointnet2H5()
elif FLAGS.version == "1":
from shape_tfds.shape.modelnet import Pointnet
builder = Pointnet()
else:
raise ValueError(
'Invalid choice of version {} - must be "1", "2", or "2h"'.format(
FLAGS.version))
download_config = tfds.core.download.DownloadConfig(
register_checksums=True)
# download_config = None
builder.download_and_prepare(download_config=download_config)
if FLAGS.vis:
import numpy as np
try:
import trimesh
except ImportError:
raise ImportError("visualizing requires trimesh")
for cloud, _ in builder.as_dataset(split="train", as_supervised=True):
pos = cloud["positions"].numpy()
print(np.min(pos, axis=0))
print(np.max(pos, axis=0))
print(np.max(np.linalg.norm(pos, axis=-1)))
trimesh.PointCloud(pos).show()
def show_texture(texture, wt, fileName):
points = []
colors = []
for i in range(mesh.elements[1].data.shape[0]):
A, B, C = Vertex[tuple(mesh.elements[1].data[i])]
for u in np.arange(0, 1, 1 / SAMPLE_SIZE):
for v in np.arange(0, 1 - u, 1 / SAMPLE_SIZE):
if wt[i][int(u * SAMPLE_SIZE * SAMPLE_SIZE +
v * SAMPLE_SIZE)] == 0:
continue
w = 1 - u - v
X = u * A + v * B + w * C
points.append(X)
color = texture[i][int(u * SAMPLE_SIZE * SAMPLE_SIZE +
v * SAMPLE_SIZE)] / wt[i][
int(u * SAMPLE_SIZE * SAMPLE_SIZE +
v * SAMPLE_SIZE)]
colors.append([int(color[2]), int(color[1]), int(color[0])])
pcd = trimesh.PointCloud(vertices=points, colors=colors)
pcd.export(fileName)
def show_cloud():
b_min = [-0.5, -1.0, 0.0]
b_max = [1.0, 1.0, 1.5]
pts_inside = esdf.debug_points(b_min, b_max)
if skrobot_found:
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
tpc = trimesh.PointCloud(pts_inside)
pclink = skrobot.model.primitives.PointCloudLink(tpc)
robot_model = skrobot.models.PR2()
#viewer.add(robot_model)
viewer.add(pclink)
viewer.show()
else:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
myscat = lambda X: ax.scatter(X[:, 0], X[:, 1], X[:, 2])
myscat(pts_inside)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
for pc_name in point_cloud_name_list:
if pc_name not in mesh_name_list:
continue
mesh_name = pc_name
print('Loading ' + pc_name)
# Read point cloud
v = o3d.io.read_point_cloud(
os.path.join(os.path.abspath(point_cloud_directory), pc_name) +
point_cloud_format)
pc = trimesh.PointCloud(np.asarray(v.points))
# Load mesh
mesh = trimesh.load_mesh(
os.path.join(os.path.abspath(mesh_directory), mesh_name) +
mesh_format)
# Change mesh vertices and faces color and set transparency
mesh.visual.face_colors[:] = np.array([255, 50, 50, 80])
mesh.visual.vertex_colors[:] = np.array([255, 50, 50, 80])
# Rotate mesh and pc of a fixed amount
transform = trimesh.transformations.rotation_matrix(
def brute_force_closest_vertex_to_joints(self):
scenes = self.get_valid_scenes()
# calculate 50 nearest vertex ids for all meshes and joints
closest_k = 50
candidates = {}
keep_searching = True
while keep_searching:
for key, scene in scenes.items():
joint = np.squeeze(scene['joint'].vertices)
distances, vertex_ids = scene['mesh'].kdtree.query(
joint, closest_k)
candidates[key] = {
vertex_id: dist
for vertex_id, dist in zip(vertex_ids, distances)
}
# only keep common ids
from functools import reduce
common_ids = reduce(np.intersect1d,
[list(c.keys()) for c in candidates.values()])
if len(common_ids) <= self.settings_loader.min_vertices:
closest_k += 10
continue
else:
keep_searching = False
# calculate average distance per mesh/joint for valid ids
mean_dist = [
np.mean([c[common_id] for c in candidates.values()])
for common_id in common_ids
]
mean_dist = {
common_id: dist
for common_id, dist in zip(common_ids, mean_dist)
}
mean_dist = {
k: v
for k, v in sorted(mean_dist.items(), key=lambda item: item[1])
}
# pick closest vertex with min average distance to all joints per mesh
closest_id = list(mean_dist)[0]
final_vertices = [closest_id]
mean_dist.pop(closest_id)
while len(final_vertices) < self.settings_loader.min_vertices:
# calculate all distance combinations between valid vertices
vertex_ids = list(mean_dist)
id_dist = [
sp.distance.cdist(s['mesh'].vertices[final_vertices],
s['mesh'].vertices[vertex_ids])
for s in scenes.values()
]
id_dist = np.mean(id_dist, axis=0)
# min the ratio between distances to joint and distance to all other vertices
best_dist = list(mean_dist.values()) / id_dist
best_id = np.argmin(best_dist)
# max the difference between distance to all other vertices and distances to joint
# best_dist = id_dist - list(mean_dist.values())
# best_id = np.argmax(best_dist)
n, m = np.unravel_index(best_id, best_dist.shape)
best_id = vertex_ids[m]
final_vertices.append(best_id)
mean_dist.pop(best_id)
vertices, joints = [], []
for scene in scenes.values():
verts = np.asarray(scene['mesh'].vertices).reshape([-1, 3])
verts = verts[final_vertices]
vertices.append(verts)
joint = np.asarray(scene['joint'].vertices).reshape([-1, 3])
joints.append(joint)
vertex_weight = np.zeros([
6890,
])
vertices = np.stack(vertices).transpose([0, 2, 1]).reshape(
[-1, len(final_vertices)])
joints = np.stack(joints).transpose([0, 2, 1]).reshape([-1])
# constraint weights to sum up to 1
vertices = np.concatenate(
[vertices, np.ones([1, vertices.shape[1]])], 0)
joints = np.concatenate([joints, np.ones(1)])
# solve with non negative least squares
weights = so.nnls(vertices, joints)[0]
vertex_weight[final_vertices] = weights
file = join('regressors',
'regressor_{}.npy'.format(self.regressor_name.text()))
with open(file, 'wb') as f:
vertex_weight = vertex_weight.astype(np.float32)
vertex_weight = np.expand_dims(vertex_weight, -1)
np.save(f, vertex_weight)
widget = QMessageBox(
icon=QMessageBox.Information,
text=
'Regressor file successfully saved to: {}\n\nClick Reset to start again'
.format(file),
buttons=[QMessageBox.Ok])
widget.exec_()
vertex_weight = np.squeeze(vertex_weight)
self.convert_button.setEnabled(False)
self.regressor_name.setEnabled(False)
for scene in self.scene_chache.values():
mesh = scene.geometry['geometry_0']
mesh.visual.vertex_colors = [200, 200, 200, 255]
mesh.visual.vertex_colors[final_vertices] = [0, 255, 0, 255]
x = np.matmul(vertex_weight, mesh.vertices[:, 0])
y = np.matmul(vertex_weight, mesh.vertices[:, 1])
z = np.matmul(vertex_weight, mesh.vertices[:, 2])
joints = np.vstack((x, y, z)).T
joints = trimesh.PointCloud(joints, colors=[0, 255, 0, 255])
scene.add_geometry(joints, geom_name='new_joints')
def point_cloud(vertices, color=None):
colors = None if color is None else broadcast_colors(
color, vertices.shape[0])
return trimesh.PointCloud(vertices, colors=colors)
tree = KDTree(coords, leaf_size=leaf_size)
sample_result, query_result = tree.rejection_ifp_sample_query(
r2, r2, tree.get_node_indices(), out_size, k_query
)
# sample_result, query_result = tree.ifp_sample_query(r2, tree.get_node_indices(),
# out_size, k_query)
print(query_result.dists[0, : query_result.counts[0]])
red = [[255, 0, 0]]
green = [[0, 255, 0]]
blue = [[0, 0, 255]]
sampled_coords = coords[sample_result.indices]
scene = trimesh.scene.Scene()
pc = trimesh.PointCloud(coords, colors=np.full((N, 3), 255, dtype=np.uint8))
scene = pc.scene()
scene.add_geometry(
trimesh.PointCloud(sampled_coords, colors=np.tile(blue, (out_size, 1)))
)
rl = query_result.counts[0]
scene.add_geometry(
trimesh.PointCloud(
coords[query_result.indices[0, :rl]], colors=np.tile(green, (rl, 1))
)
)
scene.add_geometry(trimesh.PointCloud(sampled_coords[:1], colors=red))
# scene.add_geometry(
# trimesh.primitives.Sphere(radius=radius, center=sampled_coords[0]))
# scene.add_geometry(trimesh.primitives.Sphere(radius=1.1, center=[0, 0, 0]))
scene.show(background=np.zeros((3,), dtype=np.uint8))
def _load_obj_impl(file_obj,
raw_mesh=False,
resolver=None,
split_object=False,
group_material=True):
"""
Load a Wavefront OBJ file into kwargs for a trimesh.Scene
object.
Parameters
--------------
file : file like object
Contains OBJ data
raw_mesh : bool
Only load the basic geometry data, e.g. vertices, faces. Ignore
materials, uv, etc. Besides, the vertices and faces will keep
consistent with the raw data.
resolver : trimesh.visual.resolvers.Resolver
Allow assets such as referenced textures and
material files to be loaded
split_object : bool
Split meshes at each `o` declared in file
group_material : bool
Group faces that share the same material
into the same mesh.
Returns
-------------
geometry : dict
Loaded geometry parts of obj file. Each part has a name,
which corresponds to a submesh
"""
# if we still have no resolver try using file_obj name
if resolver is None and hasattr(file_obj,
'name') and len(file_obj.name) > 0:
resolver = FilePathResolver(file_obj.name)
# get text as bytes or string blob
text = file_obj.read()
# if text was bytes decode into string
text = util.decode_text(text)
# add leading and trailing newlines so we can use the
# same logic even if they jump directly in to data lines
text = '\n{}\n'.format(text.strip().replace('\r\n', '\n'))
# Load Materials
materials = {}
if not raw_mesh:
mtl_position = text.find('mtllib')
if mtl_position >= 0:
# take the line of the material file after `mtllib`
# which should be the file location of the .mtl file
mtl_path = text[mtl_position +
6:text.find('\n', mtl_position)].strip()
try:
# use the resolver to get the data
material_kwargs = parse_mtl(resolver[mtl_path],
resolver=resolver)
# turn parsed kwargs into material objects
materials = {
k: SimpleMaterial(**v)
for k, v in material_kwargs.items()
}
except IOError:
# usually the resolver couldn't find the asset
log.warning(
'unable to load materials from: {}'.format(mtl_path))
except BaseException:
# something else happened so log a warning
log.warning(
'unable to load materials from: {}'.format(mtl_path),
exc_info=True)
# extract vertices from raw text
v, vn, vt, vc = _parse_vertices(text=text)
# get relevant chunks that have face data
# in the form of (material, object, chunk)
face_tuples = _preprocess_faces(text=text, split_object=split_object)
# combine chunks that have the same material
# some meshes end up with a LOT of components
# and will be much slower if you don't do this
if group_material:
face_tuples = _group_by_material(face_tuples)
# Load Faces
# now we have clean- ish faces grouped by material and object
# so now we have to turn them into numpy arrays and kwargs
# for trimesh mesh and scene objects
geometry = {}
while len(face_tuples) > 0:
# consume the next chunk of text
material, current_object, chunk = face_tuples.pop()
# do wangling in string form
# we need to only take the face line before a newline
# using builtin functions in a list comprehension
# is pretty fast relative to other options
# this operation is the only one that is O(len(faces))
# slower due to the tight-loop conditional:
# face_lines = [i[:i.find('\n')]
# for i in chunk.split('\nf ')[1:]
# if i.rfind('\n') >0]
# maxsplit=1 means that it can stop working
# after it finds the first newline
# passed as arg as it's not a kwarg in python2
face_lines = [i.split('\n', 1)[0] for i in chunk.split('\nf ')[1:]]
# then we are going to replace all slashes with spaces
joined = ' '.join(face_lines).replace('/', ' ')
# the fastest way to get to a numpy array
# processes the whole string at once into a 1D array
# also wavefront is 1-indexed (vs 0-indexed) so offset
array = np.fromstring(joined, sep=' ', dtype=np.int64) - 1
# get the number of raw 2D columns in a sample line
columns = len(face_lines[0].strip().replace('/', ' ').split())
# make sure we have the right number of values for vectorized
if len(array) == (columns * len(face_lines)):
# everything is a nice 2D array
faces, faces_tex, faces_norm = _parse_faces_vectorized(
array=array, columns=columns, sample_line=face_lines[0])
else:
# if we had something annoying like mixed in quads
# or faces that differ per-line we have to loop
# i.e. something like:
# '31407 31406 31408',
# '32303/2469 32304/2469 32305/2469',
log.warning('faces have mixed data, using slow fallback!')
faces, faces_tex, faces_norm = _parse_faces_fallback(face_lines)
# TODO: name usually falls back to something useless
name = current_object
if name is None or len(name) == 0 or name in geometry:
name = '{}_{}'.format(name, util.unique_id())
# try to get usable texture
mesh = {'faces': faces, 'vertices': v}
if not raw_mesh:
if faces_tex is not None:
# convert faces referencing vertices and
# faces referencing vertex texture to new faces
# where each face
if faces_norm is not None and len(faces_norm) == len(faces):
new_faces, mask_v, mask_vt, mask_vn = unmerge_faces(
faces, faces_tex, faces_norm)
else:
mask_vn = None
new_faces, mask_v, mask_vt = unmerge_faces(
faces, faces_tex)
if tol.strict:
# we should NOT have messed up the faces
# note: this is EXTREMELY slow due to all the
# float comparisons so only run this in unit tests
assert np.allclose(v[faces], v[mask_v][new_faces])
# faces should all be in bounds of vertives
assert new_faces.max() < len(v[mask_v])
try:
# survive index errors as sometimes we
# want materials without UV coordinates
uv = vt[mask_vt]
except BaseException as E:
uv = None
raise E
# mask vertices and use new faces
mesh.update({'vertices': v[mask_v].copy(), 'faces': new_faces})
else:
# otherwise just use unmasked vertices
uv = None
# check to make sure indexes are in bounds
if tol.strict:
assert faces.max() < len(v)
if vn is not None and np.shape(faces_norm) == faces.shape:
# do the crazy unmerging logic for split indices
new_faces, mask_v, mask_vn = unmerge_faces(
faces, faces_norm)
else:
# generate the mask so we only include
# referenced vertices in every new mesh
mask_v = np.zeros(len(v), dtype=np.bool)
mask_v[faces] = True
# reconstruct the faces with the new vertex indices
inverse = np.zeros(len(v), dtype=np.int64)
inverse[mask_v] = np.arange(mask_v.sum())
new_faces = inverse[faces]
# no normals
mask_vn = None
# start with vertices and faces
mesh.update({'faces': new_faces, 'vertices': v[mask_v].copy()})
# if vertex colors are OK save them
if vc is not None:
mesh['vertex_colors'] = vc[mask_v]
# if vertex normals are OK save them
if mask_vn is not None:
mesh['vertex_normals'] = vn[mask_vn]
if not raw_mesh:
visual = None
if material in materials:
visual = TextureVisuals(uv=uv, material=materials[material])
elif material is not None:
# material will be None by default
log.warning(
'specified material ({}) not loaded!'.format(material))
mesh['visual'] = visual
# process will be passed to constructor of Trimesh
# which specifies whether remove duplicates and unreferences
process = not raw_mesh
# store geometry by name
if (isinstance(mesh['vertices'], dict)
or isinstance(mesh['faces'], dict)):
geometry[name] = trimesh.Trimesh(**misc.load_dict(mesh),
process=process)
elif mesh['faces'] is None:
# vertices without faces returns a PointCloud
geometry[name] = trimesh.PointCloud(**mesh)
else:
geometry[name] = trimesh.Trimesh(**mesh, process=process)
return geometry
dataset = 'ShapeNet'
category = 'airplane'
ckpt_model = 'airplane_10b'
root_data = '/disk1/yicheng/' + dataset + '/test_data_npy/' + category + '/'
root_results = '/disk1/yicheng/' + dataset + '/results/' + ckpt_model + '/'
list_el = glob.glob(os.path.join(root_results, '*.mat'))
print(len(list_el))
for i, name in enumerate(list_el):
# read input model
input_id = Path(name).stem
pc = np.load(os.path.join(root_data, '%s.npy' % input_id))
if pc.shape[1] > 3:
pc = pc[:, :3]
pc_tri = trimesh.PointCloud(vertices=pc)
# get prediction
params_pred = sio.loadmat(name)
keypoints = get_kpts_from_non_rigid_modelling(params_pred)
keypoints_tri = trimesh.PointCloud(vertices=keypoints)
# visualize
scene = pc_tri.scene()
for kp in keypoints:
sphere = trimesh.primitives.Sphere(center=kp, radius=0.02)
sphere.visual.face_colors = [255., 0., 0., 255.]
scene.add_geometry(sphere)
scene.show()
def get_counts(tree, radius):
counts = np.zeros((positions.shape[0], ), dtype=np.int64)
for i, j in tree.query_pairs(radius):
counts[i] += 1
counts[j] += 1
return counts
for cloud, label in tqdm.tqdm(tfds.as_numpy(dataset),
total=problem.examples_per_epoch(split)):
positions = cloud['positions']
print(positions.shape)
print(np.max(np.sum(positions**2, axis=-1), axis=0))
tree = cKDTree(positions)
counts = get_counts(tree, 0.1)
mean = np.mean(counts)
means.append(mean)
if mean > 15:
print('{}: {}, {}'.format(label, class_names[label], mean))
# c2 = np.mean(get_counts(tree, 0.2))
# c4 = np.mean(get_counts(tree, 0.4))
# print(c2, c4)
trimesh.PointCloud(positions).show()
import matplotlib.pyplot as plt
plt.hist(means)
plt.show()
import numpy as np
import trimesh
from trimesh.transformations import transform_points
colors = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]]
vertices = colors
scene = trimesh.scene.Scene()
for v, c in zip(vertices, colors):
sphere = trimesh.primitives.Sphere(center=v, radius=0.1)
# add point clouds - primitives don't render well
vertex_colors = np.tile(
np.expand_dims(c, axis=0) * 255, (sphere.vertices.shape[0], 1))
pc = trimesh.PointCloud(sphere.vertices, colors=vertex_colors)
scene.add_geometry(pc)
scene.camera._resolution = (640, 480)
print('# # camera resoltuion \n', scene.camera._resolution)
camera = scene.camera
transform = camera.transform
print("\n# # Camera extrinsic")
print(transform)
K = camera.K
print("\n# # Camera intrinsic")
print(K)
scene.show()
# render scene
from io import BytesIO
img = scene.save_image(resolution=(640, 480))
from PIL import Image
rendered = Image.open(BytesIO(img)).convert("RGB")
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--input-dir', '-i', type=str,
help='input urdf',
default='/media/kosuke55/SANDISK-2/meshdata/ycb_eval/019_pitcher_base/pocky-2020-10-17-06-01-16-481902-45682') # noqa
parser.add_argument(
'--idx', type=int,
help='data idx',
default=0)
parser.add_argument(
'--large-axis', '-la', action='store_true',
help='use large axis as visualizing marker')
args = parser.parse_args()
base_dir = args.input_dir
start_idx = args.idx
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
for idx in range(start_idx, 100000):
print(idx)
if idx != start_idx:
viewer.delete(pc) # noqa
for c in contact_point_marker_list: # noqa
viewer.delete(c)
annotation_path = osp.join(
base_dir, 'annotation', '{:06}.json'.format(idx))
annotation_data = load_json(annotation_path)
color_path = osp.join(base_dir, 'color', '{:06}.png'.format(idx))
color = o3d.io.read_image(color_path)
depth_path = osp.join(base_dir, 'depth', '{:06}.npy'.format(idx))
depth = np.load(depth_path)
depth = o3d.geometry.Image(depth)
camera_info_path = osp.join(
base_dir, 'camera_info', '{:06}.yaml'.format(idx))
cameramodel = cameramodels.PinholeCameraModel.from_yaml_file(
camera_info_path)
intrinsics = cameramodel.open3d_intrinsic
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd, intrinsics)
contact_point_marker_list = []
# for manual annotaion which have labels
if 'label' in annotation_data[0]:
labels = [a['label'] for a in annotation_data]
color_map = label_colormap(max(labels) + 1)
for annotation in annotation_data:
cx = annotation['xy'][0]
cy = annotation['xy'][1]
q = np.array(annotation['quaternion'])
dep = annotation['depth']
color = [255, 0, 0]
if 'label' in annotation:
label = annotation['label']
color = color_map[label]
print(cx, cy)
pos = np.array(
cameramodel.project_pixel_to_3d_ray([cx, cy]))
length = dep * 0.001 / pos[2]
pos = pos * length
if args.large_axis:
contact_point_marker = skrobot.model.Axis(0.003, 0.05)
else:
contact_point_marker = skrobot.model.Sphere(0.003, color=color)
contact_point_marker.newcoords(
skrobot.coordinates.Coordinates(pos=pos, rot=q))
viewer.add(contact_point_marker)
contact_point_marker_list.append(contact_point_marker)
trimesh_pc = trimesh.PointCloud(
np.asarray(
pcd.points), np.asarray(
pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
viewer.add(pc)
if idx == start_idx:
viewer.show()
input('Next data?: [ENTER]')
pcd_path = pcd_paths[0]
for pcd_path in pcd_paths:
pcd = o3d.io.read_point_cloud(str(pcd_path))
paths = list(sorted(Path('./') .glob('*.json')))
pose_path = pcd_path.with_suffix('.json')
contact_points_dict = load_json(str(pose_path))
contact_points = contact_points_dict['contact_points']
contact_point_sphere_list = []
for i, cp in enumerate(contact_points):
contact_point_sphere = skrobot.model.Axis(0.003, 0.05)
contact_point_sphere.newcoords(
skrobot.coordinates.Coordinates(pos=cp[0], rot=cp[1:]))
contact_point_sphere_list.append(contact_point_sphere)
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 640))
trimesh_pc = trimesh.PointCloud(
np.asarray(pcd.points), np.asarray(pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
viewer.add(pc)
for contact_point_sphere in contact_point_sphere_list:
viewer.add(contact_point_sphere)
print('Press [q] on the window to move the next object.\n'
+ 'Press [ctrl + c] on the command line to finish.')
viewer._init_and_start_app()
scenes = {}
for is_solid in ["nonsolid", "solid"]:
name = f"{is_solid}"
if is_solid == "nonsolid":
points = models.get_pcd(class_id=2)
else:
assert is_solid == "solid"
points = models.get_solid_voxel_grid(class_id=2).points
points = morefusion.extra.open3d.voxel_down_sample(points, voxel_size=0.01)
points = points.astype(np.float32)
geom = trimesh.PointCloud(vertices=points)
scene.add_geometry(geom)
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,
def main():
current_dir = osp.dirname(osp.abspath(__file__))
trained_model_dir = osp.join(osp.dirname(osp.dirname(current_dir)),
'pretrained_model')
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--input-dir',
'-i',
type=str,
help='input directory',
default=None)
parser.add_argument('--color',
'-c',
type=str,
help='color image (.png)',
default=None)
parser.add_argument('--depth',
'-d',
type=str,
help='depth image (.npy)',
default=None)
parser.add_argument('--camera-info',
'-ci',
type=str,
help='camera info file (.yaml)',
default=None)
parser.add_argument('--pretrained_model',
'-p',
type=str,
help='Pretrained models',
default=osp.join(trained_model_dir, 'hanging_1020.pt'))
# default=osp.join(trained_model_dir, 'pouring.pt'))
# default='/media/kosuke55/SANDISK-2/meshdata/random_shape_shapenet_hanging_render/1010/gan_2000per0-1000obj_1020.pt') # gan hanging # noqa
# default='/media/kosuke55/SANDISK-2/meshdata/random_shape_shapenet_pouring_render/1227/pouring_random_20201230_0215_5epoch.pt') # gan pouring # noqa
parser.add_argument('--predict-depth',
'-pd',
type=int,
help='predict-depth',
default=0)
parser.add_argument('--task',
'-t',
type=str,
help='h(hanging) or p(pouring)'
'Not needed if roi size is the same in config.',
default='h')
args = parser.parse_args()
base_dir = args.input_dir
pretrained_model = args.pretrained_model
config_path = str(
Path(osp.abspath(__file__)).parent.parent / 'learning_scripts' /
'config' / 'gray_model.yaml')
with open(config_path) as f:
config = yaml.safe_load(f)
task_type = args.task
if task_type == 'p':
task_type = 'pouring'
else:
task_type = 'hanging'
target_size = tuple(config['target_size'])
depth_range = config['depth_range']
depth_roi_size = config['depth_roi_size'][task_type]
print('task type: {}'.format(task_type))
print('depth roi size: {}'.format(depth_roi_size))
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
model = HPNET(config).to(device)
model.load_state_dict(torch.load(pretrained_model), strict=False)
model.eval()
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
if base_dir is not None:
color_paths = list(Path(base_dir).glob('**/color/*.png'))
elif args.color is not None \
and args.depth is not None \
and args.camera_info is not None:
color_paths = [args.color]
else:
return False
is_first_loop = True
try:
for color_path in color_paths:
if not is_first_loop:
viewer.delete(pc) # noqa
for c in contact_point_sphere_list: # noqa
viewer.delete(c)
if base_dir is not None:
camera_info_path = str(
(color_path.parent.parent / 'camera_info' /
color_path.stem).with_suffix('.yaml'))
depth_path = str((color_path.parent.parent / 'depth' /
color_path.stem).with_suffix('.npy'))
color_path = str(color_path)
else:
camera_info_path = args.camera_info
color_path = args.color
depth_path = args.depth
camera_model = cameramodels.PinholeCameraModel.from_yaml_file(
camera_info_path)
camera_model.target_size = target_size
cv_bgr = cv2.imread(color_path)
intrinsics = camera_model.open3d_intrinsic
cv_bgr = cv2.resize(cv_bgr, target_size)
cv_rgb = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2RGB)
color = o3d.geometry.Image(cv_rgb)
cv_depth = np.load(depth_path)
cv_depth = cv2.resize(cv_depth,
target_size,
interpolation=cv2.INTER_NEAREST)
depth = o3d.geometry.Image(cv_depth)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd, intrinsics)
trimesh_pc = trimesh.PointCloud(np.asarray(pcd.points),
np.asarray(pcd.colors))
pc = skrobot.model.PointCloudLink(trimesh_pc)
viewer.add(pc)
if config['use_bgr2gray']:
gray = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2GRAY)
gray = cv2.resize(gray, target_size)[..., None] / 255.
normalized_depth = normalize_depth(cv_depth, depth_range[0],
depth_range[1])[..., None]
in_feature = np.concatenate((normalized_depth, gray),
axis=2).astype(np.float32)
else:
raise NotImplementedError()
transform = transforms.Compose([transforms.ToTensor()])
in_feature = transform(in_feature)
in_feature = in_feature.to(device)
in_feature = in_feature.unsqueeze(0)
confidence, depth, rotation = model(in_feature)
confidence = confidence[0, 0:1, ...]
confidence_np = confidence.cpu().detach().numpy().copy() * 255
confidence_np = confidence_np.transpose(1, 2, 0)
confidence_np[confidence_np <= 0] = 0
confidence_np[confidence_np >= 255] = 255
confidence_img = confidence_np.astype(np.uint8)
print(model.rois_list)
contact_point_sphere_list = []
roi_image = cv_bgr.copy()
for i, (roi, roi_center) in enumerate(
zip(model.rois_list[0], model.rois_center_list[0])):
if roi.tolist() == [0, 0, 0, 0]:
continue
roi = roi.cpu().detach().numpy().copy()
roi_image = draw_roi(roi_image, roi)
hanging_point_x = roi_center[0]
hanging_point_y = roi_center[1]
v = rotation[i].cpu().detach().numpy()
v /= np.linalg.norm(v)
rot = rotation_matrix_from_axis(v, [0, 1, 0], 'xy')
q = matrix2quaternion(rot)
hanging_point = np.array(
camera_model.project_pixel_to_3d_ray(
[int(hanging_point_x),
int(hanging_point_y)]))
if args.predict_depth:
dep = depth[i].cpu().detach().numpy().copy()
dep = unnormalize_depth(dep, depth_range[0],
depth_range[1]) * 0.001
length = float(dep) / hanging_point[2]
else:
depth_roi = make_box(roi_center,
width=depth_roi_size[1],
height=depth_roi_size[0],
img_shape=target_size,
xywh=False)
depth_roi_clip = cv_depth[depth_roi[0]:depth_roi[2],
depth_roi[1]:depth_roi[3]]
dep_roi_clip = depth_roi_clip[np.where(
np.logical_and(
config['depth_range'][0] < depth_roi_clip,
depth_roi_clip < config['depth_range'][1]))]
dep_roi_clip = np.median(dep_roi_clip) * 0.001
if dep_roi_clip == np.nan:
continue
length = float(dep_roi_clip) / hanging_point[2]
hanging_point *= length
contact_point_sphere = skrobot.model.Sphere(0.001,
color=[255, 0, 0])
contact_point_sphere.newcoords(
skrobot.coordinates.Coordinates(pos=hanging_point, rot=q))
viewer.add(contact_point_sphere)
contact_point_sphere_list.append(contact_point_sphere)
if is_first_loop:
viewer.show()
heatmap = overlay_heatmap(cv_bgr, confidence_img)
cv2.imshow('heatmap', heatmap)
cv2.imshow('roi', roi_image)
print('Next data: [ENTER] on image window.\n'
'Quit: [q] on image window.')
key = cv2.waitKey(0)
cv2.destroyAllWindows()
if key == ord('q'):
break
is_first_loop = False
except KeyboardInterrupt:
pass
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 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
