def get_eg_analytical(Q, R, T, A, v):
"""
Computes the analytical expression for the generalisation error of erf teacher
and student with the given order parameters.
Parameters:
-----------
Q: student-student overlap
R: teacher-student overlap
T: teacher-teacher overlap
A: teacher second layer weights
v: student second layer weights
"""
eg_analytical = 0
# student-student overlaps
sqrtQ = torch.sqrt(1 + Q.diag())
norm = torch.ger(sqrtQ, sqrtQ)
eg_analytical += torch.sum((v.t() @ v) * torch.asin(Q / norm))
# teacher-teacher overlaps
sqrtT = torch.sqrt(1 + T.diag())
norm = torch.ger(sqrtT, sqrtT)
eg_analytical += torch.sum((A.t() @ A) * torch.asin(T / norm))
# student-teacher overlaps
norm = torch.ger(sqrtQ, sqrtT)
eg_analytical -= 2.0 * torch.sum((v.t() @ A) * torch.asin(R / norm))
return eg_analytical / math.pi
def mat2euler(rot_mat, seq='xyz'):
"""
convert rotation matrix to euler angle
:param rot_mat: rotation matrix rx*ry*rz [B, 3, 3]
:param seq: seq is xyz(rotate along z first) or zyx
:return: three angles, x, y, z
"""
r11 = rot_mat[:, 0, 0]
r12 = rot_mat[:, 0, 1]
r13 = rot_mat[:, 0, 2]
r21 = rot_mat[:, 1, 0]
r22 = rot_mat[:, 1, 1]
r23 = rot_mat[:, 1, 2]
r31 = rot_mat[:, 2, 0]
r32 = rot_mat[:, 2, 1]
r33 = rot_mat[:, 2, 2]
if seq == 'xyz':
z = torch.atan2(-r12, r11)
y = torch.asin(r13)
x = torch.atan2(-r23, r33)
else:
y = torch.asin(-r31)
x = torch.atan2(r32, r33)
z = torch.atan2(r21, r11)
return torch.stack((x, y, z), dim=1)
def distance(pred, real):
'''
:param pred: shape (n,2)
:param real: shape (n,2)
:return: (n,1)
'''
R = 6357
latPred = pred[:, :, 0:1].squeeze(-1)
latPred.map_(latPred, to_radians)
lonPred = pred[:, :, 1:].squeeze(-1)
lonPred.map_(lonPred, to_radians)
latReal = real[:, :, 0:1].squeeze(-1)
latReal.map_(latPred, to_radians)
lonReal = real[:, :, 1:].squeeze(-1)
lonReal.map_(lonReal, to_radians)
E1 = 2 * R * torch.asin(
torch.sqrt(
torch.sin(torch.pow((latPred - latReal) / 2, 2)) +
torch.cos(latReal) * torch.cos(latPred) *
torch.sin(torch.pow((lonPred - lonReal) / 2, 2))))
E2 = 2 * R * torch.asin(
torch.sqrt(
torch.pow(torch.sin((latPred - latReal) / 2), 2) +
torch.cos(latReal) * torch.cos(latPred) * torch.pow(
(lonPred - lonReal) / 2, 2)))
return torch.mean(E1)
def convertToxywha(self, mode):
if mode not in ("xywha", "4xy"):
raise ValueError("mode should be 'xyxy' or 'xywh'")
if mode == self.mode:
return self
# we only have two modes, so don't need to check
# self.mode
if mode == "xywha":
x, y, w, h, theta = self.bbox.split(1, dim=-1)
bbox = torch.cat((x, y, w, h, theta), dim=-1)
bbox = RBoxList(bbox, self.size, mode=mode)
else:
x1, y1, x2, y2, x3, y3, x4, y4 = self.bbox.split(1, dim=-1)
x, y = (x1 + x2 + x3 + x4) / 4.0, \
(y1 + y2 + y3 + y4) / 4.0
w, h = torch.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)) / 2.0, \
torch.sqrt((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2)) / 2.0
theta_w, theta_h = torch.asin(torch.abs(y1/2+y2/2-y)/w), \
torch.asin(torch.abs(y2/2+y3/2-y)/h)
theta_w, theta_h = theta_w / 3.1415926 * 180, theta_h / 3.1415926 * 180
wh_cat = torch.cat((w, h), dim=-1) #[N, 2]
wh_theta_cat = torch.cat((w, h), dim=-1) #[N, 2]
longH, longH_index = torch.max(wh_cat, dim=-1) #[N, 1], [N]
shortW, _ = torch.min(wh_cat, dim=-1) #[N, 1]
longH, shortW = longH[:, None], shortW[:, None]
longTheta = torch.gather(wh_theta_cat, 1, longH_index[:, None])
# print(longTheta.shape)
bbox = torch.cat((x, y, shortW * 2, longH * 2, longTheta), dim=-1)
# print(bbox)
bbox = RBoxList(bbox, self.size, mode="xywha")
bbox._copy_extra_fields(self)
return bbox
def getVertexNorm(vertices, faces):
# Nelson Max, "Weights for Computing Vertex Normals from Facet Vectors", 1999
normals = torch.zeros_like(vertices)
v = [vertices[faces[:, 0]], vertices[faces[:, 1]], vertices[faces[:, 2]]]
for i in range(3):
v0 = v[i]
v1 = v[(i + 1) % 3]
v2 = v[(i + 2) % 3]
e1 = v1 - v0
e2 = v2 - v0
e1_len = torch.norm(e1, dim=1, keepdim=True)
e2_len = torch.norm(e2, dim=1, keepdim=True)
side_a = e1 / e1_len
side_b = e2 / e2_len
if i == 0:
n = torch.cross(side_a, side_b)
n = n / torch.norm(n, dim=1, keepdim=True)
cond = torch.sum(side_a * side_b, -1) < 0
val_a = math.pi - 2.0 * torch.asin(
0.5 * torch.norm(side_a + side_b, dim=1))
val_b = 2.0 * torch.asin(0.5 * torch.norm(side_b - side_a, dim=1))
angle = torch.where(cond, val_a, val_b)
sin_angle = torch.sin(angle)
# XXX: Inefficient but it's PyTorch's limitation
contrib = n * sin_angle.unsqueeze_(-1) / (e1_len * e2_len)
normals.index_add_(0, faces[:, i], contrib)
normals = normals / torch.norm(normals, dim=1, keepdim=True)
return normals
def depth2pts_outside(ray_o, ray_d, depth):
'''
ray_o, ray_d: [..., 3]
depth: [...]; inverse of distance to sphere origin
'''
# note: d1 becomes negative if this mid point is behind camera
d1 = -torch.sum(ray_d * ray_o, dim=-1) / torch.sum(ray_d * ray_d, dim=-1)
p_mid = ray_o + d1.unsqueeze(-1) * ray_d
p_mid_norm = torch.norm(p_mid, dim=-1)
ray_d_cos = 1. / torch.norm(ray_d, dim=-1)
d2 = torch.sqrt(1. - p_mid_norm * p_mid_norm) * ray_d_cos
p_sphere = ray_o + (d1 + d2).unsqueeze(-1) * ray_d
rot_axis = torch.cross(ray_o, p_sphere, dim=-1)
rot_axis = rot_axis / torch.norm(rot_axis, dim=-1, keepdim=True)
phi = torch.asin(p_mid_norm)
theta = torch.asin(p_mid_norm * depth) # depth is inside [0, 1]
rot_angle = (phi - theta).unsqueeze(-1) # [..., 1]
# now rotate p_sphere
# Rodrigues formula: https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
p_sphere_new = p_sphere * torch.cos(rot_angle) + \
torch.cross(rot_axis, p_sphere, dim=-1) * torch.sin(rot_angle) + \
rot_axis * torch.sum(rot_axis*p_sphere, dim=-1, keepdim=True) * (1.-torch.cos(rot_angle))
p_sphere_new = p_sphere_new / torch.norm(
p_sphere_new, dim=-1, keepdim=True)
pts = torch.cat((p_sphere_new, depth.unsqueeze(-1)), dim=-1)
# now calculate conventional depth
depth_real = 1. / (depth + TINY_NUMBER) * torch.cos(theta) * ray_d_cos + d1
return pts, depth_real
def _angles_magnitude_metric(self,
predicted_translation,
gt_translation,
loss,
rounded=False):
"""Calculates loss for magnitudes of translations and for rotations of translations."""
predicted_translation = predicted_translation.view(-1, 2)
gt_translation = gt_translation.view(-1, 2)
if rounded == True:
predicted_translation = torch.round(predicted_translation)
#Calculate magnitude of translation
magnitude_gt = torch.norm(gt_translation, dim=1)
magnitude_predicted = torch.norm(predicted_translation, dim=1)
magnitude_loss = loss(magnitude_predicted, magnitude_gt)
#Calculate rotation of translation
alpha_gt = torch.asin(gt_translation[:, 1] / (magnitude_gt + 1e-8))
alpha_predicted = torch.asin(predicted_translation[:, 1] /
(magnitude_predicted + 1e-8))
alpha_loss = loss(alpha_predicted, alpha_gt) * 180 / np.pi
return magnitude_loss, alpha_loss
def run_experiments(self, input_data: torch.Tensor):
super().run_experiments(input_data)
if not self._include_derivatives:
return torch.asin(input_data)
else:
return torch.cat(
(torch.asin(input_data),
torch.sqrt(torch.tensor(1.) - input_data**2)**(-1)), 0)
def compute_vertex_normal(vertices, indices):
# code obtained from https://github.com/BachiLi/redner
# redner/pyredner/shape.py
def dot(v1, v2):
# v1 := 13776 x 3
# v1 := 13776 x 3
# return := 13776
return torch.sum(v1 * v2, dim=1)
def squared_length(v):
# v = 13776 x 3
return torch.sum(v * v, dim=1)
def length(v):
# v = 13776 x 3
# 13776
return torch.sqrt(squared_length(v))
# Nelson Max, "Weights for Computing Vertex Normals from Facet Vectors", 1999
# vertices := 6890 x 3
# indices := 13776 x 3
normals = torch.zeros(vertices.shape,
dtype=torch.float32,
device=vertices.device)
v = [
vertices[indices[:, 0].long(), :], vertices[indices[:, 1].long(), :],
vertices[indices[:, 2].long(), :]
]
for i in range(3):
v0 = v[i]
v1 = v[(i + 1) % 3]
v2 = v[(i + 2) % 3]
e1 = v1 - v0
e2 = v2 - v0
e1_len = length(e1)
e2_len = length(e2)
side_a = e1 / torch.reshape(e1_len, [-1, 1]) # 13776, 3
side_b = e2 / torch.reshape(e2_len, [-1, 1]) # 13776, 3
if i == 0:
n = torch.cross(side_a, side_b) # 13776, 3
n = n / torch.reshape(length(n), [-1, 1])
angle = torch.where(
dot(side_a, side_b) < 0,
np.pi - 2.0 * torch.asin(0.5 * length(side_a + side_b)),
2.0 * torch.asin(0.5 * length(side_b - side_a)))
sin_angle = torch.sin(angle) # 13776
# XXX: Inefficient but it's PyTorch's limitation
contrib = n * (sin_angle / (e1_len * e2_len)).reshape(-1, 1).expand(
-1, 3) # 13776, 3
index = indices[:, i].long().reshape(-1, 1).expand(
[-1, 3]) # torch.Size([13776, 3])
normals.scatter_add_(0, index, contrib)
normals = normals / torch.reshape(length(normals), [-1, 1])
return normals.contiguous()
def log_pdf(self, mu, std, action):
std = torch.clamp(std, min=self.std_bound[0], max=self.std_bound[1])
var = std**2
log_policy_pdf = -0.5 * (action - mu)**2 / var - 0.5 * torch.log(
var * 2 * 2 * torch.asin(torch.tensor(1.)))
entropy = 0.5 * (
torch.log(2 * 2 * torch.asin(torch.tensor(1.)) * std**2) + 1.)
return torch.sum(log_policy_pdf, dim=1,
keepdim=True), torch.sum(entropy, dim=1, keepdim=True)
def cyclic_angle(self):
m = self.angles.clamp(-1, 1)
angles = [
torch.acos(m[0, 0]),
torch.asin(m[0, 1]),
torch.acos(m[1, 1]),
torch.asin(m[1, 0])
]
return torch.stack(angles)
def _valid_epoch(self, epoch):
"""
Validate after training an epoch
:param epoch: Integer, current training epoch.
:return: A log that contains information about validation
"""
self.model.eval()
self.valid_metrics.reset()
with torch.no_grad():
offset_image = self.data_loader.ref_image
offset_label = self.data_loader.ref_label
offset = offset_label - self.model(offset_image).squeeze()
for batch_idx, (trans_image,
trans_target) in enumerate(self.valid_data_loader):
trans_image, trans_target = trans_image.to(
self.device), trans_target.to(self.device)
## Difference ##
trans_output = self.model(trans_image)
center = torch.tensor([32, 32])
trans_scale = trans_output[2] * offset_label[2]
trans_rot = torch.asin(trans_output[3]) - torch.asin(
offset_label[3])
trans_translation = trans_output[:2] - center + trans_output[
2] * torch.dot(
torch.tensor([[
torch.cos(trans_rot), -torch.sin(trans_rot)
], [torch.sin(trans_rot),
torch.cos(trans_rot)]]), offset_label[:2] - center)
target_pred = torch.tensor([
trans_translation[0], trans_translation[1], trans_scale,
trans_rot
])
loss = self.criterion(target_pred, trans_target)
#loss = self.criterion(trans_output - ref_ouptut, target)
################
self.writer.set_step(
(epoch - 1) * len(self.valid_data_loader) + batch_idx,
'valid')
self.valid_metrics.update('loss', torch.mean(loss).item())
for met in self.metric_ftns:
self.valid_metrics.update(
met.__name__, met(ref_output + offset, ref_target))
self.writer.add_image(
'input', make_grid(ref_image.cpu(), nrow=8,
normalize=True))
# add histogram of model parameters to the tensorboard
for name, p in self.model.named_parameters():
self.writer.add_histogram(name, p, bins='auto')
return self.valid_metrics.result()
def g(rho, mu1, mu2):
one_plus_sqrt_one_minus_rho_sqr = 1.0 + torch.sqrt(1.0 - rho * rho)
a = torch.asin(rho) - rho / one_plus_sqrt_one_minus_rho_sqr
safe_a = torch.abs(a) + HALF_EPSILON
safe_rho = torch.abs(rho) + EPSILON
A = a / (2.0 * math.pi)
sxx = safe_a * one_plus_sqrt_one_minus_rho_sqr / safe_rho
one_ovr_sxy = (torch.asin(rho) - rho) / (safe_a * safe_rho)
return A * torch.exp(-(mu1 * mu1 + mu2 * mu2) / (2.0 * sxx) + one_ovr_sxy * mu1 * mu2)
def l_b_mu(self) -> torch.tensor:
"""Return array of particles in galactic (l,b,mu) coordinates. (l,b) in degrees. mu is distance modulus"""
xyz = self.xyz
l_b_mu = torch.zeros_like(xyz)
d = (xyz[:, 0]**2 + xyz[:, 1]**2 + xyz[:, 2]**2).sqrt()
l_b_mu[:, 0] = torch.atan2(xyz[:, 1], xyz[:, 0]) * 180 / math.pi
b_offset = torch.asin(
self.z_0 / self.r_0
) # the GC has z = -z_0, rotate b coordinate so this is at l,b=(0,0)
l_b_mu[:, 1] = (torch.asin(xyz[:, 2] / d) + b_offset) * 180 / math.pi
l_b_mu[:, 2] = 5 * (100 * d).log10()
return l_b_mu
def run_torch(lat2, lon2):
import torch
# Allocate temporary arrays
size = len(lat2)
a = torch.empty(size, dtype=torch.float64, device=torch.device('cuda'))
dlat = torch.empty(size, dtype=torch.float64, device=torch.device('cuda'))
dlon = torch.empty(size, dtype=torch.float64, device=torch.device('cuda'))
# Transfer inputs to the GPU
lat2 = torch.from_numpy(lat2).cuda()
lon2 = torch.from_numpy(lon2).cuda()
# Begin computation
lat1 = 0.70984286
lon1 = 1.23892197
MILES_CONST = 3959.0
torch.sub(lat2, lat1, out=dlat)
torch.sub(lon2, lon1, out=dlon)
# dlat = sin(dlat / 2.0) ** 2.0
torch.div(dlat, 2.0, out=dlat)
torch.sin(dlat, out=dlat)
torch.mul(dlat, dlat, out=dlat)
# a = cos(lat1) * cos(lat2)
lat1_cos = math.cos(lat1)
torch.cos(lat2, out=a)
torch.mul(a, lat1_cos, out=a)
# a = a + sin(dlon / 2.0) ** 2.0
torch.div(dlon, 2.0, out=dlon)
torch.sin(dlon, out=dlon)
torch.mul(dlon, dlon, out=dlon)
torch.mul(a, dlon, out=a)
torch.add(dlat, a, out=a)
c = a
torch.sqrt(a, out=a)
torch.asin(a, out=a)
torch.mul(a, 2.0, out=c)
mi = c
torch.mul(c, MILES_CONST, out=mi)
# Transfer outputs back to CPU
torch.cuda.synchronize()
a = a.cpu().numpy()
return a
def R_to_spherical(R, dist=120):
'''
Convert Rotation matrix R into Spherical coordinate elements (Distance, Elevation, Azimuth)
Input:
R: Estimated rotation matrix
dist: given distance from object(origin) to camera
Output:
elev: Elevation
azim: Azimuth
'''
z_axis = R.view(-1, 3, 3)[:, :, -1]
camera_position = -z_axis * dist
elev = torch.asin(camera_position[:, 1] / dist)
azim = torch.asin(camera_position[:, 0] / (dist * torch.cos(elev)))
return torch.ones_like(elev) * dist, elev * 180 / np.pi, azim * 180 / np.pi
def inverse_euler(angles):
"""Returns the euler angles that are the inverse of the input.
Args:
angles: a torch.Tensor of shape [..., 3]
Returns:
A tensor of the same shape, representing the inverse rotation.
"""
sin_angles = torch.sin(angles)
cos_angles = torch.cos(angles)
sz, sy, sx = torch.unbind(-sin_angles, dim=-1)
cz, _, cx = torch.unbind(cos_angles, dim=-1)
y = torch.asin((cx * sy * cz) + (sx * sz))
x = -torch.asin((sx * sy * cz) - (cx * sz)) / tf.cos(y)
z = -torch.asin((cx * sy * sz) - (sx * cz)) / tf.cos(y)
return torch.stack([x, y, z], dim=-1)
def forward(self, x_train, y_train, x_test=None):
# See the autograd section for explanation of what happens here.
n = x_train.size(0)
#dm = torch.Tensor([[1,0,0],[0,1,0],[0,1,0],[0,0,1]])
SIGMA = torch.diag(self.sigmaentr.pow(2).view(
-1)) #dm.mm(self.sigmaentr.pow(2).view(3,1)).view(2,2)
trOnes = torch.ones(1, n)
xext = torch.cat((trOnes, x_train.t()), 0)
xSx = xext.t().mm(SIGMA).mm(xext)
xSxd = torch.diag(xSx).view(n, 1)
kyy = (2.0 / math.pi) * torch.asin(2.0 * xSx / torch.sqrt(
(1 + 2.0 * xSxd).mm(1 + 2.0 * xSxd.t())))
kyy = 0.5 * (kyy + kyy.t()) + self.sigma_n.pow(2) * torch.eye(n)
add = torch.eig(kyy)[0][:, 0].min().detach()
while add < 0:
kyy = kyy - 2 * add * torch.eye(n)
add = torch.eig(kyy)[0][:, 0].min().detach()
c = torch.cholesky(kyy, upper=True)
# v = torch.potrs(y_train, c, upper=True)
v, _ = torch.gesv(y_train, kyy)
if x_test is None:
out = (c, v)
if x_test is not None:
nt = x_test.size(0)
teOnes = torch.ones(1, nt)
xexte = torch.cat((teOnes, x_test.t()), 0)
xSx_m = xext.t().mm(SIGMA).mm(xexte)
xSx_te_d = torch.sum(xexte.t() * (SIGMA.mm(xexte)).t(),
dim=1).view(nt, 1)
kyf = (2.0 / math.pi) * torch.asin(2.0 * xSx_m / torch.sqrt(
(1 + 2.0 * xSxd).mm(1 + 2.0 * xSx_te_d.t())))
# solve
f_test = kyf.t().mm(v)
# tmp = torch.potrs(kfy.t(), c, upper=True)
# tmp = torch.sum(kfy * tmp.t(), dim=1)
# #cov_f = something - tmp
out = f_test #, cov_f)
return out
def test_with_terminate_on_inf():
torch.manual_seed(12)
data = [
1.0,
0.8,
torch.rand(4, 4),
(1.0 / torch.randint(0, 2, size=(4, )).type(torch.float),
torch.tensor(1.234)),
torch.rand(5),
torch.asin(torch.randn(4, 4)),
0.0,
1.0,
]
def update_fn(engine, batch):
return batch
trainer = Engine(update_fn)
h = TerminateOnNan()
trainer.add_event_handler(Events.ITERATION_COMPLETED, h)
trainer.run(data, max_epochs=2)
assert trainer.state.iteration == 4
def geo_final_displacement_error(pred_pos,
pred_pos_gt,
consider_ped=None,
mode='sum'):
real_pred_pos = torch.clone(pred_pos)
real_pred_pos_gt = torch.clone(pred_pos_gt)
real_pred_pos[:, 0] = pred_pos[:, 0] * lat_field + lat_min
real_pred_pos_gt[:, 0] = pred_pos_gt[:, 0] * lat_field + lat_min
real_pred_pos[:, 1] = pred_pos[:, 1] * long_field + long_min
real_pred_pos_gt[:, 1] = pred_pos_gt[:, 1] * long_field + long_min
real_pred_pos[:, 2] = pred_pos[:, 2] * alt_field + alt_min
real_pred_pos_gt[:, 2] = pred_pos_gt[:, 2] * alt_field + alt_min
angle_distance = make_radians(real_pred_pos_gt[:, :2]) - make_radians(
real_pred_pos[:, :2])
temp = torch.sin(angle_distance[:, 0] / 2)**2 + torch.cos(
real_pred_pos[:, 0]) * torch.cos(real_pred_pos_gt[:, 0]) * torch.sin(
angle_distance[:, 1] / 2)**2
ground_distance = 2 * torch.asin(torch.sqrt(temp)) * 6371 * 1000
loss = torch.sqrt((real_pred_pos_gt[:, 2] - real_pred_pos[:, 2])**2 +
ground_distance**2)
if consider_ped is not None:
loss = loss * consider_ped
if mode == 'raw':
return loss
else:
return torch.sum(loss)
def quaternion_to_euler(quaternion: torch.Tensor) -> torch.Tensor:
"""Convert quaternion vector to euler angles.
The quaternion should be in (x, y, z, w) format.
"""
if not torch.is_tensor(quaternion):
raise TypeError("Input type is not a torch.Tensor. Got {}".format(
type(quaternion)))
if not quaternion.shape[-1] == 4:
raise ValueError(
"Input must be a tensor of shape Nx4 or 4. Got {}".format(
quaternion.shape))
# unpack input and compute conversion
x: torch.Tensor = quaternion[..., 0]
y: torch.Tensor = quaternion[..., 1]
z: torch.Tensor = quaternion[..., 2]
w: torch.Tensor = quaternion[..., 3]
sinr_cosp = 2.0 * (w * x + y * z)
cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
roll = torch.atan2(sinr_cosp, cosr_cosp)
sinp = 2.0 * (w * y - z * x)
pitch = torch.where(torch.abs(sinp) > 1.0, pi/2. * torch.sign(sinp), torch.asin(sinp))
siny_cosp = 2.0 * (w * z + x * y)
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
yaw = torch.atan2(siny_cosp, cosy_cosp)
euler = torch.stack([roll, pitch, yaw], dim=1)
return euler
def rotation_matrix_to_euler(rotation_matrix: torch.Tensor):
''' Convert 3x3 rotation matrix to roll, pitch, yaw angles
Args:
From a paper by Gregory G. Slabaugh (undated),
"Computing Euler angles from a rotation matrix
'''
def isclose(x, y, rtol=1.e-5, atol=1.e-8):
return np.isclose(x, y, rtol=rtol, atol=atol)
batch_size = rotation_matrix.shape[0]
euler_angles = torch.zeros((batch_size, 3), dtype=rotation_matrix.dtype, device=rotation_matrix.device)
for i in range(batch_size):
R = rotation_matrix[i]
yaw = 0.0
if isclose(R[2,0], -1.0):
pitch = pi/2.0
roll = torch.atan2(R[0,1], R[0,2])
elif isclose(R[2,0], 1.0):
pitch = -pi/2.0
roll = torch.atan2(-R[0,1],-R[0,2])
else:
pitch = -torch.asin(R[2,0])
cos_pitch = torch.cos(pitch)
roll = torch.atan2(R[2,1]/cos_pitch, R[2,2]/cos_pitch)
yaw = torch.atan2(R[1,0]/cos_pitch, R[0,0]/cos_pitch)
euler_angles[i, 0] = roll
euler_angles[i, 1] = pitch
euler_angles[i, 2] = yaw
return euler_angles
def Rotate(self, batch, rotation, mode='bilinear'):
#
# batch is [batch_size x 3 x h x w]
# rotation is [batch x 3], x, y, z
#
assert mode in ['bilinear', 'nearest']
R = euler2mat(rotation).transpose(1, 2)
[batch_size, _, _, _] = batch.size()
tmp = []
xyz = self.xyz.cuda() if self.CUDA else self.xyz
for i in range(batch_size):
this_img = batch[i:i + 1, :, :, :]
new_xyz = torch.matmul(xyz, R[i:i + 1, :, :].transpose(1, 2))
x = torch.unsqueeze(new_xyz[:, :, :, 0], 3)
y = torch.unsqueeze(new_xyz[:, :, :, 1], 3)
z = torch.unsqueeze(new_xyz[:, :, :, 2], 3)
lon = torch.atan2(x, z) / np.pi
lat = torch.asin(y / self.RADIUS) / (0.5 * np.pi)
loc = torch.cat([lon, lat], dim=3)
#print this_img.size()
#print torch.max(loc)
#print torch.min(loc)
#exit()
new_img = F.grid_sample(this_img, loc, mode=mode)
tmp.append(new_img)
out = torch.cat(tmp, dim=0)
return out
def safe_asin(x):
"""
return pi/2 if x == 1, otherwise return asin(x)
"""
safe_x = torch.where(x < 1, x, torch.zeros_like(x))
return torch.where(x < 1, torch.asin(safe_x),
(math.pi / 2) * torch.ones_like(x))
def relu_q(rho, mu1, mu2):
"""
Compute exp ( -Q(rho, mu1, mu2) ) for ReLU activation
"""
rho_hat_plus_one = torch.sqrt(1 - rho * rho) + 1
g_r = torch.asin(rho) - rho / rho_hat_plus_one # why minus? why no brackets
rho_s = torch.abs(rho) + EPS
g_r_s = torch.abs(g_r) + EPS
A = g_r / (2 * math.pi)
coef_sum = rho_s / (2 * g_r_s * rho_hat_plus_one)
coef_prod = (torch.asin(rho) - rho) / (rho_s * g_r_s)
return A * torch.exp(
- (mu1 * mu1 + mu2 * mu2) * coef_sum + coef_prod * mu1 * mu2)
def heaviside_q(rho, mu1, mu2):
"""
Compute exp ( -Q(rho, mu1, mu2) ) for Heaviside activation
"""
rho_hat = torch.sqrt(1 - rho * rho)
arcsin = torch.asin(rho)
rho_s = torch.abs(rho) + EPS
arcsin_s = torch.abs(torch.asin(rho)) + EPS / 2
A = arcsin / (2 * math.pi)
one_over_coef_sum = (2 * arcsin_s * rho_hat) / rho_s
one_over_coefs_prod = (arcsin_s * rho_hat * (1 + rho_hat)) / (rho * rho)
return A * torch.exp(-(
mu1 * mu1 + mu2 * mu2) / one_over_coef_sum + mu1 * mu2 / one_over_coefs_prod)
def Q2E_batch(Q):
"""
batch version
https://www.cnblogs.com/21207-iHome/p/6894128.html
Quaternion to Euler angle
Q: Tensor (batch_size, 4) x, y, z, s
return :
Euler angle, rotation along the fixed axis x-y-z
phi, theta, psi shape:(batch_size, )
"""
len = torch.sqrt(torch.sum(Q * Q, dim=1, keepdim=True)).squeeze(1)
x = Q[:, 0] / len
y = Q[:, 1] / len
z = Q[:, 2] / len
s = Q[:, 3] / len
phi = torch.atan2(2 * (s * x + y * z), 1 - 2 * (x**2 + y**2))
theta = torch.asin(2 * (s * y - z * x))
psi = torch.atan2(2 * (s * z + x * y), 1 - 2 * (y**2 + z**2))
return phi, theta, psi
def camera_position_to_spherical_angle(camera_pose):
distance_o = torch.sum(camera_pose**2, axis=1)**.5
azimuth_o = torch.atan(camera_pose[:, 0] / camera_pose[:, 2]
) % np.pi + np.pi * (camera_pose[:, 0] < 0).type(
camera_pose.dtype).to(camera_pose.device)
elevation_o = torch.asin(camera_pose[:, 1] / distance_o)
return distance_o, elevation_o, azimuth_o
def Q2E(Q):
"""
https://www.cnblogs.com/21207-iHome/p/6894128.html
Quaternion to Euler angle
Q: Tensor (4, ) x, y, z, s
return :
Euler angle, rotation along the fixed axis x-y-z
phi, theta, psi
"""
len = torch.sqrt(torch.dot(Q, Q))
if len == 0:
x = Q[0] * len
y = Q[1] * len
z = Q[2] * len
s = Q[3] * len
else:
x = Q[0] / len
y = Q[1] / len
z = Q[2] / len
s = Q[3] / len
phi = torch.atan2(2 * (s * x + y * z), 1 - 2 * (x**2 + y**2))
theta = torch.asin(2 * (s * y - z * x))
psi = torch.atan2(2 * (s * z + x * y), 1 - 2 * (y**2 + z**2))
return phi, theta, psi
def matrix_to_euler_angles(matrix, convention: str):
"""
Convert rotations given as rotation matrices to Euler angles in radians.
Args:
matrix: Rotation matrices as tensor of shape (..., 3, 3).
convention: Convention string of three uppercase letters.
Returns:
Euler angles in radians as tensor of shape (..., 3).
"""
if len(convention) != 3:
raise ValueError("Convention must have 3 letters.")
if convention[1] in (convention[0], convention[2]):
raise ValueError(f"Invalid convention {convention}.")
for letter in convention:
if letter not in ("X", "Y", "Z"):
raise ValueError(f"Invalid letter {letter} in convention string.")
if matrix.size(-1) != 3 or matrix.size(-2) != 3:
raise ValueError(f"Invalid rotation matrix shape f{matrix.shape}.")
i0 = _index_from_letter(convention[0])
i2 = _index_from_letter(convention[2])
tait_bryan = i0 != i2
if tait_bryan:
central_angle = torch.asin(matrix[..., i0, i2] *
(-1.0 if i0 - i2 in [-1, 2] else 1.0))
else:
central_angle = torch.acos(matrix[..., i0, i0])
o = (
_angle_from_tan(convention[0], convention[1], matrix[..., i2], False,
tait_bryan),
central_angle,
_angle_from_tan(convention[2], convention[1], matrix[..., i0, :], True,
tait_bryan),
)
return torch.stack(o, -1)
