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
