def visualize_pc(pc, mask=None):
'''
pc - 3 x n
mask - 1 x n, [0,1]
'''
pc = pc.permute(1, 0) # n x 3
pc = pc.cpu().detach().numpy()
pcd = geometry.PointCloud()
pcd.points = utility.Vector3dVector(pc)
if mask is not None:
mask = mask / mask.max()
#mask=mask.pow(0.5) #dilate the values
mask = mask.expand(3, -1).permute(1, 0) #nx3
red = mask.new(mask.shape).zero_()
red[:, 0] += 1.0
green = mask.new(mask.shape).zero_()
green[:, 1] += 1.0
blue = mask.new(mask.shape).zero_()
blue[:, 2] += 1.0
less = ((2 * mask).mul(green) + (1.0 - (2 * mask)).mul(blue)).mul(
mask.lt(0.5).float())
more = (((mask - 0.5) * 2).mul(red) +
(1.0 - ((mask - 0.5) * 2)).mul(green)).mul(
mask.ge(0.5).float())
mask = less + more
mask_max, _ = mask.max(dim=1, keepdim=True)
mask = mask.div(mask_max)
mask = mask.cpu().detach().numpy()
pcd.colors = utility.Vector3dVector(mask)
visualization.draw_geometries([pcd])
def get_o3d_meshes(self, hand_contact=False, normalize_pos=False):
"""Returns Open3D meshes for visualization
Draw with: o3dv.draw_geometries([hand_mesh, obj_mesh])"""
hand_color = np.asarray([224.0, 172.0, 105.0]) / 255
obj_color = np.asarray([100.0, 100.0, 100.0]) / 255
obj_centroid = self.obj_verts.mean(0)
if not normalize_pos:
obj_centroid *= 0
hand_mesh = o3dg.TriangleMesh()
hand_mesh.vertices = o3du.Vector3dVector(self.hand_verts -
obj_centroid)
hand_mesh.triangles = o3du.Vector3iVector(HandObject.closed_faces)
hand_mesh.compute_vertex_normals()
if hand_contact and self.hand_contact.mean() != 0:
util.mesh_set_color(self.hand_contact, hand_mesh)
else:
hand_mesh.paint_uniform_color(hand_color)
obj_mesh = o3dg.TriangleMesh()
obj_mesh.vertices = o3du.Vector3dVector(self.obj_verts - obj_centroid)
obj_mesh.triangles = o3du.Vector3iVector(self.obj_faces)
obj_mesh.compute_vertex_normals()
if self.obj_contact.mean() != 0:
util.mesh_set_color(self.obj_contact, obj_mesh)
else:
obj_mesh.paint_uniform_color(obj_color)
return hand_mesh, obj_mesh
def runCPDRegistration(self, sourceLM, sourceSLM, targetSLM, parameters):
from open3d import geometry
from open3d import utility
sourceArrayCombined = np.append(sourceSLM, sourceLM, axis=0)
targetArray = np.asarray(targetSLM)
#Convert to pointcloud for scaling
sourceCloud = geometry.PointCloud()
sourceCloud.points = utility.Vector3dVector(sourceArrayCombined)
targetCloud = geometry.PointCloud()
targetCloud.points = utility.Vector3dVector(targetArray)
cloudSize = np.max(targetCloud.get_max_bound() -
targetCloud.get_min_bound())
targetCloud.scale(25 / cloudSize, center=False)
sourceCloud.scale(25 / cloudSize, center=False)
#Convert back to numpy for cpd
sourceArrayCombined = np.asarray(sourceCloud.points, dtype=np.float32)
targetArray = np.asarray(targetCloud.points, dtype=np.float32)
registrationOutput = self.cpd_registration(targetArray,
sourceArrayCombined,
parameters["CPDIterations"],
parameters["CPDTolerence"],
parameters["alpha"],
parameters["beta"])
deformed_array, _ = registrationOutput.register()
#Capture output landmarks from source pointcloud
fiducial_prediction = deformed_array[-len(sourceLM):]
fiducialCloud = geometry.PointCloud()
fiducialCloud.points = utility.Vector3dVector(fiducial_prediction)
fiducialCloud.scale(cloudSize / 25, center=False)
return np.asarray(fiducialCloud.points)
def estimate_3Dpose(self):
sample_pcd = self.sample_pcd
XYZ_model = self.XYZ_model
diameter_model = self.diameter_model
model_pcd = self.model_pcd
numpy_point_data = np.asarray(sample_pcd.points)
print(numpy_point_data.shape)
XYZ_min = np.min(numpy_point_data, axis=0)
XYZ_max = np.max(numpy_point_data, axis=0)
print('minimum and maximum: ', XYZ_min, XYZ_max)
diameter_sample = np.sqrt(
np.square(XYZ_max[0] - XYZ_min[0]) +
np.square(XYZ_max[1] - XYZ_min[1]) +
np.square(XYZ_max[2] - XYZ_min[2]))
numpy_point_data = np.reshape(numpy_point_data, (-1, 3))
sample_pcd.points = utility.Vector3dVector(numpy_point_data)
# sample_pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # Flip it, otherwise the
# pointcloud will be upside down
sample_pcd.paint_uniform_color([0, 1, 0])
ratio = diameter_sample / diameter_model # Using diameter from pointcloud
print('Ratio = mushroom_diameter/mushroom_diameter_model: ', ratio)
XYZ_model_new = XYZ_model * ratio # Resize the model points to match that of a sample
model_pcd.points = utility.Vector3dVector(XYZ_model_new)
# model_pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) # Flip it, otherwise the
# pointcloud will be upside down
model_pcd.paint_uniform_color([1, 0, 0])
visualization.draw_geometries([model_pcd + sample_pcd])
rotation_matrix_estimated, quaternion_estimated = self.combined_registration(
model_pcd, sample_pcd)
print('rotation_matrix_estimated and rotation_vector_estimated: ',
rotation_matrix_estimated)
model_pcd.paint_uniform_color([1, 0, 0])
sample_pcd.paint_uniform_color([0, 1, 0])
visualization.draw_geometries([model_pcd + sample_pcd])
def loadAndScaleFiducials(self, fiducialPath, scaling):
from open3d import geometry
from open3d import utility
sourceLandmarkNode = slicer.util.loadMarkups(fiducialPath)
self.RAS2LPSTransform(sourceLandmarkNode)
point = [0, 0, 0]
sourceLandmarks_np = np.zeros(
shape=(sourceLandmarkNode.GetNumberOfFiducials(), 3))
for i in range(sourceLandmarkNode.GetNumberOfFiducials()):
sourceLandmarkNode.GetMarkupPoint(0, i, point)
sourceLandmarks_np[i, :] = point
slicer.mrmlScene.RemoveNode(sourceLandmarkNode)
cloud = geometry.PointCloud()
cloud.points = utility.Vector3dVector(sourceLandmarks_np)
cloud.scale(scaling, center=False)
fiducialVTK = self.convertPointsToVTK(cloud.points)
return fiducialVTK
def apply_semantic_colormap_to_mesh(mesh,
semantic_idx,
sigmoid_a=0.05,
invert=False):
colors = np.asarray(mesh.vertex_colors)[:, 0]
colors = mutils.texture_proc(colors, a=sigmoid_a, invert=invert)
# apply different colormaps based on finger
mesh_colors = np.zeros((len(colors), 3))
cmaps = ['Greys', 'Purples', 'Oranges', 'Greens', 'Blues', 'Reds']
cmaps = [plt.cm.get_cmap(c) for c in cmaps]
for semantic_id in np.unique(semantic_idx):
if (len(cmaps) <= semantic_id):
print('Not enough colormaps, ignoring semantic id {:d}'.format(
semantic_id))
continue
idx = semantic_idx == semantic_id
mesh_colors[idx] = cmaps[semantic_id](colors[idx])[:, :3]
mesh.vertex_colors = o3du.Vector3dVector(mesh_colors)
return mesh
def mesh_set_color(color, mesh, colormap=plt.cm.inferno):
"""
Applies colormap to object
:param color: Tensor or numpy array, (N, 1)
:param mesh: Open3D TriangleMesh
:return:
"""
# vertex_colors = np.tile(color.squeeze(), (3, 1)).T
# mesh.vertex_colors = o3du.Vector3dVector(vertex_colors)
# geometry.apply_colormap(mesh, apply_sigmoid=False)
colors = np.asarray(color).squeeze()
if len(colors.shape) > 1:
colors = colors[:, 0]
colors[colors < 0.1] = 0.1 # TODO hack to make brighter
colors = colormap(colors)[:, :3]
colors = o3du.Vector3dVector(colors)
mesh.vertex_colors = colors
def show_contactmap(p_num,
intent,
object_name,
mode='simple',
joint_sphere_radius_mm=4.0,
bone_cylinder_radius_mm=2.5,
bone_color=np.asarray([224.0, 172.0, 105.0]) / 255):
"""
mode =
simple: just contact map
simple_hands: skeleton + contact map
semantic_hands_fingers: skeleton + contact map colored by finger proximity
semantic_hands_phalanges: skeleton + contact map colored by phalange proximity
"""
cp = ContactPose(p_num, intent, object_name)
# read contactmap
mesh = o3dio.read_triangle_mesh(cp.contactmap_filename)
mesh.compute_vertex_normals()
geoms = []
# apply simple colormap to the mesh
if 'simple' in mode:
mesh = apply_colormap_to_mesh(mesh)
geoms.append(mesh)
if 'hands' in mode:
# read hands
line_ids = mutils.get_hand_line_ids()
joint_locs = cp.hand_joints()
# show hands
hand_colors = [[0, 1, 0], [1, 0, 0]]
for hand_idx, hand_joints in enumerate(joint_locs):
if hand_joints is None:
continue
# joint locations
for j in hand_joints:
m = o3dg.TriangleMesh.create_sphere(
radius=joint_sphere_radius_mm * 1e-3, resolution=10)
T = np.eye(4)
T[:3, 3] = j
m.transform(T)
m.paint_uniform_color(hand_colors[hand_idx])
m.compute_vertex_normals()
geoms.append(m)
# connecting lines
for line_idx, (idx0, idx1) in enumerate(line_ids):
bone = hand_joints[idx0] - hand_joints[idx1]
h = np.linalg.norm(bone)
l = o3dg.TriangleMesh.create_cylinder(
radius=bone_cylinder_radius_mm * 1e-3,
height=h,
resolution=10)
T = np.eye(4)
T[2, 3] = -h / 2.0
l.transform(T)
T = mutils.rotmat_from_vecs(bone, [0, 0, 1])
T[:3, 3] = hand_joints[idx0]
l.transform(T)
l.paint_uniform_color(bone_color)
l.compute_vertex_normals()
geoms.append(l)
if 'semantic' in mode:
n_lines_per_hand = len(line_ids)
n_parts_per_finger = 4
# find line equations for hand parts
lines = []
for hand_joints in joint_locs:
if hand_joints is None:
continue
for line_id in line_ids:
a = hand_joints[line_id[0]]
n = hand_joints[line_id[1]] - hand_joints[line_id[0]]
n /= np.linalg.norm(n)
lines.append(np.hstack((a, n)))
lines = np.asarray(lines)
ops = np.asarray(mesh.vertices)
d_lines = mutils.p_dist_linesegment(ops, lines)
line_idx = np.argmin(d_lines, axis=1) % n_lines_per_hand
finger_idx, part_idx = divmod(line_idx, n_parts_per_finger)
if 'phalanges' in mode:
mesh = apply_semantic_colormap_to_mesh(mesh, part_idx)
elif 'fingers' in mode:
mesh = apply_semantic_colormap_to_mesh(mesh, finger_idx)
geoms.append(mesh)
elif 'mano' in mode:
for hand in cp.mano_meshes():
if hand is None:
continue
mesh = o3dg.TriangleMesh()
mesh.vertices = o3du.Vector3dVector(hand['vertices'])
mesh.triangles = o3du.Vector3iVector(hand['faces'])
mesh.paint_uniform_color(bone_color)
mesh.compute_vertex_normals()
geoms.append(mesh)
o3dv.draw_geometries(geoms)
def apply_colormap_to_mesh(mesh, sigmoid_a=0.05, invert=False):
colors = np.asarray(mesh.vertex_colors)[:, 0]
colors = mutils.texture_proc(colors, a=sigmoid_a, invert=invert)
colors = plt.cm.inferno(colors)[:, :3]
mesh.vertex_colors = o3du.Vector3dVector(colors)
return mesh
def test_GFP():
from open3d import visualization, geometry, utility
test_points, test_labels = load_h5(test_h5_path)
nr_points = ModellingParameters.NUM_POINTS_UPSAMPLE
model = GFPNet(nr_points)
model.compile(optimizer='adam',
loss='mean_squared_error',
metrics=['mse', 'accuracy'])
# print the model summary
model.load_weights(net_params.PATH_TO_WEIGHTS)
print(model.summary())
primitive_path = PATHS.PATH_TO_PRIMITIVES.CAR.root + "CarPrimitive_15_500.off"
io_primitive = IoPrimitive(primitive_path)
io_primitive.load_primitive(normalize=False)
pathlist = Path(PATHS.NETWORK.root).glob('*.{0}'.format('off'))
for path in pathlist:
lidar_cloud_path = str(path)
file_name = lidar_cloud_path.split("\\")[-1]
label_path = PATHS.NETWORK.TEST_MODELLED + file_name
cloud_path = PATHS.NETWORK.TEST_CLOUD + file_name
observation_points = read_off_file(cloud_path)
io_observation_cloud = IoObservation(observation_points)
io_primitive.scale_relative_to(io_observation_cloud)
io_primitive.align_primitive_on_z_axis(io_observation_cloud)
io_primitive.compute_normals()
modelled_primitive_points = read_off_file(label_path)
io_modelled_primitive = IoObservation(modelled_primitive_points)
eval_X, boolIndexes = make_samples(io_observation_cloud,
io_primitive,
io_modelled_primitive,
usePrimitivePoints=False,
generate_for='test')
eval_X = upsample_point_set(
sample_collection=eval_X,
points_count=ModellingParameters.NUM_POINTS_UPSAMPLE)
cloud_bef = geometry.PointCloud()
cloud_bef.points = utility.Vector3dVector(
np.asarray(io_primitive.point_cloud.points))
cloud_bef.normals = utility.Vector3dVector(
np.asarray(io_primitive.point_cloud.normals))
cloud_bef.paint_uniform_color([255, 0, 0])
cloud_lidar = geometry.PointCloud()
cloud_lidar.points = utility.Vector3dVector(
np.asarray(io_observation_cloud.point_cloud))
cloud_lidar.paint_uniform_color([0, 0, 0])
cloud_modelled = geometry.PointCloud()
cloud_modelled.points = utility.Vector3dVector(
np.asarray(io_modelled_primitive.point_cloud.points))
cloud_modelled.paint_uniform_color([255, 255, 0])
visualization.draw_geometries([cloud_bef, cloud_lidar, cloud_modelled])
final_pts = []
idx = 0
for i in range(0, len(eval_X)):
pred = eval_X[i].reshape(-1, nr_points, 3)
points = model.predict(pred)
final_pts.append(points)
idx = i
final_pts = np.reshape(final_pts, newshape=(len(final_pts), 3))
print('Final pts len : ', len(final_pts))
final_pts = [
point * ModellingParameters.CAR.SCALE for point in final_pts
]
true_indexes = [i for i, val in enumerate(boolIndexes) if val]
for i in true_indexes:
cloud_bef.colors[i] = [0, 255, 0]
import pclpy.pcl.point_types as ptype
aux = ptype.PointXYZ()
new_points = []
for i in range(0, len(final_pts)):
val = io_primitive.point_cloud.points[true_indexes[i]] + (
final_pts[i] - ModellingParameters.NORMALIZATION_CENTER)
aux.x = val[0]
aux.y = val[1]
aux.z = val[2]
new_points.append(val)
# transform_neighbor_points_for(i, aux, srcPrimitive, None)
cloud_aft = geometry.PointCloud()
cloud_aft.points = utility.Vector3dVector(new_points)
cloud_aft.paint_uniform_color([0, 0, 255])
# cloud.normals = utility.Vector3dVector(cloud_points[:,3:6])
visualization.draw_geometries(
[cloud_bef, cloud_aft, cloud_lidar, cloud_modelled])
def return_open3d_mesh(self, vertices, triangles):
mesh = geometry.TriangleMesh()
mesh.vertices = utility.Vector3dVector(vertices)
mesh.triangles = utility.Vector3iVector(triangles)
return mesh