Skip to content

Commit

Permalink
[Refactor] Refactor camera frame creation APIs (#22)
Browse files Browse the repository at this point in the history 
* allow Ks to be None

* rename to create_camera_frames and clean up

* disable_up_triangle -> up_triangle

* add center_ray option

* update comments
  • Loading branch information
@yxlao
yxlao authored Oct 22, 2023
1 parent 217427f commit 3871b62
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 130 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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])
```

Expand Down
205 changes: 76 additions & 129 deletions camtools/camera.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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(
Expand All @@ -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


Expand All @@ -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
Expand All @@ -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:
Expand Down Expand Up @@ -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

Expand All @@ -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,
)

0 comments on commit 3871b62

Please sign in to comment.