def _sym_epi_dist(F, X, Y, if_homo=False, clamp_at=None):
# Actually sauqred
if not if_homo:
X = utils_misc._homo(X)
Y = utils_misc._homo(Y)
if len(X.size()) == 2:
nominator = (torch.diag(Y @ F @ X.t()))**2
Fx1 = torch.mm(F, X.t())
Fx2 = torch.mm(F.t(), Y.t())
denom_recp = 1. / (Fx1[0]**2 + Fx1[1]**2) + 1. / (Fx2[0]**2 +
Fx2[1]**2)
else:
# print('-', X.detach().cpu().numpy())
# print('-', Y.detach().cpu().numpy())
# print('--', F.detach().cpu().numpy())
nominator = (torch.diagonal(Y @ F @ X.transpose(1, 2), dim1=1,
dim2=2))**2
Fx1 = torch.matmul(F, X.transpose(1, 2))
# print(Fx1.detach().cpu().numpy(), torch.max(Fx1), torch.sum(Fx1))
# print(X.detach().cpu().numpy(), torch.max(X), torch.sum(X))
Fx2 = torch.matmul(F.transpose(1, 2), Y.transpose(1, 2))
denom_recp = 1. / (Fx1[:, 0]**2 + Fx1[:, 1]**2 +
1e-10) + 1. / (Fx2[:, 0]**2 + Fx2[:, 1]**2 + 1e-10)
# print(nominator.size(), denom.size())
errors = nominator * denom_recp
# print('---', nominator.detach().cpu().numpy())
# print('---------', denom_recp.detach().cpu().numpy())
if clamp_at is not None:
errors = torch.clamp(errors, max=clamp_at)
return errors
def _normalize_XY(X, Y):
""" The Hartley normalization. Following https://github.com/marktao99/python/blob/da2682f8832483650b85b0be295ae7eaf179fcc5/CVP/samples/sfm.py#L157
corrected with https://www.mathworks.com/matlabcentral/fileexchange/27541-fundamental-matrix-computation
and https://en.wikipedia.org/wiki/Eight-point_algorithm#The_normalized_eight-point_algorithm """
if X.size()[0] != Y.size()[0]:
raise ValueError("Number of points don't match.")
X = utils_misc._homo(X)
mean_1 = torch.mean(X[:, :2], dim=0, keepdim=True)
S1 = np.sqrt(2) / torch.mean(torch.norm(X[:, :2] - mean_1, 2, dim=1))
# print(mean_1.size(), S1.size())
T1 = torch.tensor(
[[S1, 0, -S1 * mean_1[0, 0]], [0, S1, -S1 * mean_1[0, 1]], [0, 0, 1]],
device=X.device)
X_normalized = utils_misc._de_homo(torch.mm(
T1, X.t()).t()) # ideally zero mean (x, y), and sqrt(2) average norm
# xxx = X_normalized.numpy()
# print(np.mean(xxx, axis=0))
# print(np.mean(np.linalg.norm(xxx, 2, axis=1)))
Y = utils_misc._homo(Y)
mean_2 = torch.mean(Y[:, :2], dim=0, keepdim=True)
S2 = np.sqrt(2) / torch.mean(torch.norm(Y[:, :2] - mean_2, 2, dim=1))
T2 = torch.tensor(
[[S2, 0, -S2 * mean_2[0, 0]], [0, S2, -S2 * mean_2[0, 1]], [0, 0, 1]],
device=X.device)
Y_normalized = utils_misc._de_homo(torch.mm(T2, Y.t()).t())
return X_normalized, Y_normalized, T1, T2
def _sampson_dist(F, X, Y, if_homo=False):
if not if_homo:
X = utils_misc._homo(X)
Y = utils_misc._homo(Y)
if len(X.size()) == 2:
nominator = (torch.diag(Y @ F @ X.t()))**2
Fx1 = torch.mm(F, X.t())
Fx2 = torch.mm(F.t(), Y.t())
denom = Fx1[0]**2 + Fx1[1]**2 + Fx2[0]**2 + Fx2[1]**2
else:
nominator = (torch.diagonal(Y @ F @ X.transpose(1, 2), dim1=1,
dim2=2))**2
Fx1 = torch.matmul(F, X.transpose(1, 2))
Fx2 = torch.matmul(F.transpose(1, 2), Y.transpose(1, 2))
denom = Fx1[:, 0]**2 + Fx1[:, 1]**2 + Fx2[:, 0]**2 + Fx2[:, 1]**2
# print(nominator.size(), denom.size())
errors = nominator / denom
return errors
def _normalize_XY_batch(X, Y):
""" The Hartley normalization. Following https://github.com/marktao99/python/blob/da2682f8832483650b85b0be295ae7eaf179fcc5/CVP/samples/sfm.py#L157
corrected with https://www.mathworks.com/matlabcentral/fileexchange/27541-fundamental-matrix-computation
and https://en.wikipedia.org/wiki/Eight-point_algorithm#The_normalized_eight-point_algorithm """
# X: [batch_size, N, 2]
if X.size()[1] != Y.size()[1]:
raise ValueError("Number of points don't match.")
X = utils_misc._homo(X)
mean_1s = torch.mean(X[:, :, :2], dim=1, keepdim=True)
S1s = np.sqrt(2) / torch.mean(torch.norm(X[:, :, :2] - mean_1s, 2, dim=2),
dim=1)
T1_list = []
for S1, mean_1 in zip(S1s, mean_1s):
T1_list.append(
torch.tensor([[S1, 0, -S1 * mean_1[0, 0]],
[0, S1, -S1 * mean_1[0, 1]], [0, 0, 1]],
device=X.device))
T1s = torch.stack(T1_list)
X_normalized = utils_misc._de_homo(
torch.bmm(T1s, X.transpose(1, 2)).transpose(
1, 2)) # ideally zero mean (x, y), and sqrt(2) average norm
# xxx = X_normalized.numpy()
# print(np.mean(xxx, axis=0))
# print(np.mean(np.linalg.norm(xxx, 2, axis=1)))
Y = utils_misc._homo(Y)
mean_2s = torch.mean(Y[:, :, :2], dim=1, keepdim=True)
S2s = np.sqrt(2) / torch.mean(torch.norm(Y[:, :, :2] - mean_2s, 2, dim=2),
dim=1)
T2_list = []
for S2, mean_2 in zip(S2s, mean_2s):
T2_list.append(
torch.tensor([[S2, 0, -S2 * mean_2[0, 0]],
[0, S2, -S2 * mean_2[0, 1]], [0, 0, 1]],
device=X.device))
T2s = torch.stack(T2_list)
Y_normalized = utils_misc._de_homo(
torch.bmm(T2s, Y.transpose(1, 2)).transpose(1, 2))
return X_normalized, Y_normalized, T1s, T2s
def _epi_distance(F, X, Y, if_homo=False):
# Not squared. https://arxiv.org/pdf/1706.07886.pdf
if not if_homo:
X = utils_misc._homo(X)
Y = utils_misc._homo(Y)
if len(X.size()) == 2:
nominator = torch.diag(Y @ F @ X.t()).abs()
Fx1 = torch.mm(F, X.t())
Fx2 = torch.mm(F.t(), Y.t())
denom_recp_Y_to_FX = 1. / torch.sqrt(Fx1[0]**2 + Fx1[1]**2)
denom_recp_X_to_FY = 1. / torch.sqrt(Fx2[0]**2 + Fx2[1]**2)
else:
nominator = (torch.diagonal(Y @ F @ X.transpose(1, 2), dim1=1,
dim2=2)).abs()
Fx1 = torch.matmul(F, X.transpose(1, 2))
Fx2 = torch.matmul(F.transpose(1, 2), Y.transpose(1, 2))
denom_recp_Y_to_FX = 1. / torch.sqrt(Fx1[:, 0]**2 + Fx1[:, 1]**2)
denom_recp_X_to_FY = 1. / torch.sqrt(Fx2[:, 0]**2 + Fx2[:, 1]**2)
# print(nominator.size(), denom.size())
dist1 = nominator * denom_recp_Y_to_FX
dist2 = nominator * denom_recp_X_to_FY
return (dist1 + dist2) / 2., dist1, dist2
def _E_from_XY_batch(
X,
Y,
K,
W=None,
if_normzliedK=False,
normalize=True,
show_debug=False
): # Ref: https://github.com/marktao99/python/blob/master/CVP/samples/sfm.py#L55
from batch_svd import batch_svd # https://github.com/KinglittleQ/torch-batch-svd.git
""" Normalized Eight Point Algorithom for E: [Manmohan] In practice, one would transform the data points by K^{-1}, then do a Hartley normalization, then estimate the F matrix (which is now E matrix), then set the singular value conditions, then denormalize. Note that it's better to set singular values first, then denormalize.
X, Y: [N, 2] """
assert X.dtype == torch.float32, 'batch_svd currently only supports torch.float32!'
if if_normzliedK:
X_normalizedK = X.float()
Y_normalizedK = Y.float()
else:
X_normalizedK = utils_misc._de_homo(
torch.bmm(torch.inverse(K),
utils_misc._homo(X).transpose(1,
2)).transpose(1,
2)).float()
Y_normalizedK = utils_misc._de_homo(
torch.bmm(torch.inverse(K),
utils_misc._homo(Y).transpose(1,
2)).transpose(1,
2)).float()
# assert normalize==False, 'Not supported in batch mode yet!'
if normalize:
X, Y, T1, T2 = _normalize_XY_batch(X_normalizedK, Y_normalizedK)
else:
X, Y = X_normalizedK, Y_normalizedK
# print(T1)
# print(T2)
# print(X)
xx = torch.cat([X, Y], dim=2)
XX = torch.stack([
xx[:, :, 2] * xx[:, :, 0], xx[:, :, 2] * xx[:, :, 1], xx[:, :, 2],
xx[:, :, 3] * xx[:, :, 0], xx[:, :, 3] * xx[:, :, 1], xx[:, :, 3],
xx[:, :, 0], xx[:, :, 1],
torch.ones_like(xx[:, :, 0])
],
dim=2)
if W is not None:
XX = torch.bmm(W, XX)
# U, D, V = torch.svd(XX, some=False)
# print(XX[0, :2])
# print(XX.size())
# U, D, V = batch_svd(XX)
V_list = []
for XX_single in XX:
_, _, V_single = torch.svd(XX_single, some=True)
V_list.append(V_single[:, -1])
V_last_col = torch.stack(V_list)
# print(V_last_col.size(), '----')
if show_debug:
print('[info.Debug @_E_from_XY] Singualr values of XX:\n',
D[0].numpy())
# F_recover = torch.reshape(V[:, :, -1], (-1, 3, 3))
F_recover = V_last_col.view(-1, 3, 3)
# FU, FD, FV= torch.svd(F_recover, some=False)
FU, FD, FV = batch_svd(F_recover)
if show_debug:
print('[info.Debug @_E_from_XY] Singular values for recovered E(F):\n',
FD[0].numpy())
# FDnew = torch.diag(FD);
# FDnew[2, 2] = 0;
# F_recover_sing = torch.mm(FU, torch.mm(FDnew, FV.t()))
S_110 = torch.diag(
torch.tensor([1., 1., 0.], dtype=FU.dtype,
device=FU.device)).unsqueeze(0).expand(
FV.size()[0], -1, -1)
E_recover_110 = torch.bmm(FU, torch.bmm(S_110, FV.transpose(1, 2)))
# F_recover_sing_rescale = F_recover_sing / torch.norm(F_recover_sing) * torch.norm(F)
# print(E_recover_110)
if normalize:
E_recover_110 = torch.bmm(T2.transpose(1, 2),
torch.bmm(E_recover_110, T1))
return -E_recover_110
def _E_from_XY(
X,
Y,
K,
W=None,
if_normzliedK=False,
normalize=True,
show_debug=False
): # Ref: https://github.com/marktao99/python/blob/master/CVP/samples/sfm.py#L55
""" Normalized Eight Point Algorithom for E: [Manmohan] In practice, one would transform the data points by K^{-1}, then do a Hartley normalization, then estimate the F matrix (which is now E matrix), then set the singular value conditions, then denormalize. Note that it's better to set singular values first, then denormalize.
X, Y: [N, 2] """
if if_normzliedK:
X_normalizedK = X
Y_normalizedK = Y
else:
X_normalizedK = utils_misc._de_homo(
torch.mm(torch.inverse(K),
utils_misc._homo(X).t()).t())
Y_normalizedK = utils_misc._de_homo(
torch.mm(torch.inverse(K),
utils_misc._homo(Y).t()).t())
if normalize:
X, Y, T1, T2 = _normalize_XY(X_normalizedK, Y_normalizedK)
else:
X, Y = X_normalizedK, Y_normalizedK
# print(T1)
# print(T2)
# print(X)
xx = torch.cat([X.t(), Y.t()], dim=0)
XX = torch.stack([
xx[2, :] * xx[0, :], xx[2, :] * xx[1, :], xx[2, :],
xx[3, :] * xx[0, :], xx[3, :] * xx[1, :], xx[3, :], xx[0, :], xx[1, :],
torch.ones_like(xx[0, :])
],
dim=0).t() # [N, 9]
# print(XX.size())
if W is not None:
XX = torch.mm(W, XX) # [N, 9]
# print(XX[:2])
U, D, V = torch.svd(XX, some=True)
if show_debug:
print('[info.Debug @_E_from_XY] Singualr values of XX:\n', D.numpy())
# U_np, D_np, V_np = np.linalg.svd(XX.numpy())
F_recover = torch.reshape(V[:, -1], (3, 3))
# print('-', F_recover)
FU, FD, FV = torch.svd(F_recover, some=True)
if show_debug:
print('[info.Debug @_E_from_XY] Singular values for recovered E(F):\n',
FD.numpy())
# FDnew = torch.diag(FD);
# FDnew[2, 2] = 0;
# F_recover_sing = torch.mm(FU, torch.mm(FDnew, FV.t()))
S_110 = torch.diag(
torch.tensor([1., 1., 0.], dtype=FU.dtype, device=FU.device))
E_recover_110 = torch.mm(FU, torch.mm(S_110, FV.t()))
# F_recover_sing_rescale = F_recover_sing / torch.norm(F_recover_sing) * torch.norm(F)
# print(E_recover_110)
if normalize:
E_recover_110 = torch.mm(T2.t(), torch.mm(E_recover_110, T1))
return E_recover_110