def logq_position_from_matrix(matrix_position):
translation = matrix_position[:, :3, 3]
rotation_matrix = matrix_position[:, :3, :3].clone()
quaternion = kornia.rotation_matrix_to_quaternion(rotation_matrix)
logq = kornia.quaternion_exp_to_log(quaternion)
position = torch.cat([translation, logq], dim=1)
return position
def test_log_quaternion(self, axis_name, angle_deg, device, dtype, atol,
rtol):
eps = torch.finfo(dtype).eps
angle = (angle_deg * kornia.pi / 180.0).to(dtype).to(device).repeat(
2, 1)
pi = torch.ones_like(angle) * kornia.pi
rot_m, axis = TestAngleOfRotations.axis_and_angle_to_rotation_matrix(
axis_name=axis_name, angle=angle, device=device, dtype=dtype)
quaternion = kornia.rotation_matrix_to_quaternion(
rot_m, eps=eps, order=QuaternionCoeffOrder.WXYZ)
log_q = kornia.quaternion_exp_to_log(quaternion,
eps=eps,
order=QuaternionCoeffOrder.WXYZ)
# compute angle_axis rotation angle
angle_hat = 2.0 * log_q.norm(p=2, dim=-1, keepdim=True)
# make sure it lands between [-pi..pi)
mask = pi < angle_hat
while torch.any(mask):
angle_hat = torch.where(mask, angle_hat - 2.0 * kornia.pi,
angle_hat)
mask = pi < angle_hat
# invert angle, if angle_axis axis points in the opposite direction of the original axis
dots = (log_q * axis).sum(dim=-1, keepdim=True)
angle_hat = torch.where(dots < 0.0, angle_hat * -1.0, angle_hat)
# angle_axis angle should match input angle
assert_allclose(angle_hat, angle, atol=atol, rtol=rtol)
# magnitude of angle should match matrix rotation angle
matrix_angle_abs = TestAngleOfRotations.matrix_angle_abs(rot_m)
assert_allclose(torch.abs(angle_hat),
matrix_angle_abs,
atol=atol,
rtol=rtol)
def test_unit_quaternion(self, device, dtype):
quaternion_exp = torch.tensor([0., 0., 0., 1.],
device=device,
dtype=dtype)
expected = torch.tensor([0., 0., 0.], device=device, dtype=dtype)
assert_allclose(kornia.quaternion_exp_to_log(quaternion_exp),
expected,
atol=1e-4,
rtol=1e-4)
def test_back_and_forth(self, device, dtype):
quaternion_exp = torch.tensor([1., 0., 0., 0.],
device=device,
dtype=dtype)
quaternion_log = kornia.quaternion_exp_to_log(quaternion_exp)
quaternion_exp_hat = kornia.quaternion_log_to_exp(quaternion_log)
assert_allclose(quaternion_exp,
quaternion_exp_hat,
atol=1e-4,
rtol=1e-4)
def test_smoke_batch(self, batch_size, device, dtype):
quaternion_exp = torch.zeros(batch_size, 4, device=device, dtype=dtype)
quaternion_log = kornia.quaternion_exp_to_log(quaternion_exp)
assert quaternion_log.shape == (batch_size, 3)
def test_back_and_forth(self, device, dtype):
quaternion_log = torch.tensor([0., 0., 0.], device=device, dtype=dtype)
quaternion_exp = kornia.quaternion_log_to_exp(quaternion_log)
quaternion_log_hat = kornia.quaternion_exp_to_log(quaternion_exp)
assert_allclose(quaternion_log, quaternion_log_hat)
def logq_from_quaternion(quaternion):
logq = kornia.quaternion_exp_to_log(quaternion)
return logq
def test_back_and_forth(self, device):
quaternion_exp = torch.tensor([1., 0., 0., 0.]).to(device)
quaternion_log = kornia.quaternion_exp_to_log(quaternion_exp)
quaternion_exp_hat = kornia.quaternion_log_to_exp(quaternion_log)
assert_allclose(quaternion_exp, quaternion_exp_hat)
def test_pi_quaternion(self, device):
quaternion_exp = torch.tensor([1., 0., 0., 0.]).to(device)
expected = torch.tensor([kornia.pi / 2, 0., 0.]).to(device)
assert_allclose(kornia.quaternion_exp_to_log(quaternion_exp), expected)
def test_unit_quaternion(self):
quaternion_exp = torch.tensor([0., 0., 0., 1.])
expected = torch.tensor([0., 0., 0.])
assert_allclose(kornia.quaternion_exp_to_log(quaternion_exp), expected)
