import enum
import warnings
from typing import Optional, Tuple
import torch
import torch.nn.functional as F
from kornia.constants import pi
from kornia.core import Tensor, concatenate, pad, stack, tensor, where
from kornia.testing import KORNIA_CHECK, KORNIA_CHECK_SHAPE
from kornia.utils.helpers import _torch_inverse_cast
from kornia.utils.misc import eye_like
__all__ = [
"rad2deg",
"deg2rad",
"pol2cart",
"cart2pol",
"convert_points_from_homogeneous",
"convert_points_to_homogeneous",
"convert_affinematrix_to_homography",
"convert_affinematrix_to_homography3d",
"angle_axis_to_rotation_matrix",
"angle_axis_to_quaternion",
"rotation_matrix_to_angle_axis",
"rotation_matrix_to_quaternion",
"quaternion_to_angle_axis",
"quaternion_to_rotation_matrix",
"quaternion_log_to_exp",
"quaternion_exp_to_log",
"quaternion_from_euler",
"euler_from_quaternion",
"denormalize_pixel_coordinates",
"normalize_pixel_coordinates",
"normalize_quaternion",
"denormalize_pixel_coordinates3d",
"normalize_pixel_coordinates3d",
"angle_to_rotation_matrix",
"normalize_homography",
"denormalize_homography",
"normalize_homography3d",
"normal_transform_pixel",
"normal_transform_pixel3d",
"normalize_points_with_intrinsics",
"denormalize_points_with_intrinsics",
"worldtocam_to_camtoworld_Rt",
"camtoworld_to_worldtocam_Rt",
"Rt_to_matrix4x4",
"matrix4x4_to_Rt",
"camtoworld_graphics_to_vision_4x4",
"camtoworld_vision_to_graphics_4x4",
"camtoworld_graphics_to_vision_Rt",
"camtoworld_vision_to_graphics_Rt",
"ARKitQTVecs_to_ColmapQTVecs",
]
class QuaternionCoeffOrder(enum.Enum):
XYZW = 'xyzw'
WXYZ = 'wxyz'
[docs]def rad2deg(tensor: Tensor) -> Tensor:
r"""Function that converts angles from radians to degrees.
Args:
tensor: Tensor of arbitrary shape.
Returns:
Tensor with same shape as input.
Example:
>>> input = tensor(3.1415926535)
>>> rad2deg(input)
tensor(180.)
"""
if not isinstance(tensor, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(tensor)}")
return 180.0 * tensor / pi.to(tensor.device).type(tensor.dtype)
[docs]def deg2rad(tensor: Tensor) -> Tensor:
r"""Function that converts angles from degrees to radians.
Args:
tensor: Tensor of arbitrary shape.
Returns:
tensor with same shape as input.
Examples:
>>> input = tensor(180.)
>>> deg2rad(input)
tensor(3.1416)
"""
if not isinstance(tensor, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(tensor)}")
return tensor * pi.to(tensor.device).type(tensor.dtype) / 180.0
[docs]def pol2cart(rho: Tensor, phi: Tensor) -> Tuple[Tensor, Tensor]:
r"""Function that converts polar coordinates to cartesian coordinates.
Args:
rho: Tensor of arbitrary shape.
phi: Tensor of same arbitrary shape.
Returns:
- x: Tensor with same shape as input.
- y: Tensor with same shape as input.
Example:
>>> rho = torch.rand(1, 3, 3)
>>> phi = torch.rand(1, 3, 3)
>>> x, y = pol2cart(rho, phi)
"""
if not (isinstance(rho, Tensor) & isinstance(phi, Tensor)):
raise TypeError(f"Input type is not a Tensor. Got {type(rho)}, {type(phi)}")
x = rho * torch.cos(phi)
y = rho * torch.sin(phi)
return x, y
[docs]def cart2pol(x: Tensor, y: Tensor, eps: float = 1.0e-8) -> Tuple[Tensor, Tensor]:
"""Function that converts cartesian coordinates to polar coordinates.
Args:
x: Tensor of arbitrary shape.
y: Tensor of same arbitrary shape.
eps: To avoid division by zero.
Returns:
- rho: Tensor with same shape as input.
- phi: Tensor with same shape as input.
Example:
>>> x = torch.rand(1, 3, 3)
>>> y = torch.rand(1, 3, 3)
>>> rho, phi = cart2pol(x, y)
"""
if not (isinstance(x, Tensor) & isinstance(y, Tensor)):
raise TypeError(f"Input type is not a Tensor. Got {type(x)}, {type(y)}")
rho = torch.sqrt(x**2 + y**2 + eps)
phi = torch.atan2(y, x)
return rho, phi
[docs]def convert_points_from_homogeneous(points: Tensor, eps: float = 1e-8) -> Tensor:
r"""Function that converts points from homogeneous to Euclidean space.
Args:
points: the points to be transformed of shape :math:`(B, N, D)`.
eps: to avoid division by zero.
Returns:
the points in Euclidean space :math:`(B, N, D-1)`.
Examples:
>>> input = tensor([[0., 0., 1.]])
>>> convert_points_from_homogeneous(input)
tensor([[0., 0.]])
"""
if not isinstance(points, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(points)}")
if len(points.shape) < 2:
raise ValueError(f"Input must be at least a 2D tensor. Got {points.shape}")
# we check for points at max_val
z_vec: Tensor = points[..., -1:]
# set the results of division by zeror/near-zero to 1.0
# follow the convention of opencv:
# https://github.com/opencv/opencv/pull/14411/files
mask: Tensor = torch.abs(z_vec) > eps
scale = where(mask, 1.0 / (z_vec + eps), torch.ones_like(z_vec))
return scale * points[..., :-1]
[docs]def convert_points_to_homogeneous(points: Tensor) -> Tensor:
r"""Function that converts points from Euclidean to homogeneous space.
Args:
points: the points to be transformed with shape :math:`(*, N, D)`.
Returns:
the points in homogeneous coordinates :math:`(*, N, D+1)`.
Examples:
>>> input = tensor([[0., 0.]])
>>> convert_points_to_homogeneous(input)
tensor([[0., 0., 1.]])
"""
if not isinstance(points, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(points)}")
if len(points.shape) < 2:
raise ValueError(f"Input must be at least a 2D tensor. Got {points.shape}")
return pad(points, [0, 1], "constant", 1.0)
def _convert_affinematrix_to_homography_impl(A: Tensor) -> Tensor:
H: Tensor = pad(A, [0, 0, 0, 1], "constant", value=0.0)
H[..., -1, -1] += 1.0
return H
[docs]def convert_affinematrix_to_homography(A: Tensor) -> Tensor:
r"""Function that converts batch of affine matrices.
Args:
A: the affine matrix with shape :math:`(B,2,3)`.
Returns:
the homography matrix with shape of :math:`(B,3,3)`.
Examples:
>>> A = tensor([[[1., 0., 0.],
... [0., 1., 0.]]])
>>> convert_affinematrix_to_homography(A)
tensor([[[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]])
"""
if not isinstance(A, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(A)}")
if not (len(A.shape) == 3 and A.shape[-2:] == (2, 3)):
raise ValueError(f"Input matrix must be a Bx2x3 tensor. Got {A.shape}")
return _convert_affinematrix_to_homography_impl(A)
def convert_affinematrix_to_homography3d(A: Tensor) -> Tensor:
r"""Function that converts batch of 3d affine matrices.
Args:
A: the affine matrix with shape :math:`(B,3,4)`.
Returns:
the homography matrix with shape of :math:`(B,4,4)`.
Examples:
>>> A = tensor([[[1., 0., 0., 0.],
... [0., 1., 0., 0.],
... [0., 0., 1., 0.]]])
>>> convert_affinematrix_to_homography3d(A)
tensor([[[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]])
"""
if not isinstance(A, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(A)}")
if not (len(A.shape) == 3 and A.shape[-2:] == (3, 4)):
raise ValueError(f"Input matrix must be a Bx3x4 tensor. Got {A.shape}")
return _convert_affinematrix_to_homography_impl(A)
[docs]def angle_axis_to_rotation_matrix(angle_axis: Tensor) -> Tensor:
r"""Convert 3d vector of axis-angle rotation to 3x3 rotation matrix.
Args:
angle_axis: tensor of 3d vector of axis-angle rotations in radians with shape :math:`(N, 3)`.
Returns:
tensor of rotation matrices of shape :math:`(N, 3, 3)`.
Example:
>>> input = tensor([[0., 0., 0.]])
>>> angle_axis_to_rotation_matrix(input)
tensor([[[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]])
>>> input = tensor([[1.5708, 0., 0.]])
>>> angle_axis_to_rotation_matrix(input)
tensor([[[ 1.0000e+00, 0.0000e+00, 0.0000e+00],
[ 0.0000e+00, -3.6200e-06, -1.0000e+00],
[ 0.0000e+00, 1.0000e+00, -3.6200e-06]]])
"""
if not isinstance(angle_axis, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(angle_axis)}")
if not angle_axis.shape[-1] == 3:
raise ValueError(f"Input size must be a (*, 3) tensor. Got {angle_axis.shape}")
def _compute_rotation_matrix(angle_axis, theta2, eps=1e-6):
# We want to be careful to only evaluate the square root if the
# norm of the angle_axis vector is greater than zero. Otherwise
# we get a division by zero.
k_one = 1.0
theta = torch.sqrt(theta2)
wxyz = angle_axis / (theta + eps)
wx, wy, wz = torch.chunk(wxyz, 3, dim=1)
cos_theta = torch.cos(theta)
sin_theta = torch.sin(theta)
r00 = cos_theta + wx * wx * (k_one - cos_theta)
r10 = wz * sin_theta + wx * wy * (k_one - cos_theta)
r20 = -wy * sin_theta + wx * wz * (k_one - cos_theta)
r01 = wx * wy * (k_one - cos_theta) - wz * sin_theta
r11 = cos_theta + wy * wy * (k_one - cos_theta)
r21 = wx * sin_theta + wy * wz * (k_one - cos_theta)
r02 = wy * sin_theta + wx * wz * (k_one - cos_theta)
r12 = -wx * sin_theta + wy * wz * (k_one - cos_theta)
r22 = cos_theta + wz * wz * (k_one - cos_theta)
rotation_matrix = concatenate([r00, r01, r02, r10, r11, r12, r20, r21, r22], dim=1)
return rotation_matrix.view(-1, 3, 3)
def _compute_rotation_matrix_taylor(angle_axis):
rx, ry, rz = torch.chunk(angle_axis, 3, dim=1)
k_one = torch.ones_like(rx)
rotation_matrix = concatenate([k_one, -rz, ry, rz, k_one, -rx, -ry, rx, k_one], dim=1)
return rotation_matrix.view(-1, 3, 3)
# stolen from ceres/rotation.h
_angle_axis = torch.unsqueeze(angle_axis, dim=1)
theta2 = torch.matmul(_angle_axis, _angle_axis.transpose(1, 2))
theta2 = torch.squeeze(theta2, dim=1)
# compute rotation matrices
rotation_matrix_normal = _compute_rotation_matrix(angle_axis, theta2)
rotation_matrix_taylor = _compute_rotation_matrix_taylor(angle_axis)
# create mask to handle both cases
eps = 1e-6
mask = (theta2 > eps).view(-1, 1, 1).to(theta2.device)
mask_pos = (mask).type_as(theta2)
mask_neg = (~mask).type_as(theta2)
# create output pose matrix
rotation_matrix = eye_like(3, angle_axis, shared_memory=False)
# fill output matrix with masked values
rotation_matrix[..., :3, :3] = mask_pos * rotation_matrix_normal + mask_neg * rotation_matrix_taylor
return rotation_matrix # Nx3x3
[docs]def rotation_matrix_to_angle_axis(rotation_matrix: Tensor) -> Tensor:
r"""Convert 3x3 rotation matrix to Rodrigues vector in radians.
Args:
rotation_matrix: rotation matrix of shape :math:`(N, 3, 3)`.
Returns:
Rodrigues vector transformation of shape :math:`(N, 3)`.
Example:
>>> input = tensor([[1., 0., 0.],
... [0., 1., 0.],
... [0., 0., 1.]])
>>> rotation_matrix_to_angle_axis(input)
tensor([0., 0., 0.])
>>> input = tensor([[1., 0., 0.],
... [0., 0., -1.],
... [0., 1., 0.]])
>>> rotation_matrix_to_angle_axis(input)
tensor([1.5708, 0.0000, 0.0000])
"""
if not isinstance(rotation_matrix, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(rotation_matrix)}")
if not rotation_matrix.shape[-2:] == (3, 3):
raise ValueError(f"Input size must be a (*, 3, 3) tensor. Got {rotation_matrix.shape}")
quaternion: Tensor = rotation_matrix_to_quaternion(rotation_matrix, order=QuaternionCoeffOrder.WXYZ)
return quaternion_to_angle_axis(quaternion, order=QuaternionCoeffOrder.WXYZ)
[docs]def rotation_matrix_to_quaternion(
rotation_matrix: Tensor, eps: float = 1.0e-8, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW
) -> Tensor:
r"""Convert 3x3 rotation matrix to 4d quaternion vector.
The quaternion vector has components in (w, x, y, z) or (x, y, z, w) format.
.. note::
The (x, y, z, w) order is going to be deprecated in favor of efficiency.
Args:
rotation_matrix: the rotation matrix to convert with shape :math:`(*, 3, 3)`.
eps: small value to avoid zero division.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
the rotation in quaternion with shape :math:`(*, 4)`.
Example:
>>> input = tensor([[1., 0., 0.],
... [0., 1., 0.],
... [0., 0., 1.]])
>>> rotation_matrix_to_quaternion(input, eps=torch.finfo(input.dtype).eps,
... order=QuaternionCoeffOrder.WXYZ)
tensor([1., 0., 0., 0.])
"""
if not isinstance(rotation_matrix, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(rotation_matrix)}")
if not rotation_matrix.shape[-2:] == (3, 3):
raise ValueError(f"Input size must be a (*, 3, 3) tensor. Got {rotation_matrix.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
def safe_zero_division(numerator: Tensor, denominator: Tensor) -> Tensor:
eps: float = torch.finfo(numerator.dtype).tiny
return numerator / torch.clamp(denominator, min=eps)
rotation_matrix_vec: Tensor = rotation_matrix.view(*rotation_matrix.shape[:-2], 9)
m00, m01, m02, m10, m11, m12, m20, m21, m22 = torch.chunk(rotation_matrix_vec, chunks=9, dim=-1)
trace: Tensor = m00 + m11 + m22
def trace_positive_cond():
sq = torch.sqrt(trace + 1.0 + eps) * 2.0 # sq = 4 * qw.
qw = 0.25 * sq
qx = safe_zero_division(m21 - m12, sq)
qy = safe_zero_division(m02 - m20, sq)
qz = safe_zero_division(m10 - m01, sq)
if order == QuaternionCoeffOrder.XYZW:
return concatenate((qx, qy, qz, qw), dim=-1)
return concatenate((qw, qx, qy, qz), dim=-1)
def cond_1():
sq = torch.sqrt(1.0 + m00 - m11 - m22 + eps) * 2.0 # sq = 4 * qx.
qw = safe_zero_division(m21 - m12, sq)
qx = 0.25 * sq
qy = safe_zero_division(m01 + m10, sq)
qz = safe_zero_division(m02 + m20, sq)
if order == QuaternionCoeffOrder.XYZW:
return concatenate((qx, qy, qz, qw), dim=-1)
return concatenate((qw, qx, qy, qz), dim=-1)
def cond_2():
sq = torch.sqrt(1.0 + m11 - m00 - m22 + eps) * 2.0 # sq = 4 * qy.
qw = safe_zero_division(m02 - m20, sq)
qx = safe_zero_division(m01 + m10, sq)
qy = 0.25 * sq
qz = safe_zero_division(m12 + m21, sq)
if order == QuaternionCoeffOrder.XYZW:
return concatenate((qx, qy, qz, qw), dim=-1)
return concatenate((qw, qx, qy, qz), dim=-1)
def cond_3():
sq = torch.sqrt(1.0 + m22 - m00 - m11 + eps) * 2.0 # sq = 4 * qz.
qw = safe_zero_division(m10 - m01, sq)
qx = safe_zero_division(m02 + m20, sq)
qy = safe_zero_division(m12 + m21, sq)
qz = 0.25 * sq
if order == QuaternionCoeffOrder.XYZW:
return concatenate((qx, qy, qz, qw), dim=-1)
return concatenate((qw, qx, qy, qz), dim=-1)
where_2 = where(m11 > m22, cond_2(), cond_3())
where_1 = where((m00 > m11) & (m00 > m22), cond_1(), where_2)
quaternion: Tensor = where(trace > 0.0, trace_positive_cond(), where_1)
return quaternion
[docs]def normalize_quaternion(quaternion: Tensor, eps: float = 1.0e-12) -> Tensor:
r"""Normalize a quaternion.
The quaternion should be in (x, y, z, w) or (w, x, y, z) format.
Args:
quaternion: a tensor containing a quaternion to be normalized.
The tensor can be of shape :math:`(*, 4)`.
eps: small value to avoid division by zero.
Return:
the normalized quaternion of shape :math:`(*, 4)`.
Example:
>>> quaternion = tensor((1., 0., 1., 0.))
>>> normalize_quaternion(quaternion)
tensor([0.7071, 0.0000, 0.7071, 0.0000])
"""
if not isinstance(quaternion, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(quaternion)}")
if not quaternion.shape[-1] == 4:
raise ValueError(f"Input must be a tensor of shape (*, 4). Got {quaternion.shape}")
return F.normalize(quaternion, p=2.0, dim=-1, eps=eps)
# based on:
# https://github.com/matthew-brett/transforms3d/blob/8965c48401d9e8e66b6a8c37c65f2fc200a076fa/transforms3d/quaternions.py#L101
# https://github.com/tensorflow/graphics/blob/master/tensorflow_graphics/geometry/transformation/rotation_matrix_3d.py#L247
[docs]def quaternion_to_rotation_matrix(
quaternion: Tensor, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW
) -> Tensor:
r"""Convert a quaternion to a rotation matrix.
The quaternion should be in (x, y, z, w) or (w, x, y, z) format.
Args:
quaternion: a tensor containing a quaternion to be converted.
The tensor can be of shape :math:`(*, 4)`.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
the rotation matrix of shape :math:`(*, 3, 3)`.
Example:
>>> quaternion = tensor((0., 0., 0., 1.))
>>> quaternion_to_rotation_matrix(quaternion, order=QuaternionCoeffOrder.WXYZ)
tensor([[-1., 0., 0.],
[ 0., -1., 0.],
[ 0., 0., 1.]])
"""
if not isinstance(quaternion, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(quaternion)}")
if not quaternion.shape[-1] == 4:
raise ValueError(f"Input must be a tensor of shape (*, 4). Got {quaternion.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
# normalize the input quaternion
quaternion_norm: Tensor = normalize_quaternion(quaternion)
# unpack the normalized quaternion components
if order == QuaternionCoeffOrder.XYZW:
x, y, z, w = torch.chunk(quaternion_norm, chunks=4, dim=-1)
else:
w, x, y, z = torch.chunk(quaternion_norm, chunks=4, dim=-1)
# compute the actual conversion
tx: Tensor = 2.0 * x
ty: Tensor = 2.0 * y
tz: Tensor = 2.0 * z
twx: Tensor = tx * w
twy: Tensor = ty * w
twz: Tensor = tz * w
txx: Tensor = tx * x
txy: Tensor = ty * x
txz: Tensor = tz * x
tyy: Tensor = ty * y
tyz: Tensor = tz * y
tzz: Tensor = tz * z
one: Tensor = tensor(1.0)
matrix: Tensor = stack(
(
one - (tyy + tzz),
txy - twz,
txz + twy,
txy + twz,
one - (txx + tzz),
tyz - twx,
txz - twy,
tyz + twx,
one - (txx + tyy),
),
dim=-1,
).view(-1, 3, 3)
if len(quaternion.shape) == 1:
matrix = torch.squeeze(matrix, dim=0)
return matrix
[docs]def quaternion_to_angle_axis(quaternion: Tensor, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW) -> Tensor:
"""Convert quaternion vector to angle axis of rotation in radians.
The quaternion should be in (x, y, z, w) or (w, x, y, z) format.
Adapted from ceres C++ library: ceres-solver/include/ceres/rotation.h
Args:
quaternion: tensor with quaternions.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
tensor with angle axis of rotation.
Shape:
- Input: :math:`(*, 4)` where `*` means, any number of dimensions
- Output: :math:`(*, 3)`
Example:
>>> quaternion = tensor((1., 0., 0., 0.))
>>> quaternion_to_angle_axis(quaternion)
tensor([3.1416, 0.0000, 0.0000])
"""
if not torch.is_tensor(quaternion):
raise TypeError(f"Input type is not a Tensor. Got {type(quaternion)}")
if not quaternion.shape[-1] == 4:
raise ValueError(f"Input must be a tensor of shape Nx4 or 4. Got {quaternion.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
# unpack input and compute conversion
q1: Tensor = tensor([])
q2: Tensor = tensor([])
q3: Tensor = tensor([])
cos_theta: Tensor = tensor([])
if order == QuaternionCoeffOrder.XYZW:
q1 = quaternion[..., 0]
q2 = quaternion[..., 1]
q3 = quaternion[..., 2]
cos_theta = quaternion[..., 3]
else:
cos_theta = quaternion[..., 0]
q1 = quaternion[..., 1]
q2 = quaternion[..., 2]
q3 = quaternion[..., 3]
sin_squared_theta: Tensor = q1 * q1 + q2 * q2 + q3 * q3
sin_theta: Tensor = torch.sqrt(sin_squared_theta)
two_theta: Tensor = 2.0 * where(
cos_theta < 0.0, torch.atan2(-sin_theta, -cos_theta), torch.atan2(sin_theta, cos_theta)
)
k_pos: Tensor = two_theta / sin_theta
k_neg: Tensor = 2.0 * torch.ones_like(sin_theta)
k: Tensor = where(sin_squared_theta > 0.0, k_pos, k_neg)
angle_axis: Tensor = torch.zeros_like(quaternion)[..., :3]
angle_axis[..., 0] += q1 * k
angle_axis[..., 1] += q2 * k
angle_axis[..., 2] += q3 * k
return angle_axis
[docs]def quaternion_log_to_exp(
quaternion: Tensor, eps: float = 1.0e-8, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW
) -> Tensor:
r"""Apply exponential map to log quaternion.
The quaternion should be in (x, y, z, w) or (w, x, y, z) format.
Args:
quaternion: a tensor containing a quaternion to be converted.
The tensor can be of shape :math:`(*, 3)`.
eps: a small number for clamping.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
the quaternion exponential map of shape :math:`(*, 4)`.
Example:
>>> quaternion = tensor((0., 0., 0.))
>>> quaternion_log_to_exp(quaternion, eps=torch.finfo(quaternion.dtype).eps,
... order=QuaternionCoeffOrder.WXYZ)
tensor([1., 0., 0., 0.])
"""
if not isinstance(quaternion, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(quaternion)}")
if not quaternion.shape[-1] == 3:
raise ValueError(f"Input must be a tensor of shape (*, 3). Got {quaternion.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
# compute quaternion norm
norm_q: Tensor = torch.norm(quaternion, p=2, dim=-1, keepdim=True).clamp(min=eps)
# compute scalar and vector
quaternion_vector: Tensor = quaternion * torch.sin(norm_q) / norm_q
quaternion_scalar: Tensor = torch.cos(norm_q)
# compose quaternion and return
quaternion_exp: Tensor = tensor([])
if order == QuaternionCoeffOrder.XYZW:
quaternion_exp = concatenate((quaternion_vector, quaternion_scalar), dim=-1)
else:
quaternion_exp = concatenate((quaternion_scalar, quaternion_vector), dim=-1)
return quaternion_exp
[docs]def quaternion_exp_to_log(
quaternion: Tensor, eps: float = 1.0e-8, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW
) -> Tensor:
r"""Apply the log map to a quaternion.
The quaternion should be in (x, y, z, w) format.
Args:
quaternion: a tensor containing a quaternion to be converted.
The tensor can be of shape :math:`(*, 4)`.
eps: a small number for clamping.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
the quaternion log map of shape :math:`(*, 3)`.
Example:
>>> quaternion = tensor((1., 0., 0., 0.))
>>> quaternion_exp_to_log(quaternion, eps=torch.finfo(quaternion.dtype).eps,
... order=QuaternionCoeffOrder.WXYZ)
tensor([0., 0., 0.])
"""
if not isinstance(quaternion, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(quaternion)}")
if not quaternion.shape[-1] == 4:
raise ValueError(f"Input must be a tensor of shape (*, 4). Got {quaternion.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
# unpack quaternion vector and scalar
quaternion_vector: Tensor = tensor([])
quaternion_scalar: Tensor = tensor([])
if order == QuaternionCoeffOrder.XYZW:
quaternion_vector = quaternion[..., 0:3]
quaternion_scalar = quaternion[..., 3:4]
else:
quaternion_scalar = quaternion[..., 0:1]
quaternion_vector = quaternion[..., 1:4]
# compute quaternion norm
norm_q: Tensor = torch.norm(quaternion_vector, p=2, dim=-1, keepdim=True).clamp(min=eps)
# apply log map
quaternion_log: Tensor = quaternion_vector * torch.acos(torch.clamp(quaternion_scalar, min=-1.0, max=1.0)) / norm_q
return quaternion_log
# based on:
# https://github.com/facebookresearch/QuaterNet/blob/master/common/quaternion.py#L138
[docs]def angle_axis_to_quaternion(angle_axis: Tensor, order: QuaternionCoeffOrder = QuaternionCoeffOrder.XYZW) -> Tensor:
r"""Convert an angle axis to a quaternion.
The quaternion vector has components in (x, y, z, w) or (w, x, y, z) format.
Adapted from ceres C++ library: ceres-solver/include/ceres/rotation.h
Args:
angle_axis: tensor with angle axis in radians.
order: quaternion coefficient order. Note: 'xyzw' will be deprecated in favor of 'wxyz'.
Return:
tensor with quaternion.
Shape:
- Input: :math:`(*, 3)` where `*` means, any number of dimensions
- Output: :math:`(*, 4)`
Example:
>>> angle_axis = tensor((0., 1., 0.))
>>> angle_axis_to_quaternion(angle_axis, order=QuaternionCoeffOrder.WXYZ)
tensor([0.8776, 0.0000, 0.4794, 0.0000])
"""
if not torch.is_tensor(angle_axis):
raise TypeError(f"Input type is not a Tensor. Got {type(angle_axis)}")
if not angle_axis.shape[-1] == 3:
raise ValueError(f"Input must be a tensor of shape Nx3 or 3. Got {angle_axis.shape}")
if not torch.jit.is_scripting():
if order.name not in QuaternionCoeffOrder.__members__.keys():
raise ValueError(f"order must be one of {QuaternionCoeffOrder.__members__.keys()}")
if order == QuaternionCoeffOrder.XYZW:
warnings.warn(
"`XYZW` quaternion coefficient order is deprecated and"
" will be removed after > 0.6. "
"Please use `QuaternionCoeffOrder.WXYZ` instead."
)
# unpack input and compute conversion
a0: Tensor = angle_axis[..., 0:1]
a1: Tensor = angle_axis[..., 1:2]
a2: Tensor = angle_axis[..., 2:3]
theta_squared: Tensor = a0 * a0 + a1 * a1 + a2 * a2
theta: Tensor = torch.sqrt(theta_squared)
half_theta: Tensor = theta * 0.5
mask: Tensor = theta_squared > 0.0
ones: Tensor = torch.ones_like(half_theta)
k_neg: Tensor = 0.5 * ones
k_pos: Tensor = torch.sin(half_theta) / theta
k: Tensor = where(mask, k_pos, k_neg)
w: Tensor = where(mask, torch.cos(half_theta), ones)
quaternion: Tensor = torch.zeros(size=(*angle_axis.shape[:-1], 4), dtype=angle_axis.dtype, device=angle_axis.device)
if order == QuaternionCoeffOrder.XYZW:
quaternion[..., 0:1] = a0 * k
quaternion[..., 1:2] = a1 * k
quaternion[..., 2:3] = a2 * k
quaternion[..., 3:4] = w
else:
quaternion[..., 1:2] = a0 * k
quaternion[..., 2:3] = a1 * k
quaternion[..., 3:4] = a2 * k
quaternion[..., 0:1] = w
return quaternion
# inspired by: https://stackoverflow.com/questions/56207448/efficient-quaternions-to-euler-transformation
def euler_from_quaternion(w: Tensor, x: Tensor, y: Tensor, z: Tensor) -> Tuple[Tensor, Tensor, Tensor]:
"""Convert a quaternion coefficients to Euler angles.
Returned angles are in radians in XYZ convention.
Args:
w: quaternion :math:`q_w` coefficient.
x: quaternion :math:`q_x` coefficient.
y: quaternion :math:`q_y` coefficient.
z: quaternion :math:`q_z` coefficient.
Return:
A tuple with euler angles`roll`, `pitch`, `yaw`.
"""
KORNIA_CHECK(w.shape == x.shape)
KORNIA_CHECK(x.shape == y.shape)
KORNIA_CHECK(y.shape == z.shape)
yy = y * y
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + yy)
roll = sinr_cosp.atan2(cosr_cosp)
sinp = 2.0 * (w * y - z * x)
sinp = sinp.clamp(min=-1.0, max=1.0)
pitch = sinp.asin()
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (yy + z * z)
yaw = siny_cosp.atan2(cosy_cosp)
return roll, pitch, yaw
def quaternion_from_euler(roll: Tensor, pitch: Tensor, yaw: Tensor) -> Tuple[Tensor, Tensor, Tensor, Tensor]:
"""Convert Euler angles to quaternion coefficients.
Euler angles are assumed to be in radians in XYZ convention.
Args:
roll: the roll euler angle.
pitch: the pitch euler angle.
yaw: the yaw euler angle.
Return:
A tuple with quaternion coefficients in order of `wxyz`.
"""
KORNIA_CHECK(roll.shape == pitch.shape)
KORNIA_CHECK(pitch.shape == yaw.shape)
roll_half = roll * 0.5
pitch_half = pitch * 0.5
yaw_half = yaw * 0.5
cy = yaw_half.cos()
sy = yaw_half.sin()
cp = pitch_half.cos()
sp = pitch_half.sin()
cr = roll_half.cos()
sr = roll_half.sin()
qw = cy * cp * cr + sy * sp * sr
qx = cy * cp * sr - sy * sp * cr
qy = sy * cp * sr + cy * sp * cr
qz = sy * cp * cr - cy * sp * sr
return qw, qx, qy, qz
# based on:
# https://github.com/ClementPinard/SfmLearner-Pytorch/blob/master/inverse_warp.py#L65-L71
[docs]def normalize_pixel_coordinates(pixel_coordinates: Tensor, height: int, width: int, eps: float = 1e-8) -> Tensor:
r"""Normalize pixel coordinates between -1 and 1.
Normalized, -1 if on extreme left, 1 if on extreme right (x = w-1).
Args:
pixel_coordinates: the grid with pixel coordinates. Shape can be :math:`(*, 2)`.
width: the maximum width in the x-axis.
height: the maximum height in the y-axis.
eps: safe division by zero.
Return:
the normalized pixel coordinates with shape :math:`(*, 2)`.
Examples:
>>> coords = tensor([[50., 100.]])
>>> normalize_pixel_coordinates(coords, 100, 50)
tensor([[1.0408, 1.0202]])
"""
if pixel_coordinates.shape[-1] != 2:
raise ValueError("Input pixel_coordinates must be of shape (*, 2). " "Got {}".format(pixel_coordinates.shape))
# compute normalization factor
hw: Tensor = stack(
[
tensor(width, device=pixel_coordinates.device, dtype=pixel_coordinates.dtype),
tensor(height, device=pixel_coordinates.device, dtype=pixel_coordinates.dtype),
]
)
factor: Tensor = tensor(2.0, device=pixel_coordinates.device, dtype=pixel_coordinates.dtype) / (hw - 1).clamp(eps)
return factor * pixel_coordinates - 1
[docs]def denormalize_pixel_coordinates(pixel_coordinates: Tensor, height: int, width: int, eps: float = 1e-8) -> Tensor:
r"""Denormalize pixel coordinates.
The input is assumed to be -1 if on extreme left, 1 if on extreme right (x = w-1).
Args:
pixel_coordinates: the normalized grid coordinates. Shape can be :math:`(*, 2)`.
width: the maximum width in the x-axis.
height: the maximum height in the y-axis.
eps: safe division by zero.
Return:
the denormalized pixel coordinates with shape :math:`(*, 2)`.
Examples:
>>> coords = tensor([[-1., -1.]])
>>> denormalize_pixel_coordinates(coords, 100, 50)
tensor([[0., 0.]])
"""
if pixel_coordinates.shape[-1] != 2:
raise ValueError("Input pixel_coordinates must be of shape (*, 2). " "Got {}".format(pixel_coordinates.shape))
# compute normalization factor
hw: Tensor = stack([tensor(width), tensor(height)]).to(pixel_coordinates.device).to(pixel_coordinates.dtype)
factor: Tensor = tensor(2.0) / (hw - 1).clamp(eps)
return tensor(1.0) / factor * (pixel_coordinates + 1)
[docs]def normalize_pixel_coordinates3d(
pixel_coordinates: Tensor, depth: int, height: int, width: int, eps: float = 1e-8
) -> Tensor:
r"""Normalize pixel coordinates between -1 and 1.
Normalized, -1 if on extreme left, 1 if on extreme right (x = w-1).
Args:
pixel_coordinates: the grid with pixel coordinates. Shape can be :math:`(*, 3)`.
depth: the maximum depth in the z-axis.
height: the maximum height in the y-axis.
width: the maximum width in the x-axis.
eps: safe division by zero.
Return:
the normalized pixel coordinates.
"""
if pixel_coordinates.shape[-1] != 3:
raise ValueError("Input pixel_coordinates must be of shape (*, 3). " "Got {}".format(pixel_coordinates.shape))
# compute normalization factor
dhw: Tensor = (
stack([tensor(depth), tensor(width), tensor(height)]).to(pixel_coordinates.device).to(pixel_coordinates.dtype)
)
factor: Tensor = tensor(2.0) / (dhw - 1).clamp(eps)
return factor * pixel_coordinates - 1
[docs]def denormalize_pixel_coordinates3d(
pixel_coordinates: Tensor, depth: int, height: int, width: int, eps: float = 1e-8
) -> Tensor:
r"""Denormalize pixel coordinates.
The input is assumed to be -1 if on extreme left, 1 if on extreme right (x = w-1).
Args:
pixel_coordinates: the normalized grid coordinates. Shape can be :math:`(*, 3)`.
depth: the maximum depth in the x-axis.
height: the maximum height in the y-axis.
width: the maximum width in the x-axis.
eps: safe division by zero.
Return:
the denormalized pixel coordinates.
"""
if pixel_coordinates.shape[-1] != 3:
raise ValueError("Input pixel_coordinates must be of shape (*, 3). " "Got {}".format(pixel_coordinates.shape))
# compute normalization factor
dhw: Tensor = (
stack([tensor(depth), tensor(width), tensor(height)]).to(pixel_coordinates.device).to(pixel_coordinates.dtype)
)
factor: Tensor = tensor(2.0) / (dhw - 1).clamp(eps)
return tensor(1.0) / factor * (pixel_coordinates + 1)
[docs]def angle_to_rotation_matrix(angle: Tensor) -> Tensor:
r"""Create a rotation matrix out of angles in degrees.
Args:
angle: tensor of angles in degrees, any shape :math:`(*)`.
Returns:
tensor of rotation matrices with shape :math:`(*, 2, 2)`.
Example:
>>> input = torch.rand(1, 3) # Nx3
>>> output = angle_to_rotation_matrix(input) # Nx3x2x2
"""
ang_rad = deg2rad(angle)
cos_a: Tensor = torch.cos(ang_rad)
sin_a: Tensor = torch.sin(ang_rad)
return stack([cos_a, sin_a, -sin_a, cos_a], dim=-1).view(*angle.shape, 2, 2)
[docs]def normalize_homography(
dst_pix_trans_src_pix: Tensor, dsize_src: Tuple[int, int], dsize_dst: Tuple[int, int]
) -> Tensor:
r"""Normalize a given homography in pixels to [-1, 1].
Args:
dst_pix_trans_src_pix: homography/ies from source to destination to be
normalized. :math:`(B, 3, 3)`
dsize_src: size of the source image (height, width).
dsize_dst: size of the destination image (height, width).
Returns:
the normalized homography of shape :math:`(B, 3, 3)`.
"""
if not isinstance(dst_pix_trans_src_pix, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(dst_pix_trans_src_pix)}")
if not (len(dst_pix_trans_src_pix.shape) == 3 or dst_pix_trans_src_pix.shape[-2:] == (3, 3)):
raise ValueError(f"Input dst_pix_trans_src_pix must be a Bx3x3 tensor. Got {dst_pix_trans_src_pix.shape}")
# source and destination sizes
src_h, src_w = dsize_src
dst_h, dst_w = dsize_dst
# compute the transformation pixel/norm for src/dst
src_norm_trans_src_pix: Tensor = normal_transform_pixel(src_h, src_w).to(dst_pix_trans_src_pix)
src_pix_trans_src_norm = _torch_inverse_cast(src_norm_trans_src_pix)
dst_norm_trans_dst_pix: Tensor = normal_transform_pixel(dst_h, dst_w).to(dst_pix_trans_src_pix)
# compute chain transformations
dst_norm_trans_src_norm: Tensor = dst_norm_trans_dst_pix @ (dst_pix_trans_src_pix @ src_pix_trans_src_norm)
return dst_norm_trans_src_norm
def normal_transform_pixel(
height: int,
width: int,
eps: float = 1e-14,
device: Optional[torch.device] = None,
dtype: Optional[torch.dtype] = None,
) -> Tensor:
r"""Compute the normalization matrix from image size in pixels to [-1, 1].
Args:
height image height.
width: image width.
eps: epsilon to prevent divide-by-zero errors
Returns:
normalized transform with shape :math:`(1, 3, 3)`.
"""
tr_mat = tensor([[1.0, 0.0, -1.0], [0.0, 1.0, -1.0], [0.0, 0.0, 1.0]], device=device, dtype=dtype) # 3x3
# prevent divide by zero bugs
width_denom: float = eps if width == 1 else width - 1.0
height_denom: float = eps if height == 1 else height - 1.0
tr_mat[0, 0] = tr_mat[0, 0] * 2.0 / width_denom
tr_mat[1, 1] = tr_mat[1, 1] * 2.0 / height_denom
return tr_mat.unsqueeze(0) # 1x3x3
def normal_transform_pixel3d(
depth: int,
height: int,
width: int,
eps: float = 1e-14,
device: Optional[torch.device] = None,
dtype: Optional[torch.dtype] = None,
) -> Tensor:
r"""Compute the normalization matrix from image size in pixels to [-1, 1].
Args:
depth: image depth.
height: image height.
width: image width.
eps: epsilon to prevent divide-by-zero errors
Returns:
normalized transform with shape :math:`(1, 4, 4)`.
"""
tr_mat = tensor(
[[1.0, 0.0, 0.0, -1.0], [0.0, 1.0, 0.0, -1.0], [0.0, 0.0, 1.0, -1.0], [0.0, 0.0, 0.0, 1.0]],
device=device,
dtype=dtype,
) # 4x4
# prevent divide by zero bugs
width_denom: float = eps if width == 1 else width - 1.0
height_denom: float = eps if height == 1 else height - 1.0
depth_denom: float = eps if depth == 1 else depth - 1.0
tr_mat[0, 0] = tr_mat[0, 0] * 2.0 / width_denom
tr_mat[1, 1] = tr_mat[1, 1] * 2.0 / height_denom
tr_mat[2, 2] = tr_mat[2, 2] * 2.0 / depth_denom
return tr_mat.unsqueeze(0) # 1x4x4
[docs]def denormalize_homography(
dst_pix_trans_src_pix: Tensor, dsize_src: Tuple[int, int], dsize_dst: Tuple[int, int]
) -> Tensor:
r"""De-normalize a given homography in pixels from [-1, 1] to actual height and width.
Args:
dst_pix_trans_src_pix: homography/ies from source to destination to be
denormalized. :math:`(B, 3, 3)`
dsize_src: size of the source image (height, width).
dsize_dst: size of the destination image (height, width).
Returns:
the denormalized homography of shape :math:`(B, 3, 3)`.
"""
if not isinstance(dst_pix_trans_src_pix, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(dst_pix_trans_src_pix)}")
if not (len(dst_pix_trans_src_pix.shape) == 3 or dst_pix_trans_src_pix.shape[-2:] == (3, 3)):
raise ValueError(f"Input dst_pix_trans_src_pix must be a Bx3x3 tensor. Got {dst_pix_trans_src_pix.shape}")
# source and destination sizes
src_h, src_w = dsize_src
dst_h, dst_w = dsize_dst
# compute the transformation pixel/norm for src/dst
src_norm_trans_src_pix: Tensor = normal_transform_pixel(src_h, src_w).to(dst_pix_trans_src_pix)
dst_norm_trans_dst_pix: Tensor = normal_transform_pixel(dst_h, dst_w).to(dst_pix_trans_src_pix)
dst_denorm_trans_dst_pix = _torch_inverse_cast(dst_norm_trans_dst_pix)
# compute chain transformations
dst_norm_trans_src_norm: Tensor = dst_denorm_trans_dst_pix @ (dst_pix_trans_src_pix @ src_norm_trans_src_pix)
return dst_norm_trans_src_norm
[docs]def normalize_homography3d(
dst_pix_trans_src_pix: Tensor, dsize_src: Tuple[int, int, int], dsize_dst: Tuple[int, int, int]
) -> Tensor:
r"""Normalize a given homography in pixels to [-1, 1].
Args:
dst_pix_trans_src_pix: homography/ies from source to destination to be
normalized. :math:`(B, 4, 4)`
dsize_src: size of the source image (depth, height, width).
dsize_src: size of the destination image (depth, height, width).
Returns:
the normalized homography.
Shape:
Output: :math:`(B, 4, 4)`
"""
if not isinstance(dst_pix_trans_src_pix, Tensor):
raise TypeError(f"Input type is not a Tensor. Got {type(dst_pix_trans_src_pix)}")
if not (len(dst_pix_trans_src_pix.shape) == 3 or dst_pix_trans_src_pix.shape[-2:] == (4, 4)):
raise ValueError(f"Input dst_pix_trans_src_pix must be a Bx3x3 tensor. Got {dst_pix_trans_src_pix.shape}")
# source and destination sizes
src_d, src_h, src_w = dsize_src
dst_d, dst_h, dst_w = dsize_dst
# compute the transformation pixel/norm for src/dst
src_norm_trans_src_pix: Tensor = normal_transform_pixel3d(src_d, src_h, src_w).to(dst_pix_trans_src_pix)
src_pix_trans_src_norm = _torch_inverse_cast(src_norm_trans_src_pix)
dst_norm_trans_dst_pix: Tensor = normal_transform_pixel3d(dst_d, dst_h, dst_w).to(dst_pix_trans_src_pix)
# compute chain transformations
dst_norm_trans_src_norm: Tensor = dst_norm_trans_dst_pix @ (dst_pix_trans_src_pix @ src_pix_trans_src_norm)
return dst_norm_trans_src_norm
[docs]def normalize_points_with_intrinsics(point_2d: Tensor, camera_matrix: Tensor):
r"""Normalizes points with intrinsics. Useful for conversion of keypoints to be used with essential matrix.
Args:
point_2d: tensor containing the 2d points in the image pixel coordinates.
The shape of the tensor can be :math:`(*, 2)`.
camera_matrix: tensor containing the intrinsics camera
matrix. The tensor shape must be :math:`(*, 3, 3)`.
Returns:
tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.
Example:
>>> _ = torch.manual_seed(0)
>>> X = torch.rand(1, 2)
>>> K = torch.eye(3)[None]
>>> normalize_points_with_intrinsics(X, K)
tensor([[0.4963, 0.7682]])
"""
KORNIA_CHECK_SHAPE(point_2d, ["*", "2"])
KORNIA_CHECK_SHAPE(camera_matrix, ["*", "3", "3"])
# projection eq. K_inv * [u v 1]'
# x = (u - cx) * Z / fx
# y = (v - cy) * Z / fy
# unpack coordinates
u_coord: Tensor = point_2d[..., 0]
v_coord: Tensor = point_2d[..., 1]
# unpack intrinsics
fx: Tensor = camera_matrix[..., 0, 0]
fy: Tensor = camera_matrix[..., 1, 1]
cx: Tensor = camera_matrix[..., 0, 2]
cy: Tensor = camera_matrix[..., 1, 2]
# projective
x_coord: Tensor = (u_coord - cx) / fx
y_coord: Tensor = (v_coord - cy) / fy
xy: Tensor = stack([x_coord, y_coord], dim=-1)
return xy
[docs]def denormalize_points_with_intrinsics(point_2d_norm: Tensor, camera_matrix: Tensor):
r"""Normalizes points with intrinsics. Useful for conversion of keypoints to be used with essential matrix.
Args:
point_2d_norm: tensor containing the 2d points in the image pixel coordinates.
The shape of the tensor can be :math:`(*, 2)`.
camera_matrix: tensor containing the intrinsics camera
matrix. The tensor shape must be :math:`(*, 3, 3)`.
Returns:
tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.
Example:
>>> _ = torch.manual_seed(0)
>>> X = torch.rand(1, 2)
>>> K = torch.eye(3)[None]
>>> denormalize_points_with_intrinsics(X, K)
tensor([[0.4963, 0.7682]])
"""
KORNIA_CHECK_SHAPE(point_2d_norm, ["*", "2"])
KORNIA_CHECK_SHAPE(camera_matrix, ["*", "3", "3"])
# projection eq. [u, v, w]' = K * [x y z 1]'
# u = fx * X + cx
# v = fy * Y + cy
# unpack coordinates
x_coord: Tensor = point_2d_norm[..., 0]
y_coord: Tensor = point_2d_norm[..., 1]
# unpack intrinsics
fx: Tensor = camera_matrix[..., 0, 0]
fy: Tensor = camera_matrix[..., 1, 1]
cx: Tensor = camera_matrix[..., 0, 2]
cy: Tensor = camera_matrix[..., 1, 2]
# apply intrinsics ans return
u_coord: Tensor = x_coord * fx + cx
v_coord: Tensor = y_coord * fy + cy
return stack([u_coord, v_coord], dim=-1)
[docs]def Rt_to_matrix4x4(R: Tensor, t: Tensor) -> Tensor:
r"""Combines 3x3 rotation matrix R and 1x3 translation vector t into 4x4 extrinsics.
Args:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Returns:
the extrinsics :math:`(B, 4, 4)`.
Example:
>>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
>>> Rt_to_matrix4x4(R, t)
tensor([[[1., 0., 0., 1.],
[0., 1., 0., 1.],
[0., 0., 1., 1.],
[0., 0., 0., 1.]]])
"""
KORNIA_CHECK_SHAPE(R, ["B", "3", "3"])
KORNIA_CHECK_SHAPE(t, ["B", "3", "1"])
Rt = concatenate([R, t], dim=2)
return convert_affinematrix_to_homography3d(Rt)
[docs]def matrix4x4_to_Rt(extrinsics: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts 4x4 extrinsics into 3x3 rotation matrix R and 1x3 translation vector ts.
Args:
extrinsics: pose matrix :math:`(B, 4, 4)`.
Returns:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Example:
>>> ext = torch.eye(4)[None]
>>> matrix4x4_to_Rt(ext)
(tensor([[[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]]), tensor([[[0.],
[0.],
[0.]]]))
"""
KORNIA_CHECK_SHAPE(extrinsics, ["B", "4", "4"])
R, t = extrinsics[:, :3, :3], extrinsics[:, :3, 3:]
return R, t
[docs]def camtoworld_graphics_to_vision_4x4(extrinsics_graphics: Tensor) -> Tensor:
r"""Converts graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.), , i.e. flips y
and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards]. Vision convention: [+x, +y, +z] ==
[right, down, forwards]
Args:
extrinsics: pose matrix :math:`(B, 4, 4)`.
Returns:
extrinsics: pose matrix :math:`(B, 4, 4)`.
Example:
>>> ext = torch.eye(4)[None]
>>> camtoworld_graphics_to_vision_4x4(ext)
tensor([[[ 1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., -1., 0.],
[ 0., 0., 0., 1.]]])
"""
KORNIA_CHECK_SHAPE(extrinsics_graphics, ["B", "4", "4"])
invert_yz = tensor(
[[[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1.0]]],
dtype=extrinsics_graphics.dtype,
device=extrinsics_graphics.device,
)
return extrinsics_graphics @ invert_yz
[docs]def camtoworld_graphics_to_vision_Rt(R: Tensor, t: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.), , i.e. flips y
and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards]. Vision convention: [+x, +y, +z] ==
[right, down, forwards]
Args:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Returns:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Example:
>>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
>>> camtoworld_graphics_to_vision_Rt(R, t)
(tensor([[[ 1., 0., 0.],
[ 0., -1., 0.],
[ 0., 0., -1.]]]), tensor([[[1.],
[1.],
[1.]]]))
"""
KORNIA_CHECK_SHAPE(R, ["B", "3", "3"])
KORNIA_CHECK_SHAPE(t, ["B", "3", "1"])
mat4x4 = camtoworld_graphics_to_vision_4x4(Rt_to_matrix4x4(R, t))
return matrix4x4_to_Rt(mat4x4)
[docs]def camtoworld_vision_to_graphics_4x4(extrinsics_vision: Tensor) -> Tensor:
r"""Converts vision coordinate frame (e.g. OpenCV) to graphics coordinate frame (e.g. OpenGK.), i.e. flips y and
z axis Graphics convention: [+x, +y, +z] == [right, up, backwards]. Vision convention: [+x, +y, +z] == [right,
down, forwards]
Args:
extrinsics: pose matrix :math:`(B, 4, 4)`.
Returns:
extrinsics: pose matrix :math:`(B, 4, 4)`.
Example:
>>> ext = torch.eye(4)[None]
>>> camtoworld_vision_to_graphics_4x4(ext)
tensor([[[ 1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., -1., 0.],
[ 0., 0., 0., 1.]]])
"""
KORNIA_CHECK_SHAPE(extrinsics_vision, ["B", "4", "4"])
invert_yz = tensor(
[[[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1.0]]],
dtype=extrinsics_vision.dtype,
device=extrinsics_vision.device,
)
return extrinsics_vision @ invert_yz
[docs]def camtoworld_vision_to_graphics_Rt(R: Tensor, t: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.), , i.e. flips y
and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards]. Vision convention: [+x, +y, +z] ==
[right, down, forwards]
Args:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Returns:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Example:
>>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
>>> camtoworld_vision_to_graphics_Rt(R, t)
(tensor([[[ 1., 0., 0.],
[ 0., -1., 0.],
[ 0., 0., -1.]]]), tensor([[[1.],
[1.],
[1.]]]))
"""
KORNIA_CHECK_SHAPE(R, ["B", "3", "3"])
KORNIA_CHECK_SHAPE(t, ["B", "3", "1"])
mat4x4 = camtoworld_vision_to_graphics_4x4(Rt_to_matrix4x4(R, t))
return matrix4x4_to_Rt(mat4x4)
[docs]def camtoworld_to_worldtocam_Rt(R: Tensor, t: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts camtoworld, i.e. projection from camera coordinate system to world coordinate system, to worldtocam
frame i.e. projection from world to the camera coordinate system (used in Colmap).
See
long-url: https://colmap.github.io/format.html#output-format
Args:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Returns:
Rinv: Rotation matrix, :math:`(B, 3, 3).`
tinv: Translation matrix :math:`(B, 3, 1)`.
Example:
>>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
>>> camtoworld_to_worldtocam_Rt(R, t)
(tensor([[[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]]), tensor([[[-1.],
[-1.],
[-1.]]]))
"""
KORNIA_CHECK_SHAPE(R, ["B", "3", "3"])
KORNIA_CHECK_SHAPE(t, ["B", "3", "1"])
R_inv = R.transpose(1, 2)
new_t: Tensor = -R_inv @ t
return (R_inv, new_t)
[docs]def worldtocam_to_camtoworld_Rt(R: Tensor, t: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts worldtocam frame i.e. projection from world to the camera coordinate system (used in Colmap) to
camtoworld, i.e. projection from camera coordinate system to world coordinate system.
Args:
R: Rotation matrix, :math:`(B, 3, 3).`
t: Translation matrix :math:`(B, 3, 1)`.
Returns:
Rinv: Rotation matrix, :math:`(B, 3, 3).`
tinv: Translation matrix :math:`(B, 3, 1)`.
Example:
>>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
>>> worldtocam_to_camtoworld_Rt(R, t)
(tensor([[[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]]]), tensor([[[-1.],
[-1.],
[-1.]]]))
"""
KORNIA_CHECK_SHAPE(R, ["B", "3", "3"])
KORNIA_CHECK_SHAPE(t, ["B", "3", "1"])
R_inv = R.transpose(1, 2)
new_t: Tensor = -R_inv @ t
return (R_inv, new_t)
[docs]def ARKitQTVecs_to_ColmapQTVecs(qvec: Tensor, tvec: Tensor) -> Tuple[Tensor, Tensor]:
r"""Converts output of Apple ARKit screen pose (in quaternion representation) to the camera-to-world
transformation, expected by Colmap, also in quaternion representation.
Args:
qvec: ARKit rotation quaternion :math:`(B, 4)`, [x, y, z, w] format.
tvec: translation vector :math:`(B, 3, 1)`, [x, y, z]
Returns:
qvec: Colmap rotation quaternion :math:`(B, 4)`, [w, x, y, z] format.
tvec: translation vector :math:`(B, 3, 1)`, [x, y, z]
Example:
>>> q, t = tensor([0, 1, 0, 1.])[None], torch.ones(3).reshape(1, 3, 1)
>>> ARKitQTVecs_to_ColmapQTVecs(q, t)
(tensor([[0.7071, 0.0000, 0.7071, 0.0000]]), tensor([[[-1.0000],
[-1.0000],
[ 1.0000]]]))
"""
# ToDo: integrate QuaterniaonAPI
Rcg = quaternion_to_rotation_matrix(qvec, order=QuaternionCoeffOrder.WXYZ)
Rcv, Tcv = camtoworld_graphics_to_vision_Rt(Rcg, tvec)
R_colmap, t_colmap = camtoworld_to_worldtocam_Rt(Rcv, Tcv)
t_colmap = t_colmap.reshape(-1, 3, 1)
q_colmap = rotation_matrix_to_quaternion(R_colmap.contiguous(), order=QuaternionCoeffOrder.WXYZ)
return q_colmap, t_colmap