def laf_to_boundary_points(LAF: torch.Tensor, n_pts: int = 50) -> torch.Tensor:
"""
Converts LAFs to boundary points of the regions + center.
Used for local features visualization, see visualize_laf function
Args:
LAF: (torch.Tensor).
n_pts: number of points to output
Returns:
pts: (torch.Tensor) tensor of boundary points
Shape:
- Input: :math:`(B, N, 2, 3)`
- Output: :math:`(B, N, n_pts, 2)`
"""
raise_error_if_laf_is_not_valid(LAF)
B, N, _, _ = LAF.size()
pts = torch.cat([torch.sin(torch.linspace(0, 2 * math.pi, n_pts - 1)).unsqueeze(-1),
torch.cos(torch.linspace(0, 2 * math.pi, n_pts - 1)).unsqueeze(-1),
torch.ones(n_pts - 1, 1)], dim=1)
# Add origin to draw also the orientation
pts = torch.cat([torch.tensor([0., 0., 1.]).view(1, 3), pts], dim=0).unsqueeze(0).expand(B * N, n_pts, 3)
pts = pts.to(LAF.device).to(LAF.dtype)
aux = torch.tensor([0., 0., 1.]).view(1, 1, 3).expand(B * N, 1, 3)
HLAF = torch.cat([LAF.view(-1, 2, 3), aux.to(LAF.device).to(LAF.dtype)], dim=1)
pts_h = torch.bmm(HLAF, pts.permute(0, 2, 1)).permute(0, 2, 1)
return kornia.convert_points_from_homogeneous(pts_h.view(B, N, n_pts, 3))
def test_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., 0.5],
], [
[0., -0.5],
]],
device=device,
dtype=dtype)
# to euclidean
points = kornia.convert_points_from_homogeneous(points_h)
assert_allclose(points, expected, atol=1e-4, rtol=1e-4)
def triangulate_points(P1: torch.Tensor, P2: torch.Tensor,
points1: torch.Tensor,
points2: torch.Tensor) -> torch.Tensor:
r"""Reconstructs a bunch of points by triangulation.
Triangulates the 3d position of 2d correspondences between several images.
Reference: Internally it uses DLT method from Hartley/Zisserman 12.2 pag.312
The input points are assumed to be in homogeneous coordinate system and being inliers
correspondences. The method does not perform any robust estimation.
Args:
P1: The projection matrix for the first camera with shape :math:`(*, 3, 4)`.
P2: The projection matrix for the second camera with shape :math:`(*, 3, 4)`.
points1: The set of points seen from the first camera frame in the camera plane
coordinates with shape :math:`(*, N, 2)`.
points2: The set of points seen from the second camera frame in the camera plane
coordinates with shape :math:`(*, N, 2)`.
Returns:
The reconstructed 3d points in the world frame with shape :math:`(*, N, 3)`.
"""
if not (len(P1.shape) >= 2 and P1.shape[-2:] == (3, 4)):
raise AssertionError(P1.shape)
if not (len(P2.shape) >= 2 and P2.shape[-2:] == (3, 4)):
raise AssertionError(P2.shape)
if len(P1.shape[:-2]) != len(P2.shape[:-2]):
raise AssertionError(P1.shape, P2.shape)
if not (len(points1.shape) >= 2 and points1.shape[-1] == 2):
raise AssertionError(points1.shape)
if not (len(points2.shape) >= 2 and points2.shape[-1] == 2):
raise AssertionError(points2.shape)
if len(points1.shape[:-2]) != len(points2.shape[:-2]):
raise AssertionError(points1.shape, points2.shape)
if len(P1.shape[:-2]) != len(points1.shape[:-2]):
raise AssertionError(P1.shape, points1.shape)
# allocate and construct the equations matrix with shape (*, 4, 4)
points_shape = max(points1.shape,
points2.shape) # this allows broadcasting
X = torch.zeros(points_shape[:-1] + (4, 4)).type_as(points1)
for i in range(4):
X[..., 0, i] = points1[..., 0] * P1[..., 2:3, i] - P1[..., 0:1, i]
X[..., 1, i] = points1[..., 1] * P1[..., 2:3, i] - P1[..., 1:2, i]
X[..., 2, i] = points2[..., 0] * P2[..., 2:3, i] - P2[..., 0:1, i]
X[..., 3, i] = points2[..., 1] * P2[..., 2:3, i] - P2[..., 1:2, i]
# 1. Solve the system Ax=0 with smallest eigenvalue
# 2. Return homogeneous coordinates
_, _, V = torch.svd(X)
points3d_h = V[..., -1]
points3d: torch.Tensor = kornia.convert_points_from_homogeneous(points3d_h)
return points3d
def test_jit(self):
@torch.jit.script
def op_script(input):
return kornia.convert_points_from_homogeneous(input)
points_h = torch.zeros(1, 2, 3)
actual = op_script(points_h)
expected = kornia.convert_points_from_homogeneous(points_h)
assert_allclose(actual, expected)
def symmetric_transfer_error(pts1: torch.Tensor,
pts2: torch.Tensor,
H: torch.Tensor,
squared: bool = True,
eps: float = 1e-8) -> torch.Tensor:
r"""Return Symmetric transfer error for correspondences given the homography matrix.
Args:
pts1: correspondences from the left images with shape
(B, N, 2 or 3). If they are homogeneous, converted automatically.
pts2: correspondences from the right images with shape
(B, N, 2 or 3). If they are homogeneous, converted automatically.
H: Homographies with shape :math:`(B, 3, 3)`.
squared: if True (default), the squared distance is returned.
eps: Small constant for safe sqrt.
Returns:
the computed distance with shape :math:`(B, N)`.
"""
if not isinstance(H, torch.Tensor):
raise TypeError(f"H type is not a torch.Tensor. Got {type(H)}")
if (len(H.shape) != 3) or not H.shape[-2:] == (3, 3):
raise ValueError(f"H must be a (*, 3, 3) tensor. Got {H.shape}")
if pts1.size(-1) == 3:
pts1 = kornia.convert_points_from_homogeneous(pts1)
if pts2.size(-1) == 3:
pts2 = kornia.convert_points_from_homogeneous(pts2)
# From Hartley and Zisserman, Symmetric transfer error (4.7)
# dist = \sum_{i} (d(x, H^-1 x')**2 + d(x', Hx)**2)
there: torch.Tensor = oneway_transfer_error(pts1, pts2, H, True, eps)
back: torch.Tensor = oneway_transfer_error(pts2, pts1, torch.inverse(H),
True, eps)
out = there + back
if squared:
return out
return (out + eps).sqrt()
def forward(self, frame_id, point_id):
#indexing ## it is the same wiht torch where TODO
indexKF = torch.where(frame_id == self.idxKF)[1]
indexMP = torch.where(point_id == self.idxMP)[1]
# In test set, tKF is the Twc inverse. We may need to store something else in Memory
points = (self.tKF[indexKF]
@ self.tMPhomo[indexMP].unsqueeze(-1)).squeeze(-1)
Pc = kn.convert_points_from_homogeneous(points)
return kn.project_points(Pc, self.K)
def oneway_transfer_error(pts1: torch.Tensor,
pts2: torch.Tensor,
H: torch.Tensor,
squared: bool = True,
eps: float = 1e-8) -> torch.Tensor:
r"""Return transfer error in image 2 for correspondences given the homography matrix.
Args:
pts1: correspondences from the left images with shape
(B, N, 2 or 3). If they are homogeneous, converted automatically.
pts2: correspondences from the right images with shape
(B, N, 2 or 3). If they are homogeneous, converted automatically.
H: Homographies with shape :math:`(B, 3, 3)`.
squared: if True (default), the squared distance is returned.
eps: Small constant for safe sqrt.
Returns:
the computed distance with shape :math:`(B, N)`.
"""
if not isinstance(H, torch.Tensor):
raise TypeError(f"H type is not a torch.Tensor. Got {type(H)}")
if (len(H.shape) != 3) or not H.shape[-2:] == (3, 3):
raise ValueError(f"H must be a (*, 3, 3) tensor. Got {H.shape}")
if pts1.size(-1) == 3:
pts1 = kornia.convert_points_from_homogeneous(pts1)
if pts2.size(-1) == 3:
pts2 = kornia.convert_points_from_homogeneous(pts2)
# From Hartley and Zisserman, Error in one image (4.6)
# dist = \sum_{i} ( d(x', Hx)**2)
pts1_in_2: torch.Tensor = kornia.transform_points(H, pts1)
error_squared: torch.Tensor = (pts1_in_2 - pts2).pow(2).sum(dim=-1)
if squared:
return error_squared
return (error_squared + eps).sqrt()
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.FloatTensor([[
[2, 1, 0],
], [
[0, 1, 2],
], [
[0, 1, -2],
]]).to(torch.device(device_type))
expected = torch.FloatTensor([[
[2, 1],
], [
[0, 0.5],
], [
[0, -0.5],
]]).to(torch.device(device_type))
# to euclidean
points = kornia.convert_points_from_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.],
[0., 0.5],
[2., 1.],
[1., 2.],
[0., -0.5],
]).to(device)
# to euclidean
points = kornia.convert_points_from_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 test_cardinality(self, device, dtype, batch_shape):
points_h = torch.rand(batch_shape, device=device, dtype=dtype)
points = kornia.convert_points_from_homogeneous(points_h)
assert points.shape == points.shape[:-1] + (2, )
def op_script(input):
return kornia.convert_points_from_homogeneous(input)