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))