File size: 4,019 Bytes
205a7af |
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 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 |
"""Utility functions for conversions between different representations."""
from typing import Optional
import torch
def skew_symmetric(v: torch.Tensor) -> torch.Tensor:
"""Create a skew-symmetric matrix from a (batched) vector of size (..., 3).
Args:
(torch.Tensor): Vector of size (..., 3).
Returns:
(torch.Tensor): Skew-symmetric matrix of size (..., 3, 3).
"""
z = torch.zeros_like(v[..., 0])
return torch.stack(
[
z,
-v[..., 2],
v[..., 1],
v[..., 2],
z,
-v[..., 0],
-v[..., 1],
v[..., 0],
z,
],
dim=-1,
).reshape(v.shape[:-1] + (3, 3))
def rad2rotmat(
roll: torch.Tensor, pitch: torch.Tensor, yaw: Optional[torch.Tensor] = None
) -> torch.Tensor:
"""Convert (batched) roll, pitch, yaw angles (in radians) to rotation matrix.
Args:
roll (torch.Tensor): Roll angle in radians.
pitch (torch.Tensor): Pitch angle in radians.
yaw (torch.Tensor, optional): Yaw angle in radians. Defaults to None.
Returns:
torch.Tensor: Rotation matrix of shape (..., 3, 3).
"""
if yaw is None:
yaw = roll.new_zeros(roll.shape)
Rx = pitch.new_zeros(pitch.shape + (3, 3))
Rx[..., 0, 0] = 1
Rx[..., 1, 1] = torch.cos(pitch)
Rx[..., 1, 2] = torch.sin(pitch)
Rx[..., 2, 1] = -torch.sin(pitch)
Rx[..., 2, 2] = torch.cos(pitch)
Ry = yaw.new_zeros(yaw.shape + (3, 3))
Ry[..., 0, 0] = torch.cos(yaw)
Ry[..., 0, 2] = -torch.sin(yaw)
Ry[..., 1, 1] = 1
Ry[..., 2, 0] = torch.sin(yaw)
Ry[..., 2, 2] = torch.cos(yaw)
Rz = roll.new_zeros(roll.shape + (3, 3))
Rz[..., 0, 0] = torch.cos(roll)
Rz[..., 0, 1] = torch.sin(roll)
Rz[..., 1, 0] = -torch.sin(roll)
Rz[..., 1, 1] = torch.cos(roll)
Rz[..., 2, 2] = 1
return Rz @ Rx @ Ry
def fov2focal(fov: torch.Tensor, size: torch.Tensor) -> torch.Tensor:
"""Compute focal length from (vertical/horizontal) field of view.
Args:
fov (torch.Tensor): Field of view in radians.
size (torch.Tensor): Image height / width in pixels.
Returns:
torch.Tensor: Focal length in pixels.
"""
return size / 2 / torch.tan(fov / 2)
def focal2fov(focal: torch.Tensor, size: torch.Tensor) -> torch.Tensor:
"""Compute (vertical/horizontal) field of view from focal length.
Args:
focal (torch.Tensor): Focal length in pixels.
size (torch.Tensor): Image height / width in pixels.
Returns:
torch.Tensor: Field of view in radians.
"""
return 2 * torch.arctan(size / (2 * focal))
def pitch2rho(pitch: torch.Tensor, f: torch.Tensor, h: torch.Tensor) -> torch.Tensor:
"""Compute the distance from principal point to the horizon.
Args:
pitch (torch.Tensor): Pitch angle in radians.
f (torch.Tensor): Focal length in pixels.
h (torch.Tensor): Image height in pixels.
Returns:
torch.Tensor: Relative distance to the horizon.
"""
return torch.tan(pitch) * f / h
def rho2pitch(rho: torch.Tensor, f: torch.Tensor, h: torch.Tensor) -> torch.Tensor:
"""Compute the pitch angle from the distance to the horizon.
Args:
rho (torch.Tensor): Relative distance to the horizon.
f (torch.Tensor): Focal length in pixels.
h (torch.Tensor): Image height in pixels.
Returns:
torch.Tensor: Pitch angle in radians.
"""
return torch.atan(rho * h / f)
def rad2deg(rad: torch.Tensor) -> torch.Tensor:
"""Convert radians to degrees.
Args:
rad (torch.Tensor): Angle in radians.
Returns:
torch.Tensor: Angle in degrees.
"""
return rad / torch.pi * 180
def deg2rad(deg: torch.Tensor) -> torch.Tensor:
"""Convert degrees to radians.
Args:
deg (torch.Tensor): Angle in degrees.
Returns:
torch.Tensor: Angle in radians.
"""
return deg / 180 * torch.pi
|