def essential_from_Rt(R1: torch.Tensor, t1: torch.Tensor, R2: torch.Tensor,
t2: torch.Tensor) -> torch.Tensor:
r"""Get the Essential matrix from Camera motion (Rs and ts).
Reference: Hartley/Zisserman 9.6 pag 257 (formula 9.12)
Args:
R1 (torch.Tensor): The first camera rotation matrix with shape :math:`(*, 3, 3)`.
t1 (torch.Tensor): The first camera translation vector with shape :math:`(*, 3, 1)`.
R2 (torch.Tensor): The second camera rotation matrix with shape :math:`(*, 3, 3)`.
t2 (torch.Tensor): The second camera translation vector with shape :math:`(*, 3, 1)`.
Returns:
torch.Tensor: The Essential matrix with the shape :math:`(*, 3, 3)`.
"""
assert len(R1.shape) >= 2 and R1.shape[-2:] == (3, 3), R1.shape
assert len(t1.shape) >= 2 and t1.shape[-2:] == (3, 1), t1.shape
assert len(R2.shape) >= 2 and R2.shape[-2:] == (3, 3), R2.shape
assert len(t2.shape) >= 2 and t2.shape[-2:] == (3, 1), t2.shape
# first compute the camera relative motion
R, t = relative_camera_motion(R1, t1, R2, t2)
# get the cross product from relative translation vector
Tx = numeric.cross_product_matrix(t[..., 0])
return Tx @ R
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 decompose_essential_matrix(
E_mat: torch.Tensor
) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
r"""Decompose an essential matrix to possible rotations and translation.
This function decomposes the essential matrix E using svd decomposition [96]
and give the possible solutions: :math:`R1, R2, t`.
Args:
E_mat (torch.Tensor): The essential matrix in the form of :math:`(*, 3, 3)`.
Returns:
Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: A tuple containing the first and
second possible rotation matrices and the translation vector. The shape of the tensors
with be same input :math:`[(*, 3, 3), (*, 3, 3), (*, 3, 1)]`.
"""
assert len(E_mat.shape) >= 2 and E_mat.shape[-2:], E_mat.shape
# decompose matrix by its singular values
U, S, V = torch.svd(E_mat)
Vt = V.transpose(-2, -1)
mask = torch.ones_like(E_mat)
mask[..., -1:] *= -1.0 # fill last column with negative values
maskt = mask.transpose(-2, -1)
# avoid singularities
U = torch.where((torch.det(U) < 0.0)[..., None, None], U * mask, U)
Vt = torch.where((torch.det(Vt) < 0.0)[..., None, None], Vt * maskt, Vt)
W = numeric.cross_product_matrix(
torch.tensor([[0.0, 0.0, 1.0]]).type_as(E_mat))
W[..., 2, 2] += 1.0
# reconstruct rotations and retrieve translation vector
U_W_Vt = U @ W @ Vt
U_Wt_Vt = U @ W.transpose(-2, -1) @ Vt
# return values
R1 = U_W_Vt
R2 = U_Wt_Vt
T = U[..., -1:]
return (R1, R2, T)
def compute_symmetrical_epipolar_errors(data):
"""
Update:
data (dict):{"epi_errs": [M]}
"""
Tx = numeric.cross_product_matrix(data['T_0to1'][:, :3, 3])
E_mat = Tx @ data['T_0to1'][:, :3, :3]
m_bids = data['m_bids']
pts0 = data['mkpts0_f']
pts1 = data['mkpts1_f']
epi_errs = []
for bs in range(Tx.size(0)):
mask = m_bids == bs
epi_errs.append(
symmetric_epipolar_distance(pts0[mask], pts1[mask], E_mat[bs],
data['K0'][bs], data['K1'][bs]))
epi_errs = torch.cat(epi_errs, dim=0)
data.update({'epi_errs': epi_errs})