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