def show_contactmap()

in scripts/show_contactmap.py [0:0]

• 76 lines of code
• 18 McCabe index (conditional complexity)

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,
                    show_axes=False):
  """
  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
  show_axes: visualize coordinate axes if True
  """
  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)
  
  if show_axes:
    geoms.append(o3dg.TriangleMesh.create_coordinate_frame(size=0.2))
  o3dv.draw_geometries(geoms)