Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
726287fcbThis file is pinned to the selected release. Download the original to open it in your design tool.
import torch
from rotation_conversions import axis_angle_to_matrix, matrix_to_euler_angles
def euler_to_quaternion(r):
x = r[..., 0]
y = r[..., 1]
z = r[..., 2]
z = z / 2.0
y = y / 2.0
x = x / 2.0
cz = torch.cos(z)
sz = torch.sin(z)
cy = torch.cos(y)
sy = torch.sin(y)
cx = torch.cos(x)
sx = torch.sin(x)
quaternion = torch.zeros_like(r.repeat(1, 2))[..., :4].to(r.device)
quaternion[..., 0] += cx * cy * cz - sx * sy * sz
quaternion[..., 1] += cx * sy * sz + cy * cz * sx
quaternion[..., 2] += cx * cz * sy - sx * cy * sz
quaternion[..., 3] += cx * cy * sz + sx * cz * sy
return quaternion
def quaternion_to_angle_axis(quaternion: torch.Tensor):
"""Convert quaternion vector to angle axis of rotation. TODO: CORRECT.
Adapted from ceres C++ library: ceres-solver/include/ceres/rotation.h
Args:
quaternion (torch.Tensor): tensor with quaternions.
Return:
torch.Tensor: tensor with angle axis of rotation.
Shape:
- Input: :math:`(*, 4)` where `*` means, any number of dimensions
- Output: :math:`(*, 3)`
Example:
>>> quaternion = torch.rand(2, 4)
>>> angle_axis = tgm.quaternion_to_angle_axis(quaternion)
"""
if not torch.is_tensor(quaternion):
raise TypeError(f"Input type is not a torch.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}")
q1: torch.Tensor = quaternion[..., 1]
q2: torch.Tensor = quaternion[..., 2]
q3: torch.Tensor = quaternion[..., 3]
sin_squared_theta: torch.Tensor = q1 * q1 + q2 * q2 + q3 * q3
sin_theta: torch.Tensor = torch.sqrt(sin_squared_theta + 1e-11)
cos_theta: torch.Tensor = quaternion[..., 0]
two_theta: torch.Tensor = 2.0 * torch.where(
cos_theta < 0.0, torch.atan2(-sin_theta, -cos_theta), torch.atan2(sin_theta, cos_theta)
)
k_pos: torch.Tensor = two_theta / sin_theta
k_neg: torch.Tensor = 2.0 * torch.ones_like(sin_theta).to(quaternion.device)
k: torch.Tensor = torch.where(sin_squared_theta > 0.0, k_pos, k_neg)
angle_axis: torch.Tensor = torch.zeros_like(quaternion).to(quaternion.device)[..., :3]
angle_axis[..., 0] += q1 * k
angle_axis[..., 1] += q2 * k
angle_axis[..., 2] += q3 * k
return angle_axis
def batch_euler2axis(r):
return quaternion_to_angle_axis(euler_to_quaternion(r))
def batch_axis2euler(theta):
return matrix_to_euler_angles(axis_angle_to_matrix(theta), "XYZ")