# Copyright 2020 Toyota Research Institute. All rights reserved.
import torch
import numpy as np
########################################################################################################################
[docs]def euler2mat(angle):
"""Convert euler angles to rotation matrix"""
B = angle.size(0)
x, y, z = angle[:, 0], angle[:, 1], angle[:, 2]
cosz = torch.cos(z)
sinz = torch.sin(z)
zeros = z.detach() * 0
ones = zeros.detach() + 1
zmat = torch.stack([cosz, -sinz, zeros,
sinz, cosz, zeros,
zeros, zeros, ones], dim=1).view(B, 3, 3)
cosy = torch.cos(y)
siny = torch.sin(y)
ymat = torch.stack([cosy, zeros, siny,
zeros, ones, zeros,
-siny, zeros, cosy], dim=1).view(B, 3, 3)
cosx = torch.cos(x)
sinx = torch.sin(x)
xmat = torch.stack([ones, zeros, zeros,
zeros, cosx, -sinx,
zeros, sinx, cosx], dim=1).view(B, 3, 3)
rot_mat = xmat.bmm(ymat).bmm(zmat)
return rot_mat
########################################################################################################################
[docs]def pose_vec2mat(vec, mode='euler'):
"""Convert Euler parameters to transformation matrix."""
if mode is None:
return vec
trans, rot = vec[:, :3].unsqueeze(-1), vec[:, 3:]
if mode == 'euler':
rot_mat = euler2mat(rot)
else:
raise ValueError('Rotation mode not supported {}'.format(mode))
mat = torch.cat([rot_mat, trans], dim=2) # [B,3,4]
return mat
########################################################################################################################
[docs]def invert_pose(T):
"""Inverts a [B,4,4] torch.tensor pose"""
Tinv = torch.eye(4, device=T.device, dtype=T.dtype).repeat([len(T), 1, 1])
Tinv[:, :3, :3] = torch.transpose(T[:, :3, :3], -2, -1)
Tinv[:, :3, -1] = torch.bmm(-1. * Tinv[:, :3, :3], T[:, :3, -1].unsqueeze(-1)).squeeze(-1)
return Tinv
########################################################################################################################
[docs]def invert_pose_numpy(T):
"""Inverts a [4,4] np.array pose"""
Tinv = np.copy(T)
R, t = Tinv[:3, :3], Tinv[:3, 3]
Tinv[:3, :3], Tinv[:3, 3] = R.T, - np.matmul(R.T, t)
return Tinv
########################################################################################################################