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:
>>> X = torch.rand(1, 3)
>>> K = torch.eye(3)[None]
>>> project_points(X, K)
tensor([[1.4210, 0.9953]])
"""
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 triangulate_points(P1: torch.Tensor, P2: torch.Tensor,
points1: torch.Tensor,
points2: torch.Tensor) -> torch.Tensor:
r"""Reconstructs a bunch of points by triangulation.
Triangulates the 3d position of 2d correspondences between several images.
Reference: Internally it uses DLT method from Hartley/Zisserman 12.2 pag.312
The input points are assumed to be in homogeneous coordinate system and being inliers
correspondences. The method does not perform any robust estimation.
Args:
P1: The projection matrix for the first camera with shape :math:`(*, 3, 4)`.
P2: The projection matrix for the second camera with shape :math:`(*, 3, 4)`.
points1: The set of points seen from the first camera frame in the camera plane
coordinates with shape :math:`(*, N, 2)`.
points2: The set of points seen from the second camera frame in the camera plane
coordinates with shape :math:`(*, N, 2)`.
Returns:
The reconstructed 3d points in the world frame with shape :math:`(*, N, 3)`.
"""
if not (len(P1.shape) >= 2 and P1.shape[-2:] == (3, 4)):
raise AssertionError(P1.shape)
if not (len(P2.shape) >= 2 and P2.shape[-2:] == (3, 4)):
raise AssertionError(P2.shape)
if len(P1.shape[:-2]) != len(P2.shape[:-2]):
raise AssertionError(P1.shape, P2.shape)
if not (len(points1.shape) >= 2 and points1.shape[-1] == 2):
raise AssertionError(points1.shape)
if not (len(points2.shape) >= 2 and points2.shape[-1] == 2):
raise AssertionError(points2.shape)
if len(points1.shape[:-2]) != len(points2.shape[:-2]):
raise AssertionError(points1.shape, points2.shape)
if len(P1.shape[:-2]) != len(points1.shape[:-2]):
raise AssertionError(P1.shape, points1.shape)
# allocate and construct the equations matrix with shape (*, 4, 4)
points_shape = max(points1.shape,
points2.shape) # this allows broadcasting
X = torch.zeros(points_shape[:-1] + (4, 4)).type_as(points1)
for i in range(4):
X[..., 0, i] = points1[..., 0] * P1[..., 2:3, i] - P1[..., 0:1, i]
X[..., 1, i] = points1[..., 1] * P1[..., 2:3, i] - P1[..., 1:2, i]
X[..., 2, i] = points2[..., 0] * P2[..., 2:3, i] - P2[..., 0:1, i]
X[..., 3, i] = points2[..., 1] * P2[..., 2:3, i] - P2[..., 1:2, i]
# 1. Solve the system Ax=0 with smallest eigenvalue
# 2. Return homogeneous coordinates
_, _, V = torch.svd(X)
points3d_h = V[..., -1]
points3d: torch.Tensor = convert_points_from_homogeneous(points3d_h)
return points3d
def transform_points(trans_01: torch.Tensor,
points_1: torch.Tensor) -> torch.Tensor:
r"""Function that applies transformations to a set of points.
Args:
trans_01 (torch.Tensor): tensor for transformations of shape
:math:`(B, D+1, D+1)`.
points_1 (torch.Tensor): tensor of points of shape :math:`(B, N, D)`.
Returns:
torch.Tensor: tensor of N-dimensional points.
Shape:
- Output: :math:`(B, N, D)`
Examples:
>>> points_1 = torch.rand(2, 4, 3) # BxNx3
>>> trans_01 = torch.eye(4).view(1, 4, 4) # Bx4x4
>>> points_0 = transform_points(trans_01, points_1) # BxNx3
"""
check_is_tensor(trans_01)
check_is_tensor(points_1)
if not (trans_01.device == points_1.device
and trans_01.dtype == points_1.dtype):
raise TypeError(
"Tensor must be in the same device and dtype. "
f"Got trans_01 with ({trans_01.dtype}, {points_1.dtype}) and "
f"points_1 with ({points_1.dtype}, {points_1.dtype})")
if not trans_01.shape[0] == points_1.shape[0] and trans_01.shape[0] != 1:
raise ValueError(
"Input batch size must be the same for both tensors or 1")
if not trans_01.shape[-1] == (points_1.shape[-1] + 1):
raise ValueError("Last input dimensions must differ by one unit")
# We reshape to BxNxD in case we get more dimensions, e.g., MxBxNxD
shape_inp = list(points_1.shape)
points_1 = points_1.reshape(-1, points_1.shape[-2], points_1.shape[-1])
trans_01 = trans_01.reshape(-1, trans_01.shape[-2], trans_01.shape[-1])
# We expand trans_01 to match the dimensions needed for bmm
trans_01 = torch.repeat_interleave(trans_01,
repeats=points_1.shape[0] //
trans_01.shape[0],
dim=0)
# to homogeneous
points_1_h = convert_points_to_homogeneous(points_1) # BxNxD+1
# transform coordinates
points_0_h = torch.bmm(points_1_h, trans_01.permute(0, 2, 1))
points_0_h = torch.squeeze(points_0_h, dim=-1)
# to euclidean
points_0 = convert_points_from_homogeneous(points_0_h) # BxNxD
# reshape to the input shape
shape_inp[-2] = points_0.shape[-2]
shape_inp[-1] = points_0.shape[-1]
points_0 = points_0.reshape(shape_inp)
return points_0
def project_points(
point_3d: torch.Tensor,
camera_matrix: torch.Tensor) -> torch.Tensor:
r"""Projects a 3d point onto the 2d camera plane.
Args:
point3d (torch.Tensor): tensor containing the 3d points to be projected
to the camera plane. The shape of the tensor can be :math:`(*, 3)`.
camera_matrix (torch.Tensor): tensor containing the intrinsics camera
matrix. The tensor shape must be Bx4x4.
Returns:
torch.Tensor: array of (u, v) cam coordinates with shape :math:`(*, 2)`.
"""
if not torch.is_tensor(point_3d):
raise TypeError("Input point_3d type is not a torch.Tensor. Got {}"
.format(type(point_3d)))
if not torch.is_tensor(camera_matrix):
raise TypeError("Input camera_matrix type is not a torch.Tensor. Got {}"
.format(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:1]
y_coord: torch.Tensor = xy_coords[..., 1:2]
# unpack intrinsics
fx: torch.Tensor = camera_matrix[..., 0:1, 0]
fy: torch.Tensor = camera_matrix[..., 1:2, 1]
cx: torch.Tensor = camera_matrix[..., 0:1, 2]
cy: torch.Tensor = camera_matrix[..., 1:2, 2]
# apply intrinsics ans return
u_coord: torch.Tensor = x_coord * fx + cx
v_coord: torch.Tensor = y_coord * fy + cy
return torch.cat([u_coord, v_coord], dim=-1)
def project_to_image(project, points):
"""
Project points to image
Args:
project [torch.tensor(..., 3, 4)]: Projection matrix
points [torch.Tensor(..., 3)]: 3D points
Returns:
points_img [torch.Tensor(..., 2)]: Points in image
points_depth [torch.Tensor(...)]: Depth of each point
"""
# Reshape tensors to expected shape
points = convert_points_to_homogeneous(points)
points = points.unsqueeze(dim=-1)
project = project.unsqueeze(dim=1)
# Transform points to image and get depths
points_t = project @ points
points_t = points_t.squeeze(dim=-1)
points_img = convert_points_from_homogeneous(points_t)
points_depth = points_t[..., -1] - project[..., 2, 3]
return points_img, points_depth
def transform_points(trans_01: torch.Tensor,
points_1: torch.Tensor) -> torch.Tensor:
r"""Function that applies transformations to a set of points.
Args:
trans_01 (torch.Tensor): tensor for transformations of shape
:math:`(B, D+1, D+1)`.
points_1 (torch.Tensor): tensor of points of shape :math:`(B, N, D)`.
Returns:
torch.Tensor: tensor of N-dimensional points.
Shape:
- Output: :math:`(B, N, D)`
Examples:
>>> points_1 = torch.rand(2, 4, 3) # BxNx3
>>> trans_01 = torch.eye(4).view(1, 4, 4) # Bx4x4
>>> points_0 = kornia.transform_points(trans_01, points_1) # BxNx3
"""
if not torch.is_tensor(trans_01) or not torch.is_tensor(points_1):
raise TypeError("Input type is not a torch.Tensor")
if not trans_01.device == points_1.device:
raise TypeError("Tensor must be in the same device")
if not trans_01.shape[0] == points_1.shape[0] and trans_01.shape[0] != 1:
raise ValueError(
"Input batch size must be the same for both tensors or 1")
if not trans_01.shape[-1] == (points_1.shape[-1] + 1):
raise ValueError("Last input dimensions must differe by one unit")
# to homogeneous
points_1_h = convert_points_to_homogeneous(points_1) # BxNxD+1
# transform coordinates
points_0_h = torch.matmul(trans_01.unsqueeze(1), points_1_h.unsqueeze(-1))
points_0_h = torch.squeeze(points_0_h, dim=-1)
# to euclidean
points_0 = convert_points_from_homogeneous(points_0_h) # BxNxD
return points_0
def laf_to_boundary_points(LAF: torch.Tensor, n_pts: int = 50) -> torch.Tensor:
"""Convert LAFs to boundary points of the regions + center.
Used for local features visualization, see visualize_laf function.
Args:
LAF:
n_pts: number of points to output.
Returns:
tensor of boundary points.
Shape:
- Input: :math:`(B, N, 2, 3)`
- Output: :math:`(B, N, n_pts, 2)`
"""
raise_error_if_laf_is_not_valid(LAF)
B, N, _, _ = LAF.size()
pts = torch.cat(
[
torch.sin(torch.linspace(0, 2 * math.pi, n_pts - 1)).unsqueeze(-1),
torch.cos(torch.linspace(0, 2 * math.pi, n_pts - 1)).unsqueeze(-1),
torch.ones(n_pts - 1, 1),
],
dim=1,
)
# Add origin to draw also the orientation
pts = torch.cat([torch.tensor([0.0, 0.0, 1.0]).view(1, 3), pts],
dim=0).unsqueeze(0).expand(B * N, n_pts, 3)
pts = pts.to(LAF.device).to(LAF.dtype)
aux = torch.tensor([0.0, 0.0, 1.0]).view(1, 1, 3).expand(B * N, 1, 3)
HLAF = torch.cat([LAF.view(-1, 2, 3),
aux.to(LAF.device).to(LAF.dtype)],
dim=1)
pts_h = torch.bmm(HLAF, pts.permute(0, 2, 1)).permute(0, 2, 1)
return convert_points_from_homogeneous(pts_h.view(B, N, n_pts, 3))
