def symmetrical_epipolar_distance(pts1: torch.Tensor,
pts2: torch.Tensor,
Fm: torch.Tensor,
squared: bool = True,
eps: float = 1e-8) -> torch.Tensor:
r"""Returns symmetrical epipolar distance for correspondences given the fundamental matrix.
Args:
pts1 (torch.Tensor): correspondences from the left images with shape
(B, N, 2 or 3). If they are not homogeneous, converted automatically.
pts2 (torch.Tensor): correspondences from the right images with shape
(B, N, 2 or 3). If they are not homogeneous, converted automatically.
Fm (torch.Tensor): Fundamental matrices with shape :math:`(B, 3, 3)`. Called Fm to
avoid ambiguity with torch.nn.functional.
squared (bool): if True (default), the squared distance is returned.
eps (float): Small constant for safe sqrt. Default 1e-9.
Returns:
torch.Tensor: the computed Symmetrical distance with shape :math:`(B, N)`.
"""
if not isinstance(Fm, torch.Tensor):
raise TypeError("Fm type is not a torch.Tensor. Got {}".format(
type(Fm)))
if (len(Fm.shape) != 3) or not Fm.shape[-2:] == (3, 3):
raise ValueError("Fm must be a (*, 3, 3) tensor. Got {}".format(
Fm.shape))
if pts1.size(-1) == 2:
pts1 = kornia.convert_points_to_homogeneous(pts1)
if pts2.size(-1) == 2:
pts2 = kornia.convert_points_to_homogeneous(pts2)
# From Hartley and Zisserman, symmetric epipolar distance (11.10)
# sed = (x'^T F x) ** 2 / (((Fx)_1**2) + (Fx)_2**2)) + 1/ (((F^Tx')_1**2) + (F^Tx')_2**2))
# line1_in_2: torch.Tensor = (F @ pts1.permute(0,2,1)).permute(0,2,1)
# line2_in_1: torch.Tensor = (F.permute(0,2,1) @ pts2.permute(0,2,1)).permute(0,2,1)
# Instead we can just transpose F once and switch the order of multiplication
F_t: torch.Tensor = Fm.permute(0, 2, 1)
line1_in_2: torch.Tensor = pts1 @ F_t
line2_in_1: torch.Tensor = pts2 @ Fm
# numerator = (x'^T F x) ** 2
numerator: torch.Tensor = (pts2 * line1_in_2).sum(2).pow(2)
# denominator_inv = 1/ (((Fx)_1**2) + (Fx)_2**2)) + 1/ (((F^Tx')_1**2) + (F^Tx')_2**2))
denominator_inv: torch.Tensor = (
1. / (line1_in_2[..., :2].norm(2, dim=2).pow(2)) + 1. /
(line2_in_1[..., :2].norm(2, dim=2).pow(2)))
out: torch.Tensor = numerator * denominator_inv
if squared:
return out
return (out + eps).sqrt()
def compute_correspond_epilines(points: torch.Tensor,
F_mat: torch.Tensor) -> torch.Tensor:
r"""Computes the corresponding epipolar line for a given set of points.
Args:
points: tensor containing the set of points to project in the shape of :math:`(B, N, 2)`.
F_mat: the fundamental to use for projection the points in the shape of :math:`(B, 3, 3)`.
Returns:
a tensor with shape :math:`(B, N, 3)` containing a vector of the epipolar
lines corresponding to the points to the other image. Each line is described as
:math:`ax + by + c = 0` and encoding the vectors as :math:`(a, b, c)`.
"""
assert len(points.shape) == 3 and points.shape[2] == 2, points.shape
assert len(F_mat.shape) == 3 and F_mat.shape[-2:] == (3, 3), F_mat.shape
points_h: torch.Tensor = kornia.convert_points_to_homogeneous(points)
# project points and retrieve lines components
a, b, c = torch.chunk(F_mat @ points_h.permute(0, 2, 1), dim=1, chunks=3)
# compute normal and compose equation line
nu: torch.Tensor = a * a + b * b
nu = torch.where(nu > 0.0, 1.0 / torch.sqrt(nu), torch.ones_like(nu))
line = torch.cat([a * nu, b * nu, c * nu], dim=1) # Bx3xN
return line.permute(0, 2, 1) # BxNx3
def test_convert_points_batch(self, device, dtype):
# generate input data
points_h = torch.tensor([[
[2., 1., 0.],
], [
[0., 1., 2.],
], [
[0., 1., -2.],
]],
device=device,
dtype=dtype)
expected = torch.tensor([[
[2., 1., 0., 1.],
], [
[0., 1., 2., 1.],
], [
[0., 1., -2., 1.],
]],
device=device,
dtype=dtype)
# to euclidean
points = kornia.convert_points_to_homogeneous(points_h)
assert_allclose(points, expected, atol=1e-4, rtol=1e-4)
def test_jit(self):
@torch.jit.script
def op_script(input):
return kornia.convert_points_to_homogeneous(input)
points_h = torch.zeros(1, 2, 3)
actual = op_script(points_h)
expected = kornia.convert_points_to_homogeneous(points_h)
assert_allclose(actual, expected)
def __init__(self, MPs, KFs, K):
super().__init__()
self.K = K
self.tMP = nn.Parameter(MPs[:, 1:])
self.tKF = nn.Parameter(KFs[:, 1:].view(-1, 4, 4))
# indexing the matches of key frames and Map Point
self.idxMP = MPs[:, 0].type(torch.int)
self.idxKF = KFs[:, 0].type(torch.int)
self.tMPhomo = kn.convert_points_to_homogeneous(self.tMP)
def test_convert_points_to_homogeneous(batch_shape, device_type):
# generate input data
points = torch.rand(batch_shape)
points = points.to(torch.device(device_type))
# to homogeneous
points_h = kornia.convert_points_to_homogeneous(points)
assert points_h.shape[-2] == batch_shape[-2]
assert (points_h[..., -1] == torch.ones(points_h[..., -1].shape)).all()
# evaluate function gradient
points = utils.tensor_to_gradcheck_var(points) # to var
assert gradcheck(kornia.convert_points_to_homogeneous, (points,),
raise_exception=True)
def get_shifts_from_sky_hom(hom, horizon_line):
hom = torch.from_numpy(hom).float().unsqueeze(0)
horizon_line = horizon_line * 2 - 1
points_src = torch.tensor([[
[-1, -1], # left top
[1, -1], # right top
[-1, horizon_line], # left bottom
[1, horizon_line]
]]).float() # right bottom
points_src_hom = convert_points_to_homogeneous(points_src)
points_tgt_hom = (hom @ points_src_hom.transpose(1, 2)).transpose(1, 2)
points_tgt = convert_points_from_homogeneous(points_tgt_hom)
shifts_flat = points_tgt - points_src
shifts = shifts_flat.view(2, 2, 2)
return shifts.numpy()
def test_convert_points_batch(self, device_type):
# generate input data
points_h = torch.tensor([[
[2., 1., 0.],
], [
[0., 1., 2.],
], [
[0., 1., -2.],
]]).to(torch.device(device_type))
expected = torch.tensor([[
[2., 1., 0., 1.],
], [
[0., 1., 2., 1.],
], [
[0., 1., -2., 1.],
]]).to(torch.device(device_type))
# to euclidean
points = kornia.convert_points_to_homogeneous(points_h)
assert_allclose(points, expected)
def test_convert_points(self, device):
# generate input data
points_h = torch.tensor([
[1., 2., 1.],
[0., 1., 2.],
[2., 1., 0.],
[-1., -2., -1.],
[0., 1., -2.],
]).to(device)
expected = torch.tensor([
[1., 2., 1., 1.],
[0., 1., 2., 1.],
[2., 1., 0., 1.],
[-1., -2., -1., 1.],
[0., 1., -2., 1.],
]).to(device)
# to euclidean
points = kornia.convert_points_to_homogeneous(points_h)
assert_allclose(points, expected)
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 = kornia.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 = kornia.convert_points_from_homogeneous(points_t)
points_depth = points_t[..., -1] - project[..., 2, 3]
return points_img, points_depth
def get_id_grid(height, width):
grid = create_meshgrid(height, width,
normalized_coordinates=False) # 1xHxWx2
return convert_points_to_homogeneous(grid)
def op_script(input):
return kornia.convert_points_to_homogeneous(input)
