text: """
  Utilities for rendering camera frustums, 3d point clouds, and coordinate frames using the Open3d library.
  
  Authors: John Lambert
  """
  
  import argparse
  from typing import List
  
  import numpy as np
  import open3d
  from colour import Color
  from gtsam import Cal3Bundler, Pose3
  
  from gtsfm.common.view_frustum import ViewFrustum
  
  
  def create_colored_point_cloud_open3d(point_cloud: np.ndarray, rgb: np.ndarray) -> open3d.geometry.PointCloud:
      """Render a point cloud as individual colored points, using Open3d.
  
      Args:
          point_cloud: array of shape (N,3) representing 3d points.
          rgb: uint8 array of shape (N,3) representing colors in RGB order, in the range [0,255].
  
      Returns:
          pcd: Open3d geometry object representing a colored 3d point cloud.
      """
      colors = rgb.astype(np.float64) / 255
  
      pcd = open3d.geometry.PointCloud()
      pcd.points = open3d.utility.Vector3dVector(point_cloud)
      pcd.colors = open3d.utility.Vector3dVector(colors)
  
      return pcd
  
  
  def create_colored_spheres_open3d(
      point_cloud: np.ndarray, rgb: np.ndarray, sphere_radius: float
  ) -> List[open3d.geometry.TriangleMesh]:
      """Create a colored sphere mesh for every point inside the point cloud, using Open3d.
  
      Note: this is quite computationally expensive.
  
      Args:
          point_cloud: array of shape (N,3) representing 3d points.
          rgb: uint8 array of shape (N,3) representing colors in RGB order, in the range [0,255].
          sphere_radius: radius of each rendered sphere.
  
      Returns:
          spheres: list of Open3d geometry objects, where each element (a sphere) represents a 3d point.
      """
      colors = rgb.astype(np.float64) / 255
  
      spheres = []
      n = point_cloud.shape[0]
  
      for j in range(n):
          wTj = np.eye(4)
          wTj[:3, 3] = point_cloud[j]
          mesh = open3d.geometry.TriangleMesh.create_sphere(radius=sphere_radius, resolution=20)
          mesh.transform(wTj)
          mesh.paint_uniform_color(colors[j])
  
          spheres.append(mesh)
  
      return spheres
  
  
  def create_all_frustums_open3d(
      wTi_list: List[Pose3], calibrations: List[Cal3Bundler], frustum_ray_len: float = 0.3
  ) -> List[open3d.geometry.LineSet]:
      """Render camera frustums as collections of line segments, using Open3d.
  
      Frustums are colored red-to-green by image order (for ordered collections, this corresponds to trajectory order).
  
      Args:
          wTi_list: list of camera poses for each image
          calibrations: calibration object for each camera
  
      Returns:
          line_sets: list of line segments that together parameterize all camera frustums
      """
      line_sets = []
  
      # get array of shape Nx3 representing RGB values in [0,1], incrementally shifting from red to green
      colormap = np.array(
          [[color_obj.rgb] for color_obj in Color("red").range_to(Color("green"), len(wTi_list))]
      ).squeeze()
  
      for i, (K, wTi) in enumerate(zip(calibrations, wTi_list)):
  
          K = K.K()
          fx = K[0, 0]
  
          # Use 2*principal point as proxy measure for image height and width
          # TODO (in future PR): use the real image height and width
          px = K[0, 2]
          py = K[1, 2]
          img_w = px * 2
          img_h = py * 2
          frustum_obj = ViewFrustum(fx, img_w, img_h, frustum_ray_len=frustum_ray_len)
  
          edges_worldfr = frustum_obj.get_mesh_edges_worldframe(wTi)
          for verts_worldfr in edges_worldfr:
  
              lines = [[0, 1]]
              # color is in range [0,1]
              color = tuple(colormap[i].tolist())
              colors = [color for i in range(len(lines))]
  
              line_set = open3d.geometry.LineSet(
                  points=open3d.utility.Vector3dVector(verts_worldfr),
                  lines=open3d.utility.Vector2iVector(lines),
              )
              line_set.colors = open3d.utility.Vector3dVector(colors)
              line_sets.append(line_set)
  
      return line_sets
  
  
  def draw_coordinate_frame(wTc: Pose3, axis_length: float = 1.0) -> List[open3d.geometry.LineSet]:
      """Draw 3 orthogonal axes representing a camera coordinate frame.
  
      Note: x,y,z axes correspond to red, green, blue colors.
  
      Args:
          wTc: Pose of a camera in the world frame.
          axis_length: length to use for line segments (representing coordinate frame axes).
  
      Returns:
          line_sets: list of Open3D LineSet objects, representing 3 axes (a coordinate frame).
      """
      RED = np.array([1, 0, 0])  # x-axis
      GREEN = np.array([0, 1, 0])  # y-axis
      BLUE = np.array([0, 0, 1])  # z-axis
      colors = (RED, GREEN, BLUE)
  
      # line segment on each axis will connect just 2 vertices.
      lines = [[0, 1]]
  
      line_sets = []
      for axis, color in zip([0, 1, 2], colors):
          # one point at optical center, other point along specified axis.
          verts_camfr = np.zeros((2, 3))
          verts_camfr[0, axis] = axis_length
  
          verts_worldfr = []
          for i in range(2):
              verts_worldfr.append(wTc.transformFrom(verts_camfr[i]))
          verts_worldfr = np.array(verts_worldfr)
  
          line_set = open3d.geometry.LineSet(
              points=open3d.utility.Vector3dVector(verts_worldfr),
              lines=open3d.utility.Vector2iVector(lines),
          )
          line_set.colors = open3d.utility.Vector3dVector(color.reshape(1, 3))
          line_sets.append(line_set)
  
      return line_sets
  
  
  def draw_scene_open3d(
      point_cloud: np.ndarray,
      rgb: np.ndarray,
      wTi_list: List[Pose3],
      calibrations: List[Cal3Bundler],
      args: argparse.Namespace,
  ) -> None:
      """Render camera frustums and a 3d point cloud, using Open3d.
  
      Args:
          point_cloud: array of shape (N,3) representing 3d points.
          rgb: uint8 array of shape (N,3) representing colors in RGB order, in the range [0,255].
          wTi_list: list of camera poses for each image.
          calibrations: calibration object for each camera.
          args: rendering options.
      """
      frustums = create_all_frustums_open3d(wTi_list, calibrations, args.frustum_ray_len)
      if args.point_rendering_mode == "point":
          pcd = create_colored_point_cloud_open3d(point_cloud, rgb)
          geometries = frustums + [pcd]
      elif args.point_rendering_mode == "sphere":
          spheres = create_colored_spheres_open3d(point_cloud, rgb, args.sphere_radius)
          geometries = frustums + spheres
  
      open3d.visualization.draw_geometries(geometries)
  