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_quaternion(self, axis_name, angle_deg, device, dtype, atol, rtol):
eps = torch.finfo(dtype).eps
angle = torch.tensor((angle_deg * kornia.pi / 180.0, ),
device=device,
dtype=dtype).repeat(2, 1)
pi = torch.ones_like(angle) * kornia.pi
assert 2 <= len(angle.shape)
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)
# compute quaternion rotation angle
# See Section 2.4.4 Equation (105a) in https://arxiv.org/pdf/1711.02508.pdf
angle_hat = 2.0 * torch.atan2(
quaternion[..., 1:4].norm(p=2, dim=-1, keepdim=True),
quaternion[..., 0:1])
# 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 quaternion axis points in the opposite direction of the original axis
dots = (quaternion[..., 1:4] * axis).sum(dim=-1, keepdim=True)
angle_hat = torch.where(dots < 0.0, angle_hat * -1.0, angle_hat)
# quaternion 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_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_back_and_forth(self, device):
matrix = torch.tensor([
[1., 0., 0.],
[0., 0., -1.],
[0., 1., 0.],
]).to(device)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
matrix_hat = kornia.quaternion_to_rotation_matrix(quaternion)
assert_allclose(matrix, matrix_hat)
def test_identity(self, device):
matrix = torch.tensor([
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
]).to(device)
expected = torch.tensor([0., 0., 0., 1.], ).to(device)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
assert_allclose(quaternion, expected)
def test_rot_x_45(self, device):
matrix = torch.tensor([
[1., 0., 0.],
[0., 0., -1.],
[0., 1., 0.],
]).to(device)
pi_half2 = torch.cos(kornia.pi / 4).to(device)
expected = torch.tensor([pi_half2, 0., 0., pi_half2], ).to(device)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
assert_allclose(quaternion, expected)
def test_identity(self, device, dtype):
matrix = torch.tensor([
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
], device=device, dtype=dtype)
expected = torch.tensor(
[0., 0., 0., 1.], device=device, dtype=dtype)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
assert_allclose(quaternion, expected, atol=1e-4, rtol=1e-4)
def test_corner_case(self, device, dtype):
matrix = torch.tensor([
[-0.7799533010, -0.5432914495, 0.3106555045],
[0.0492402576, -0.5481169224, -0.8349509239],
[0.6238971353, -0.6359263659, 0.4542570710]
], device=device, dtype=dtype)
quaternion_true = torch.tensor([0.280136495828629, -0.440902262926102,
0.834015488624573, 0.177614107728004], device=device, dtype=dtype)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
torch.set_printoptions(precision=10)
assert_allclose(quaternion_true, quaternion)
def test_triplet_amq(self, axis, device, dtype, atol, rtol):
array = [[0.0, 0.0, 0.0]]
array[0][axis] = kornia.pi / 2.0
angle_axis = torch.tensor(array, device=device, dtype=dtype)
assert angle_axis.shape[-1] == 3
rot_m = kornia.angle_axis_to_rotation_matrix(angle_axis)
assert rot_m.shape[-1] == 3
assert rot_m.shape[-2] == 3
quaternion = kornia.rotation_matrix_to_quaternion(rot_m, order=QuaternionCoeffOrder.WXYZ)
assert quaternion.shape[-1] == 4
angle_axis_hat = kornia.quaternion_to_angle_axis(quaternion, order=QuaternionCoeffOrder.WXYZ)
assert_close(angle_axis_hat, angle_axis, atol=atol, rtol=rtol)
def test_triplet_qam_xyzw(self, axis, device, dtype, atol, rtol):
array = [[0.0, 0.0, 0.0, 0.0]]
array[0][axis] = 1.0
quaternion = torch.tensor(array, device=device, dtype=dtype)
assert quaternion.shape[-1] == 4
with pytest.warns(UserWarning):
angle_axis = kornia.quaternion_to_angle_axis(quaternion, order=QuaternionCoeffOrder.XYZW)
assert angle_axis.shape[-1] == 3
rot_m = kornia.angle_axis_to_rotation_matrix(angle_axis)
assert rot_m.shape[-1] == 3
assert rot_m.shape[-2] == 3
with pytest.warns(UserWarning):
quaternion_hat = kornia.rotation_matrix_to_quaternion(rot_m, order=QuaternionCoeffOrder.XYZW)
assert_close(quaternion_hat, quaternion, atol=atol, rtol=rtol)
def test_smoke_batch(self, batch_size, device, dtype):
matrix = torch.zeros(batch_size, 3, 3, device=device, dtype=dtype)
quaternion = kornia.rotation_matrix_to_quaternion(matrix)
assert quaternion.shape == (batch_size, 4)
def quaternion_from_matrix(matrix_position):
rotation_matrix = matrix_position[:, :3, :3].clone()
return kornia.rotation_matrix_to_quaternion(rotation_matrix)
# dynamic
mesh = trimesh.load(pclist[i])
mesh.vertices[:, :2] *= -1
overts = torch.Tensor(mesh.vertices).cuda()
except:
pass
# extract camera in/ex
verts = overts.clone()
#rotmat = cv2.Rodrigues(np.asarray([6.28*i/nframes,0.,0.]))[0] # x-axis
#rotmat = cv2.Rodrigues(np.asarray([-0.05*6.28, 0.10*6.28*(-0.5+i/nframes),0.]))[0] # y-axis
rotmat = cv2.Rodrigues(
np.asarray([-0.02 * 6.28, 0.25 * 6.28 * (-0.5 + i / nframes),
0.]))[0] # y-axis
#rotmat = cv2.Rodrigues(np.asarray([-0.01*6.28, 0.10*6.28*(-0.5+i/nframes),0.]))[0] # y-axis temple
#rotmat = cv2.Rodrigues(np.asarray([ 0.05*6.28, 0.10*6.28*(-0.5+i/nframes),0.]))[0] # y-axis temple
quat = kornia.rotation_matrix_to_quaternion(torch.Tensor(rotmat).cuda())
proj_cam = torch.zeros(1, 7).cuda()
depth = torch.zeros(1, 1).cuda()
proj_cam[:, 0] = 5 # focal=10
proj_cam[:, 1] = 0. # x translation = 0
proj_cam[:, 2] = 0. # y translation = 0
proj_cam[:, 3] = quat[3]
proj_cam[:, 4:] = quat[:3]
#depth[:,0] = 0.05 # for temple
depth[:, 0] = 1 # z translation (depth) =10 for spot
#depth[:,0] = 0.5 # z translation (depth) =10 for spot
#depth[:,0] = 200 # for kitti
# obj-cam transform
Rmat = kornia.quaternion_to_rotation_matrix(
torch.cat((-proj_cam[:, 4:], proj_cam[:, 3:4]), 1))
