32 lines
1.1 KiB
Python
32 lines
1.1 KiB
Python
def unproject_depth_frame(depth, intrinsics, extrinsics):
|
|
import numpy as np
|
|
|
|
height, width = depth.shape
|
|
x, y = np.meshgrid(
|
|
np.arange(width, dtype=np.float32),
|
|
np.arange(height, dtype=np.float32),
|
|
)
|
|
pixels = np.stack((x, y, np.ones_like(x)), axis=-1).reshape(-1, 3)
|
|
rays = (np.linalg.inv(intrinsics) @ pixels.T).T
|
|
camera_points = rays * depth.reshape(-1, 1)
|
|
camera_points = np.column_stack(
|
|
(camera_points, np.ones(camera_points.shape[0], dtype=np.float32))
|
|
)
|
|
world_to_camera = np.eye(4, dtype=np.float32)
|
|
world_to_camera[: extrinsics.shape[0], :4] = extrinsics
|
|
world_points = (np.linalg.inv(world_to_camera) @ camera_points.T).T
|
|
return world_points[:, :3].reshape(height, width, 3)
|
|
|
|
|
|
def unproject_camera_depth(depth, intrinsics):
|
|
import numpy as np
|
|
|
|
height, width = depth.shape
|
|
x, y = np.meshgrid(
|
|
np.arange(width, dtype=np.float32),
|
|
np.arange(height, dtype=np.float32),
|
|
)
|
|
pixels = np.stack((x, y, np.ones_like(x)), axis=-1).reshape(-1, 3)
|
|
rays = (np.linalg.inv(intrinsics) @ pixels.T).T
|
|
return (rays * depth.reshape(-1, 1)).reshape(height, width, 3)
|