File size: 5,184 Bytes
36c95ba | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 | import torch
import torch.nn.functional as F
from kornia.geometry.conversions import convert_points_from_homogeneous, convert_points_to_homogeneous
def project_points(point_3d: torch.Tensor, camera_matrix: torch.Tensor) -> torch.Tensor:
r"""Project a 3d point onto the 2d camera plane.
Args:
point3d: tensor containing the 3d points to be projected
to the camera plane. The shape of the tensor can be :math:`(*, 3)`.
camera_matrix: tensor containing the intrinsics camera
matrix. The tensor shape must be :math:`(*, 3, 3)`.
Returns:
tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.
Example:
>>> _ = torch.manual_seed(0)
>>> X = torch.rand(1, 3)
>>> K = torch.eye(3)[None]
>>> project_points(X, K)
tensor([[5.6088, 8.6827]])
"""
if not isinstance(point_3d, torch.Tensor):
raise TypeError(f"Input point_3d type is not a torch.Tensor. Got {type(point_3d)}")
if not isinstance(camera_matrix, torch.Tensor):
raise TypeError(f"Input camera_matrix type is not a torch.Tensor. Got {type(camera_matrix)}")
if not (point_3d.device == camera_matrix.device):
raise ValueError("Input tensors must be all in the same device.")
if not point_3d.shape[-1] == 3:
raise ValueError("Input points_3d must be in the shape of (*, 3)." " Got {}".format(point_3d.shape))
if not camera_matrix.shape[-2:] == (3, 3):
raise ValueError("Input camera_matrix must be in the shape of (*, 3, 3).")
# projection eq. [u, v, w]' = K * [x y z 1]'
# u = fx * X / Z + cx
# v = fy * Y / Z + cy
# project back using depth dividing in a safe way
xy_coords: torch.Tensor = convert_points_from_homogeneous(point_3d)
x_coord: torch.Tensor = xy_coords[..., 0]
y_coord: torch.Tensor = xy_coords[..., 1]
# unpack intrinsics
fx: torch.Tensor = camera_matrix[..., 0, 0]
fy: torch.Tensor = camera_matrix[..., 1, 1]
cx: torch.Tensor = camera_matrix[..., 0, 2]
cy: torch.Tensor = camera_matrix[..., 1, 2]
# apply intrinsics ans return
u_coord: torch.Tensor = x_coord * fx + cx
v_coord: torch.Tensor = y_coord * fy + cy
return torch.stack([u_coord, v_coord], dim=-1)
def unproject_points(
point_2d: torch.Tensor, depth: torch.Tensor, camera_matrix: torch.Tensor, normalize: bool = False
) -> torch.Tensor:
r"""Unproject a 2d point in 3d.
Transform coordinates in the pixel frame to the camera frame.
Args:
point2d: tensor containing the 2d to be projected to
world coordinates. The shape of the tensor can be :math:`(*, 2)`.
depth: tensor containing the depth value of each 2d
points. The tensor shape must be equal to point2d :math:`(*, 1)`.
camera_matrix: tensor containing the intrinsics camera
matrix. The tensor shape must be :math:`(*, 3, 3)`.
normalize: whether to normalize the pointcloud. This
must be set to `True` when the depth is represented as the Euclidean
ray length from the camera position.
Returns:
tensor of (x, y, z) world coordinates with shape :math:`(*, 3)`.
Example:
>>> _ = torch.manual_seed(0)
>>> x = torch.rand(1, 2)
>>> depth = torch.ones(1, 1)
>>> K = torch.eye(3)[None]
>>> unproject_points(x, depth, K)
tensor([[0.4963, 0.7682, 1.0000]])
"""
if not isinstance(point_2d, torch.Tensor):
raise TypeError(f"Input point_2d type is not a torch.Tensor. Got {type(point_2d)}")
if not isinstance(depth, torch.Tensor):
raise TypeError(f"Input depth type is not a torch.Tensor. Got {type(depth)}")
if not isinstance(camera_matrix, torch.Tensor):
raise TypeError(f"Input camera_matrix type is not a torch.Tensor. Got {type(camera_matrix)}")
if not (point_2d.device == depth.device == camera_matrix.device):
raise ValueError("Input tensors must be all in the same device.")
if not point_2d.shape[-1] == 2:
raise ValueError("Input points_2d must be in the shape of (*, 2)." " Got {}".format(point_2d.shape))
if not depth.shape[-1] == 1:
raise ValueError("Input depth must be in the shape of (*, 1)." " Got {}".format(depth.shape))
if not camera_matrix.shape[-2:] == (3, 3):
raise ValueError("Input camera_matrix must be in the shape of (*, 3, 3).")
# projection eq. K_inv * [u v 1]'
# x = (u - cx) * Z / fx
# y = (v - cy) * Z / fy
# unpack coordinates
u_coord: torch.Tensor = point_2d[..., 0]
v_coord: torch.Tensor = point_2d[..., 1]
# unpack intrinsics
fx: torch.Tensor = camera_matrix[..., 0, 0]
fy: torch.Tensor = camera_matrix[..., 1, 1]
cx: torch.Tensor = camera_matrix[..., 0, 2]
cy: torch.Tensor = camera_matrix[..., 1, 2]
# projective
x_coord: torch.Tensor = (u_coord - cx) / fx
y_coord: torch.Tensor = (v_coord - cy) / fy
xyz: torch.Tensor = torch.stack([x_coord, y_coord], dim=-1)
xyz = convert_points_to_homogeneous(xyz)
if normalize:
xyz = F.normalize(xyz, dim=-1, p=2.0)
return xyz * depth
|