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)