def intrinsics_like(focal: float, input: torch.Tensor) -> torch.Tensor: r"""Return a 3x3 instrinsics matrix, with same size as the input. The center of projection will be based in the input image size. Args: focal: the focal length for the camera matrix. input: image tensor that will determine the batch size and image height and width. It is assumed to be a tensor in the shape of :math:`(B, C, H, W)`. Returns: The camera matrix with the shape of :math:`(B, 3, 3)`. """ if len(input.shape) != 4: raise AssertionError(input.shape) if focal <= 0: raise AssertionError(focal) _, _, H, W = input.shape intrinsics = numeric.eye_like(3, input) intrinsics[..., 0, 0] *= focal intrinsics[..., 1, 1] *= focal intrinsics[..., 0, 2] += 1.0 * W / 2 intrinsics[..., 1, 2] += 1.0 * H / 2 return intrinsics
def intrinsics_like(focal: float, input: torch.Tensor) -> torch.Tensor: r"""Returns a 3x3 instrinsics matrix, with same size as the input. The center of projection will be based in the input image size. Args: focal (float): the focal length for tha camera matrix. input (torch.Tensor): image tensor that will determine the batch size and image height and width. It is assumed to be a tensor in the shape of :math:`(B, C, H, W)`. Returns: torch.Tensor: The camera matrix with the shape of :math:`(B, 3, 3)`. """ assert len(input.shape) == 4, input.shape assert focal > 0, focal B, _, H, W = input.shape intrinsics = numeric.eye_like(3, input) intrinsics[..., 0, 0] *= focal intrinsics[..., 1, 1] *= focal intrinsics[..., 0, 2] += 1. * W / 2 intrinsics[..., 1, 2] += 1. * H / 2 return intrinsics
def projections_from_fundamental(F_mat: torch.Tensor) -> torch.Tensor: r"""Get the projection matrices from the Fundamental Matrix. Args: F_mat: the fundamental matrix with the shape :math:`(B, 3, 3)`. Returns: The projection matrices with shape :math:`(B, 3, 4, 2)`. """ if len(F_mat.shape) != 3: raise AssertionError(F_mat.shape) if F_mat.shape[-2:] != (3, 3): raise AssertionError(F_mat.shape) R1 = numeric.eye_like(3, F_mat) # Bx3x3 t1 = numeric.vec_like(3, F_mat) # Bx3 Ft_mat = F_mat.transpose(-2, -1) _, e2 = _nullspace(Ft_mat) R2 = numeric.cross_product_matrix(e2) @ F_mat # Bx3x3 t2 = e2[..., :, None] # Bx3x1 P1 = torch.cat([R1, t1], dim=-1) # Bx3x4 P2 = torch.cat([R2, t2], dim=-1) # Bx3x4 return torch.stack([P1, P2], dim=-1)
def motion_from_essential_choose_solution( E_mat: torch.Tensor, K1: torch.Tensor, K2: torch.Tensor, x1: torch.Tensor, x2: torch.Tensor, mask: Optional[torch.Tensor] = None, ) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: r"""Recover the relative camera rotation and the translation from an estimated essential matrix. The method checks the corresponding points in two images and also returns the triangulated 3d points. Internally uses :py:meth:`~kornia.geometry.epipolar.decompose_essential_matrix` and then chooses the best solution based on the combination that gives more 3d points in front of the camera plane from :py:meth:`~kornia.geometry.epipolar.triangulate_points`. Args: E_mat (torch.Tensor): The essential matrix in the form of :math:`(*, 3, 3)`. K1 (torch.Tensor): The camera matrix from first camera with shape :math:`(*, 3, 3)`. K2 (torch.Tensor): The camera matrix from second camera with shape :math:`(*, 3, 3)`. x1 (torch.Tensor): The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. x2 (torch.Tensor): The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. mask (torch.Tensor): A boolean mask which can be used to exclude some points from choosing the best solution. This is useful for using this function with sets of points of different cardinality (for instance after filtering with RANSAC) while keeping batch semantics. Mask is of shape :math:`(*, N)`. Returns: Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: The rotation and translation plus the 3d triangulated points. The tuple is as following :math:`[(*, 3, 3), (*, 3, 1), (*, N, 3)]`. """ assert len(E_mat.shape) >= 2 and E_mat.shape[-2:], E_mat.shape assert len(K1.shape) >= 2 and K1.shape[-2:] == (3, 3), K1.shape assert len(K2.shape) >= 2 and K2.shape[-2:] == (3, 3), K2.shape assert len(x1.shape) >= 2 and x1.shape[-1] == 2, x1.shape assert len(x2.shape) >= 2 and x2.shape[-1] == 2, x2.shape assert len(E_mat.shape[:-2]) == len(K1.shape[:-2]) == len(K2.shape[:-2]) if mask is not None: assert len(mask.shape) >= 1, mask.shape assert mask.shape == x1.shape[:-1], mask.shape unbatched = len(E_mat.shape) == 2 if unbatched: # add a leading batch dimension. We will remove it at the end, before # returning the results E_mat = E_mat[None] K1 = K1[None] K2 = K2[None] x1 = x1[None] x2 = x2[None] if mask is not None: mask = mask[None] # compute four possible pose solutions Rs, ts = motion_from_essential(E_mat) # set reference view pose and compute projection matrix R1 = numeric.eye_like(3, E_mat) # Bx3x3 t1 = numeric.vec_like(3, E_mat) # Bx3x1 # compute the projection matrices for first camera R1 = R1[:, None].expand(-1, 4, -1, -1) t1 = t1[:, None].expand(-1, 4, -1, -1) K1 = K1[:, None].expand(-1, 4, -1, -1) P1 = projection.projection_from_KRt(K1, R1, t1) # 1x4x4x4 # compute the projection matrices for second camera R2 = Rs t2 = ts K2 = K2[:, None].expand(-1, 4, -1, -1) P2 = projection.projection_from_KRt(K2, R2, t2) # Bx4x4x4 # triangulate the points x1 = x1[:, None].expand(-1, 4, -1, -1) x2 = x2[:, None].expand(-1, 4, -1, -1) X = triangulation.triangulate_points(P1, P2, x1, x2) # Bx4xNx3 # project points and compute their depth values d1 = projection.depth(R1, t1, X) d2 = projection.depth(R2, t2, X) # verify the point values that have a postive depth value depth_mask = (d1 > 0.0) & (d2 > 0.0) if mask is not None: depth_mask &= mask.unsqueeze(1) mask_indices = torch.max(depth_mask.sum(-1), dim=-1, keepdim=True)[1] # get pose and points 3d and return R_out = Rs[:, mask_indices][:, 0, 0] t_out = ts[:, mask_indices][:, 0, 0] points3d_out = X[:, mask_indices][:, 0, 0] if unbatched: R_out = R_out[0] t_out = t_out[0] points3d_out = points3d_out[0] return R_out, t_out, points3d_out
def motion_from_essential_choose_solution( E_mat: torch.Tensor, K1: torch.Tensor, K2: torch.Tensor, x1: torch.Tensor, x2: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: r"""Recovers the relative camera rotation and the translation from an estimated essential matrix. The method check the corresponding points in two images and also returns the triangulated 3d points. Internally uses :py:meth:`~kornia.geometry.epipolar.decompose_essential_matrix` and then chooses the best solution based on the combination that gives more 3d points in front of the camera plane from :py:meth:`~kornia.geometry.epipolar.triangulate_points`. Args: E_mat (torch.Tensor): The essential matrix in the form of :math:`(*, 3, 3)`. K1 (torch.Tensor): The camera matrix from first camera with shape :math:`(*, 3, 3)`. K2 (torch.Tensor): The camera matrix from second camera with shape :math:`(*, 3, 3)`. x1 (torch.Tensor): The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. x2 (torch.Tensor): The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. Returns: Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: The rotation and translation plus the 3d triangulated points. The tuple is as following :math:`[(*, 3, 3), (*, 3, 1), (*, N, 3)]`. """ assert len(E_mat.shape) >= 2 and E_mat.shape[-2:], E_mat.shape assert len(K1.shape) >= 2 and K1.shape[-2:] == (3, 3), K1.shape assert len(K2.shape) >= 2 and K2.shape[-2:] == (3, 3), K2.shape assert len(x1.shape) >= 2 and x1.shape[-1] == 2, x1.shape assert len(x2.shape) >= 2 and x2.shape[-1] == 2, x2.shape assert len(E_mat.shape[:-2]) == len(K1.shape[:-2]) == len(K2.shape[:-2]) # compute four possible pose solutions Rs, ts = motion_from_essential(E_mat) # set reference view pose and compute projection matrix R1 = numeric.eye_like(3, E_mat) # Bx3x3 t1 = numeric.vec_like(3, E_mat) # Bx3x1 # compute the projection matrices for first camera R1 = R1[:, None].expand(-1, 4, -1, -1) t1 = t1[:, None].expand(-1, 4, -1, -1) K1 = K1[:, None].expand(-1, 4, -1, -1) P1 = projection.projection_from_KRt(K1, R1, t1) # 1x4x4x4 # compute the projection matrices for second camera R2 = Rs t2 = ts K2 = K2[:, None].expand(-1, 4, -1, -1) P2 = projection.projection_from_KRt(K2, R2, t2) # Bx4x4x4 # triangulate the points x1 = x1[:, None].expand(-1, 4, -1, -1) x2 = x2[:, None].expand(-1, 4, -1, -1) X = triangulation.triangulate_points(P1, P2, x1, x2) # Bx4xNx3 # project points and compute their depth values d1 = projection.depth(R1, t1, X) d2 = projection.depth(R2, t2, X) # verify the point values that have a postive depth value mask = ((d1 > 0.) & (d2 > 0.)) mask_indices = torch.max(mask.sum(-1), dim=-1, keepdim=True)[1] # get pose and points 3d and return R_out = Rs[:, mask_indices][:, 0, 0] t_out = ts[:, mask_indices][:, 0, 0] points3d_out = X[:, mask_indices][:, 0, 0] return R_out, t_out, points3d_out