Source code for packnet_sfm.geometry.pose_utils

# 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
########################################################################################################################