def test_triplet_qma_xyzw(self, axis, device, dtype, atol, rtol):
array = [[0.0, 0.0, 0.0, 0.0]]
array[0][axis] = 1.0 # `0 + axis` should fail when WXYZ
quaternion = torch.tensor(array, device=device, dtype=dtype)
assert quaternion.shape[-1] == 4
with pytest.warns(UserWarning):
mm = kornia.quaternion_to_rotation_matrix(
quaternion, order=QuaternionCoeffOrder.XYZW)
assert mm.shape[-1] == 3
assert mm.shape[-2] == 3
angle_axis = kornia.rotation_matrix_to_angle_axis(mm)
assert angle_axis.shape[-1] == 3
angle_axis_expected = [[0.0, 0.0, 0.0]]
angle_axis_expected[0][axis] = kornia.pi
angle_axis_expected = torch.tensor(angle_axis_expected,
device=device,
dtype=dtype)
assert_allclose(angle_axis, angle_axis_expected, atol=atol, rtol=rtol)
with pytest.warns(UserWarning):
quaternion_hat = kornia.angle_axis_to_quaternion(
angle_axis, order=QuaternionCoeffOrder.XYZW)
assert_allclose(quaternion_hat, quaternion, atol=atol, rtol=rtol)
def matrot2aa(pose_matrot):
'''
:param pose_matrot: Nx1xnum_jointsx9
:return: Nx1xnum_jointsx3
'''
batch_size = pose_matrot.size(0)
homogen_matrot = F.pad(pose_matrot.view(-1, 3, 3), [0, 1])
pose = kornia.rotation_matrix_to_angle_axis(homogen_matrot).view(
batch_size, 1, -1, 3).contiguous()
return pose
def __init__(self, current_matrix: np.ndarray, next_matrix: np.ndarray):
self._current_position_name = "current_position"
self._current_angle_name = "current_angle"
self._next_position_name = "next_position"
self._next_angle_name = "next_angle"
self._delta_position_name = "delta_position"
self._delta_angle_name = "delta_angle"
self._current_angle = kornia.rotation_matrix_to_angle_axis(
torch.from_numpy(current_matrix[:3, :3].astype('float32')).reshape(
1, 3, 3)).permute(1, 0).squeeze()
self._next_angle = kornia.rotation_matrix_to_angle_axis(
torch.from_numpy(next_matrix[:3, :3].astype('float32')).reshape(
1, 3, 3)).permute(1, 0).squeeze()
self._current_position = torch.from_numpy(current_matrix[:3, 3])
self._next_position = torch.from_numpy(next_matrix[:3, 3])
delta_matrix = kornia.relative_transformation(
torch.from_numpy(current_matrix.astype('float32')),
torch.from_numpy(next_matrix.astype('float32')))
rotation_matrix = delta_matrix[:3, :3].reshape(1, 3, 3).clone()
self._delta_angle = kornia.rotation_matrix_to_angle_axis(
rotation_matrix).permute(1, 0).squeeze()
self._delta_position = delta_matrix[:3, 3]
def test_rotation_matrix_to_angle_axis(device_type):
device = torch.device(device_type)
rmat_1 = torch.tensor([[-0.30382753, -0.95095137, -0.05814062, 0.],
[-0.71581715, 0.26812278, -0.64476041, 0.],
[0.62872461, -0.15427791, -0.76217038, 0.]])
rvec_1 = torch.tensor([1.50485376, -2.10737739, 0.7214174])
rmat_2 = torch.tensor([[0.6027768, -0.79275544, -0.09054801, 0.],
[-0.67915707, -0.56931658, 0.46327563, 0.],
[-0.41881476, -0.21775548, -0.88157628, 0.]])
rvec_2 = torch.tensor([-2.44916812, 1.18053411, 0.4085298])
rmat = torch.stack([rmat_2, rmat_1], dim=0).to(device)
rvec = torch.stack([rvec_2, rvec_1], dim=0).to(device)
assert check_equal_torch(kornia.rotation_matrix_to_angle_axis(rmat), rvec)
def test_triplet_aqm(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
quaternion = kornia.angle_axis_to_quaternion(angle_axis, order=QuaternionCoeffOrder.WXYZ)
assert quaternion.shape[-1] == 4
rot_m = kornia.quaternion_to_rotation_matrix(quaternion, order=QuaternionCoeffOrder.WXYZ)
assert rot_m.shape[-1] == 3
assert rot_m.shape[-2] == 3
angle_axis_hat = kornia.rotation_matrix_to_angle_axis(rot_m)
assert_close(angle_axis_hat, angle_axis, atol=atol, rtol=rtol)
def test_angle_axis(self, axis_name, angle_deg, device, dtype, atol, rtol):
angle = (angle_deg * kornia.pi / 180.0).to(dtype).to(device).repeat(2, 1)
rot_m, axis = TestAngleOfRotations.axis_and_angle_to_rotation_matrix(
axis_name=axis_name, angle=angle, device=device, dtype=dtype
)
angle_axis = kornia.rotation_matrix_to_angle_axis(rot_m)
# compute angle_axis rotation angle
angle_hat = angle_axis.norm(p=2, dim=-1, keepdim=True)
# invert angle, if angle_axis axis points in the opposite direction of the original axis
dots = (angle_axis * 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_close(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_close(torch.abs(angle_hat), matrix_angle_abs, atol=atol, rtol=rtol)
def test_triplet_qma(self, axis, device, dtype, atol, rtol):
array = [[0.0, 0.0, 0.0, 0.0]]
array[0][1 + axis] = 1.0 # `1 + axis` this should fail when XYZW
quaternion = torch.tensor(array, device=device, dtype=dtype)
assert quaternion.shape[-1] == 4
mm = kornia.quaternion_to_rotation_matrix(quaternion, order=QuaternionCoeffOrder.WXYZ)
assert mm.shape[-1] == 3
assert mm.shape[-2] == 3
angle_axis = kornia.rotation_matrix_to_angle_axis(mm)
assert angle_axis.shape[-1] == 3
angle_axis_expected = [[0.0, 0.0, 0.0]]
angle_axis_expected[0][axis] = kornia.pi
angle_axis_expected = torch.tensor(angle_axis_expected, device=device, dtype=dtype)
assert_close(angle_axis, angle_axis_expected, atol=atol, rtol=rtol)
quaternion_hat = kornia.angle_axis_to_quaternion(angle_axis, order=QuaternionCoeffOrder.WXYZ)
assert_close(quaternion_hat, quaternion, atol=atol, rtol=rtol)
def test_triplet_aqm_xyzw(self, axis, device, dtype, atol, rtol):
array = [[0., 0., 0.]]
array[0][axis] = kornia.pi / 2.
angle_axis = torch.tensor(array, device=device, dtype=dtype)
assert angle_axis.shape[-1] == 3
with pytest.warns(UserWarning):
quaternion = kornia.angle_axis_to_quaternion(
angle_axis, order=QuaternionCoeffOrder.XYZW)
assert quaternion.shape[-1] == 4
with pytest.warns(UserWarning):
rot_m = kornia.quaternion_to_rotation_matrix(
quaternion, order=QuaternionCoeffOrder.XYZW)
assert rot_m.shape[-1] == 3
assert rot_m.shape[-2] == 3
angle_axis_hat = kornia.rotation_matrix_to_angle_axis(rot_m)
assert_allclose(angle_axis_hat, angle_axis, atol=atol, rtol=rtol)
import kornia as kn
import pickle
import math
import cv2
device = 'cuda'
data = pickle.load(open('demo_data/toyexample_6_data.p', 'rb'))
img = torch.tensor(cv2.imread('demo_data/toyexample_6.png',
cv2.IMREAD_GRAYSCALE),
device=device,
dtype=torch.float)[None, None, ...]
pts3d_gt = torch.tensor(data['3d_points'], device=device, dtype=torch.float)
n = pts3d_gt.size(0)
R = kn.rotation_matrix_to_angle_axis(torch.eye(3, device=device))
T = torch.tensor([0., 0., 0.], device=device)
P = torch.cat((R, T))[None, ...]
q_gt = kn.angle_axis_to_quaternion(P[0, 0:3])
K = torch.tensor(data['K'], device=device, dtype=torch.float)
pts2d_gt = BPnP.batch_project(P, pts3d_gt, K)
bpnp = BPnP.BPnP.apply
ite = 2000
pts2d = pts2d_gt.clone() + torch.round(10 * torch.randn_like(pts2d_gt))
pts2d.requires_grad_()
optimizer = torch.optim.SGD([{'params': pts2d}], lr=0.05)
# model = torchvision.models.vgg11()
