[Refactor] Refactor camera frame creation APIs

README.md

@@ -19,7 +19,7 @@ clear and easy-to-use APIs.
    ```python
    import camtools as ct
    import open3d as o3d
-   cameras = ct.camera.create_camera_ray_frames(Ks, Ts)
+   cameras = ct.camera.create_camera_frames(Ks, Ts)
    o3d.visualization.draw_geometries([cameras])
    ```
 

camtools/camera.py

@@ -19,132 +19,22 @@ def create_camera_center_line(Ts, color=np.array([1, 0, 0])):
     return ls
 
 
-def create_camera_frame(T, size=0.1, color=[0, 0, 1]):
-    R, t = T[:3, :3], T[:3, 3]
-
-    C0 = convert.R_t_to_C(R, t).ravel()
-    C1 = (
-        C0 + R.T.dot(np.array([[-size], [-size], [3 * size]], dtype=np.float32)).ravel()
-    )
-    C2 = (
-        C0 + R.T.dot(np.array([[-size], [+size], [3 * size]], dtype=np.float32)).ravel()
-    )
-    C3 = (
-        C0 + R.T.dot(np.array([[+size], [+size], [3 * size]], dtype=np.float32)).ravel()
-    )
-    C4 = (
-        C0 + R.T.dot(np.array([[+size], [-size], [3 * size]], dtype=np.float32)).ravel()
-    )
-
-    ls = o3d.geometry.LineSet()
-    points = np.array([C0, C1, C2, C3, C4])
-    lines = [[0, 1], [0, 2], [0, 3], [0, 4], [1, 2], [2, 3], [3, 4], [4, 1]]
-    colors = np.tile(color, (len(lines), 1))
-    ls.points = o3d.utility.Vector3dVector(points)
-    ls.lines = o3d.utility.Vector2iVector(lines)
-    ls.colors = o3d.utility.Vector3dVector(colors)
-
-    return ls
-
-
-def create_camera_frames(
-    Ts,
-    size=0.1,
-    color=[0, 0, 1],
-    start_color=[0, 1, 0],
-    end_color=[1, 0, 0],
-    center_line=True,
-    center_line_color=[1, 0, 0],
+def _create_camera_frame(
+    K,
+    T,
+    image_wh,
+    size,
+    color,
+    up_triangle,
+    center_ray,
 ):
-    camera_frames = o3d.geometry.LineSet()
-    for index, T in enumerate(Ts):
-        if index == 0:
-            frame_color = start_color
-        elif index == len(Ts) - 1:
-            frame_color = end_color
-        else:
-            frame_color = color
-        camera_frame = create_camera_frame(T, size=size, color=frame_color)
-        camera_frames += camera_frame
-
-    if len(Ts) > 1 and center_line:
-        center_line = create_camera_center_line(Ts, color=center_line_color)
-        camera_frames += center_line
-
-    return camera_frames
-
-
-def create_camera_center_ray(K, T, size=0.1, color=[0, 0, 1]):
-    """
-    K: 3x3
-    T: 4x4
-
-    Returns a linset of two points. The line starts the camera center and passes
-    through the center of the image.
-    """
-    sanity.assert_T(T)
-    sanity.assert_K(K)
-
-    # Pick point at the center of the image
-    # Assumes that the camera offset is exactly at the center of the image.
-    col = K[0, 2]
-    row = K[1, 2]
-    points = np.array(
-        [
-            [col, row, 1],
-        ]
-    )
-
-    # Transform to camera space
-    points = (np.linalg.inv(K) @ points.T).T
-
-    # Normalize to have 1 distance
-    points = points / np.linalg.norm(points, axis=1, keepdims=True) * size
-
-    # Transform to world space
-    R, _ = convert.T_to_R_t(T)
-    C = convert.T_to_C(T)
-    points = (np.linalg.inv(R) @ points.T).T + C
-
-    # Create line set
-    points = np.vstack((C, points))
-    lines = np.array(
-        [
-            [0, 1],
-        ]
-    )
-    ls = o3d.geometry.LineSet()
-    ls.points = o3d.utility.Vector3dVector(points)
-    ls.lines = o3d.utility.Vector2iVector(lines)
-
-    return ls
-
-
-def create_camera_center_rays(Ks, Ts, size=0.1, color=[0, 0, 1]):
-    """
-    K: 3x3
-    T: 4x4
-
-    Returns a linset of two points. The line starts the camera center and passes
-    through the center of the image.
-    """
-    if len(Ts) != len(Ks):
-        raise ValueError(f"len(Ts) != len(Ks)")
-
-    camera_rays = o3d.geometry.LineSet()
-    for T, K in zip(Ts, Ks):
-        camera_rays += create_camera_center_ray(T, K, size=size, color=color)
-
-    return camera_rays
-
-
-def _create_camera_ray_frame(K, T, image_wh, size, color, disable_up_triangle):
     """
     K: (3, 3)
     T: (4, 4)
     image:_wh: (2,)
     size: float
-    disable_up_triangle: bool
+    up_triangle: bool
+    center_ray: bool
     """
     T, K, color = np.asarray(T), np.asarray(K), np.asarray(color)
     sanity.assert_T(T)
@@ -214,7 +104,7 @@ def points_2d_to_3d_world(points_2d):
     ls.lines = o3d.utility.Vector2iVector(lines)
     ls.paint_uniform_color(color)
 
-    if not disable_up_triangle:
+    if up_triangle:
         up_gap = 0.1 * h
         up_height = 0.5 * h
         up_points_2d = np.array(
@@ -238,6 +128,17 @@ def points_2d_to_3d_world(points_2d):
         up_ls.paint_uniform_color(color)
         ls += up_ls
 
+    if center_ray:
+        center_px_2d = np.array([[(w - 1) / 2, (h - 1) / 2]])
+        center_px_3d = points_2d_to_3d_world(center_px_2d)
+        center_ray_points = np.vstack((C, center_px_3d))
+        center_ray_lines = np.array([[0, 1]])
+        center_ray_ls = o3d.geometry.LineSet()
+        center_ray_ls.points = o3d.utility.Vector3dVector(center_ray_points)
+        center_ray_ls.lines = o3d.utility.Vector2iVector(center_ray_lines)
+        center_ray_ls.paint_uniform_color(color)
+        ls += center_ray_ls
+
     return ls
 
 
@@ -257,20 +158,25 @@ def _wrap_dim(dim: int, max_dim: int, inclusive: bool = False) -> int:
     return dim
 
 
-def create_camera_ray_frames(
+def create_camera_frames(
     Ks,
     Ts,
     image_whs=None,
     size=0.1,
-    color=[0, 0, 1],
+    color=(0, 0, 1),
     highlight_color_map=None,
     center_line=True,
-    center_line_color=[1, 0, 0],
-    disable_up_triangle=False,
+    center_line_color=(1, 0, 0),
+    up_triangle=True,
+    center_ray=False,
 ):
     """
+    Draw camera frames in line sets.
+
     Args:
-        Ks: List of 3x3 camera intrinsics matrices.
+        Ks: List of 3x3 camera intrinsics matrices. You can set Ks to None if
+            the intrinsics are not available. In this case, a dummy intrinsics
+            matrix will be used.
         Ts: List of 4x4 camera extrinsics matrices.
         image_whs: List of image width and height. If None, the image width and
             height are determined from the camera intrinsics by assuming that
@@ -284,8 +190,20 @@ def create_camera_ray_frames(
             camera is highlighted.
         center_line: If True, the camera center line will be drawn.
         center_line_color: Color of the camera center line.
-        disable_up_triangle: If True, the up triangle will not be drawn.
+        up_triangle: If True, the up triangle will be drawn.
+        center_ray: If True, the ray from camera center to the center pixel in
+            the image plane will be drawn.
+
+    Return:
+        An Open3D LinetSet containing all the camera frames.
     """
+    if Ks is None:
+        cx = 320
+        cy = 240
+        fx = 320
+        fy = 320
+        K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
+        Ks = [K for _ in range(len(Ts))]
     if len(Ts) != len(Ks):
         raise ValueError(f"len(Ts) != len(Ks): {len(Ts)} != {len(Ks)}")
     for K in Ks:
@@ -329,13 +247,14 @@ def create_camera_ray_frames(
             frame_color = highlight_color_map[index]
         else:
             frame_color = color
-        camera_frame = _create_camera_ray_frame(
+        camera_frame = _create_camera_frame(
             K,
             T,
             image_wh=image_wh,
             size=size,
             color=frame_color,
-            disable_up_triangle=disable_up_triangle,
+            up_triangle=up_triangle,
+            center_ray=center_ray,
         )
         ls += camera_frame
 
@@ -347,3 +266,31 @@ def create_camera_ray_frames(
         ls += center_line
 
     return ls
+
+
+def create_camera_frames_with_Ts(
+    Ts,
+    image_whs=None,
+    size=0.1,
+    color=(0, 0, 1),
+    highlight_color_map=None,
+    center_line=True,
+    center_line_color=(1, 0, 0),
+    up_triangle=True,
+    center_ray=False,
+):
+    """
+    Returns ct.camera.create_camera_frames(Ks=None, Ts, ...).
+    """
+    return create_camera_frames(
+        Ks=None,
+        Ts=Ts,
+        image_whs=image_whs,
+        size=size,
+        color=color,
+        highlight_color_map=highlight_color_map,
+        center_line=center_line,
+        center_line_color=center_line_color,
+        up_triangle=up_triangle,
+        center_ray=center_ray,
+    )