File size: 2,850 Bytes
36c95ba |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 |
"""Module with the functionalites for triangulation."""
import torch
from kornia.geometry.conversions import convert_points_from_homogeneous
# https://github.com/opencv/opencv_contrib/blob/master/modules/sfm/src/triangulation.cpp#L68
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 = convert_points_from_homogeneous(points3d_h)
return points3d
|