From e0882ea32bb13e62275b678ddb60915058397d35 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Wed, 23 Jan 2019 09:41:36 -0800 Subject: [PATCH 1/4] implement relative_pose function --- test/test_transformations.py | 71 ++++++++++++++++++++++++++++++++ torchgeometry/__init__.py | 1 + torchgeometry/transformations.py | 44 ++++++++++++++++++++ 3 files changed, 116 insertions(+) create mode 100644 test/test_transformations.py create mode 100644 torchgeometry/transformations.py diff --git a/test/test_transformations.py b/test/test_transformations.py new file mode 100644 index 0000000000..80d146ef02 --- /dev/null +++ b/test/test_transformations.py @@ -0,0 +1,71 @@ +import pytest + +import torch +import torchgeometry as tgm +from torch.autograd import gradcheck + +import utils # test utilities +from common import TEST_DEVICES + +class TestTransformPose: + def test_identity(self): + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) + pose_21 = tgm.relative_pose(pose_1, pose_2) + assert utils.check_equal_torch(pose_21, torch.eye(4).unsqueeze(0)) + + def test_translation(self): + offset = 10. + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) + pose_2[..., :3, -1:] += offset # add translation + + # compute relative pose + pose_21 = tgm.relative_pose(pose_1, pose_2) + assert utils.check_equal_torch(pose_21[...,:3, -1:], offset) + + def test_rotation(self): + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.zeros(1, 4, 4) # Rz (90deg) + pose_2[..., 0, 1] = -1.0 + pose_2[..., 1, 0] = 1.0 + pose_2[..., 2, 2] = 1.0 + pose_2[..., 3, 3] = 1.0 + + # compute relative pose + pose_21 = tgm.relative_pose(pose_1, pose_2) + assert utils.check_equal_torch(pose_21, pose_2) + + @pytest.mark.parametrize("device_type", TEST_DEVICES) + @pytest.mark.parametrize("batch_size", [1, 2, 5]) + def test_integration(self, batch_size, device_type): + pose_1 = torch.eye(4).repeat(batch_size, 1, 1) + pose_1 = pose_1.to(torch.device(device_type)) + + pose_2 = torch.eye(4).repeat(batch_size, 1, 1) + pose_2[..., :3, :3] = torch.rand(batch_size, 3, 3) + pose_2[..., :3, -1:] = torch.rand(batch_size, 3, 1) + pose_2 = pose_2.to(torch.device(device_type)) + + pose_21 = tgm.relative_pose(pose_1, pose_2) + assert utils.check_equal_torch( + torch.matmul(pose_21, pose_1), pose_2) + + @pytest.mark.skip("Converting a tensor to a Python boolean ...") + def test_jit(self): + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) + + pose_21 = tgm.relative_pose(pose_1, pose_2) + pose_21_jit = torch.jit.trace( + tgm.relative_pose, (pose_1, pose_2,))(pose_1, pose_2) + assert utils.check_equal_torch(pose_21, pose_21_jit) + + def test_gradcheck(self): + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) + + pose_1 = utils.tensor_to_gradcheck_var(pose_1) # to var + pose_2 = utils.tensor_to_gradcheck_var(pose_2) # to var + assert gradcheck(tgm.relative_pose, (pose_1, pose_2,), + raise_exception=True) diff --git a/torchgeometry/__init__.py b/torchgeometry/__init__.py index 9126467b8c..645a9da739 100644 --- a/torchgeometry/__init__.py +++ b/torchgeometry/__init__.py @@ -4,6 +4,7 @@ from .conversions import * from .utils import * from .imgwarp import * +from .transformations import * from torchgeometry import image from torchgeometry import losses diff --git a/torchgeometry/transformations.py b/torchgeometry/transformations.py new file mode 100644 index 0000000000..d55caa1282 --- /dev/null +++ b/torchgeometry/transformations.py @@ -0,0 +1,44 @@ +from typing import Optional + +import torch + + +__all__ = [ + "relative_pose", +] + + +def relative_pose(pose_1: torch.Tensor, pose_2: torch.Tensor, eps: Optional[float] = 1e-6) -> torch.Tensor: + """Function that computes the relative transformation from a reference + pose 1 to 2. + """ + if not torch.is_tensor(pose_1): + raise TypeError("Input pose_1 type is not a torch.Tensor. Got {}" + .format(type(pose_1))) + if not torch.is_tensor(pose_2): + raise TypeError("Input pose_2 type is not a torch.Tensor. Got {}" + .format(type(pose_2))) + if not (len(pose_1.shape) == 3 and pose_1.shape[-2:] == (4, 4)): + raise ValueError("Input must be a of the shape Nx4x4." + " Got {}".format(pose_1.shape, pose_2.shape)) + if not pose_1.shape == pose_2.shape: + raise ValueError("Input pose_1 and pose_2 must be a of the same shape." + " Got {}".format(pose_1.shape, pose_2.shape)) + # unpack input data + rot_mat_1 = pose_1[..., :3, :3] # Nx3x3 + rot_mat_2 = pose_2[..., :3, :3] # Nx3x3 + t_vec_1 = pose_1[..., :3, -1:] # Nx3x1 + t_vec_2 = pose_2[..., :3, -1:] # Nx3x1 + + # compute relative pose + rot_mat_21 = torch.matmul(rot_mat_2, rot_mat_1.transpose(1, 2)) + t_vec_21 = t_vec_2 - torch.matmul(rot_mat_21, t_vec_1) + + # pack output data + pose_21 = torch.zeros_like(pose_1) + pose_21[..., :3, :3] = rot_mat_21 + pose_21[..., :3, -1:] = t_vec_21 + pose_21[..., -1, -1] += 1.0 + return pose_21 + eps + + From cc1899004c2bb5869678bac256b55c755a476389 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Wed, 23 Jan 2019 20:45:47 +0100 Subject: [PATCH 2/4] add relative_pose documentation --- docs/source/index.rst | 1 + docs/source/transformations.rst | 6 +++++ torchgeometry/transformations.py | 39 ++++++++++++++++++++++++-------- 3 files changed, 37 insertions(+), 9 deletions(-) create mode 100644 docs/source/transformations.rst diff --git a/docs/source/index.rst b/docs/source/index.rst index a6f4311562..ba0ca7b458 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -21,6 +21,7 @@ TGM focuses on Image and tensor warping functions such as: :caption: Package Reference geometric + transformations pinhole conversions warping diff --git a/docs/source/transformations.rst b/docs/source/transformations.rst new file mode 100644 index 0000000000..1a48949452 --- /dev/null +++ b/docs/source/transformations.rst @@ -0,0 +1,6 @@ +Linear Transformations +---------------------- + +.. currentmodule:: torchgeometry + +.. autofunction:: relative_pose diff --git a/torchgeometry/transformations.py b/torchgeometry/transformations.py index d55caa1282..119898a22d 100644 --- a/torchgeometry/transformations.py +++ b/torchgeometry/transformations.py @@ -9,8 +9,30 @@ def relative_pose(pose_1: torch.Tensor, pose_2: torch.Tensor, eps: Optional[float] = 1e-6) -> torch.Tensor: - """Function that computes the relative transformation from a reference - pose 1 to 2. + r"""Function that computes the relative transformation from a reference + pose :math:`P_1^{\{W\}} = \begin{bmatrix} R_1 & t_1 \\ \mathbf{0} & 1 \end{bmatrix}` to destination :math:`P_2^{\{W\}} = \begin{bmatrix} R_2 & t_2 \\ \mathbf{0} & 1 \end{bmatrix}`. + + The relative transformation is computed as follows: + + .. math:: + + P_1^{2} = \begin{bmatrix} R_2 R_1^T & R_1^T (t_2 - t_1) \\ \mathbf{0} & 1\end{bmatrix} + + Arguments: + pose_1 (torch.Tensor): reference pose tensor of shape :math:`(N, 4, 4)`. + pose_2 (torch.Tensor): destination pose tensor of shape :math:`(N, 4, 4)`. + + Shape: + - Output: :math:`(N, 4, 4)` + + Returns: + torch.Tensor: the relative transformation between the poses. + + Example:: + + >>> pose_1 = torch.eye(4).unsqueeze(0) # 1x4x4 + >>> pose_2 = torch.eye(4).unsqueeze(0) # 1x4x4 + >>> pose_21 = tgm.relative_pose(pose_1, pose_2) # 1x4x4 """ if not torch.is_tensor(pose_1): raise TypeError("Input pose_1 type is not a torch.Tensor. Got {}" @@ -25,20 +47,19 @@ def relative_pose(pose_1: torch.Tensor, pose_2: torch.Tensor, eps: Optional[floa raise ValueError("Input pose_1 and pose_2 must be a of the same shape." " Got {}".format(pose_1.shape, pose_2.shape)) # unpack input data - rot_mat_1 = pose_1[..., :3, :3] # Nx3x3 - rot_mat_2 = pose_2[..., :3, :3] # Nx3x3 + r_mat_1 = pose_1[..., :3, :3] # Nx3x3 + r_mat_2 = pose_2[..., :3, :3] # Nx3x3 t_vec_1 = pose_1[..., :3, -1:] # Nx3x1 t_vec_2 = pose_2[..., :3, -1:] # Nx3x1 # compute relative pose - rot_mat_21 = torch.matmul(rot_mat_2, rot_mat_1.transpose(1, 2)) - t_vec_21 = t_vec_2 - torch.matmul(rot_mat_21, t_vec_1) + r_mat_1_trans = r_mat_1.transpose(1, 2) + r_mat_21 = torch.matmul(r_mat_2, r_mat_1_trans) + t_vec_21 = torch.matmul(r_mat_1_trans, t_vec_2 - t_vec_1) # pack output data pose_21 = torch.zeros_like(pose_1) - pose_21[..., :3, :3] = rot_mat_21 + pose_21[..., :3, :3] = r_mat_21 pose_21[..., :3, -1:] = t_vec_21 pose_21[..., -1, -1] += 1.0 return pose_21 + eps - - From a8b85d8e139ec6602f2dc4cdc14b1b2c9c0372ba Mon Sep 17 00:00:00 2001 From: edgarriba Date: Wed, 23 Jan 2019 20:54:34 +0100 Subject: [PATCH 3/4] apply autopep8 format --- test/test_transformations.py | 47 ++++++++++++++++---------------- torchgeometry/transformations.py | 16 +++++++---- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/test/test_transformations.py b/test/test_transformations.py index 80d146ef02..83b9ddddb7 100644 --- a/test/test_transformations.py +++ b/test/test_transformations.py @@ -7,12 +7,13 @@ import utils # test utilities from common import TEST_DEVICES + class TestTransformPose: def test_identity(self): pose_1 = torch.eye(4).unsqueeze(0) pose_2 = torch.eye(4).unsqueeze(0) pose_21 = tgm.relative_pose(pose_1, pose_2) - assert utils.check_equal_torch(pose_21, torch.eye(4).unsqueeze(0)) + assert utils.check_equal_torch(pose_21, torch.eye(4).unsqueeze(0)) def test_translation(self): offset = 10. @@ -22,7 +23,7 @@ def test_translation(self): # compute relative pose pose_21 = tgm.relative_pose(pose_1, pose_2) - assert utils.check_equal_torch(pose_21[...,:3, -1:], offset) + assert utils.check_equal_torch(pose_21[..., :3, -1:], offset) def test_rotation(self): pose_1 = torch.eye(4).unsqueeze(0) @@ -39,33 +40,33 @@ def test_rotation(self): @pytest.mark.parametrize("device_type", TEST_DEVICES) @pytest.mark.parametrize("batch_size", [1, 2, 5]) def test_integration(self, batch_size, device_type): - pose_1 = torch.eye(4).repeat(batch_size, 1, 1) - pose_1 = pose_1.to(torch.device(device_type)) + pose_1 = torch.eye(4).repeat(batch_size, 1, 1) + pose_1 = pose_1.to(torch.device(device_type)) - pose_2 = torch.eye(4).repeat(batch_size, 1, 1) - pose_2[..., :3, :3] = torch.rand(batch_size, 3, 3) - pose_2[..., :3, -1:] = torch.rand(batch_size, 3, 1) - pose_2 = pose_2.to(torch.device(device_type)) + pose_2 = torch.eye(4).repeat(batch_size, 1, 1) + pose_2[..., :3, :3] = torch.rand(batch_size, 3, 3) + pose_2[..., :3, -1:] = torch.rand(batch_size, 3, 1) + pose_2 = pose_2.to(torch.device(device_type)) - pose_21 = tgm.relative_pose(pose_1, pose_2) - assert utils.check_equal_torch( - torch.matmul(pose_21, pose_1), pose_2) + pose_21 = tgm.relative_pose(pose_1, pose_2) + assert utils.check_equal_torch( + torch.matmul(pose_21, pose_1), pose_2) @pytest.mark.skip("Converting a tensor to a Python boolean ...") def test_jit(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) - pose_21 = tgm.relative_pose(pose_1, pose_2) - pose_21_jit = torch.jit.trace( - tgm.relative_pose, (pose_1, pose_2,))(pose_1, pose_2) - assert utils.check_equal_torch(pose_21, pose_21_jit) + pose_21 = tgm.relative_pose(pose_1, pose_2) + pose_21_jit = torch.jit.trace( + tgm.relative_pose, (pose_1, pose_2,))(pose_1, pose_2) + assert utils.check_equal_torch(pose_21, pose_21_jit) def test_gradcheck(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + pose_1 = torch.eye(4).unsqueeze(0) + pose_2 = torch.eye(4).unsqueeze(0) - pose_1 = utils.tensor_to_gradcheck_var(pose_1) # to var - pose_2 = utils.tensor_to_gradcheck_var(pose_2) # to var - assert gradcheck(tgm.relative_pose, (pose_1, pose_2,), - raise_exception=True) + pose_1 = utils.tensor_to_gradcheck_var(pose_1) # to var + pose_2 = utils.tensor_to_gradcheck_var(pose_2) # to var + assert gradcheck(tgm.relative_pose, (pose_1, pose_2,), + raise_exception=True) diff --git a/torchgeometry/transformations.py b/torchgeometry/transformations.py index 119898a22d..6c46d9e454 100644 --- a/torchgeometry/transformations.py +++ b/torchgeometry/transformations.py @@ -8,19 +8,25 @@ ] -def relative_pose(pose_1: torch.Tensor, pose_2: torch.Tensor, eps: Optional[float] = 1e-6) -> torch.Tensor: +def relative_pose(pose_1: torch.Tensor, pose_2: torch.Tensor, + eps: Optional[float] = 1e-6) -> torch.Tensor: r"""Function that computes the relative transformation from a reference - pose :math:`P_1^{\{W\}} = \begin{bmatrix} R_1 & t_1 \\ \mathbf{0} & 1 \end{bmatrix}` to destination :math:`P_2^{\{W\}} = \begin{bmatrix} R_2 & t_2 \\ \mathbf{0} & 1 \end{bmatrix}`. + pose :math:`P_1^{\{W\}} = \begin{bmatrix} R_1 & t_1 \\ \mathbf{0} & 1 + \end{bmatrix}` to destination :math:`P_2^{\{W\}} = \begin{bmatrix} R_2 & + t_2 \\ \mathbf{0} & 1 \end{bmatrix}`. The relative transformation is computed as follows: .. math:: - P_1^{2} = \begin{bmatrix} R_2 R_1^T & R_1^T (t_2 - t_1) \\ \mathbf{0} & 1\end{bmatrix} + P_1^{2} = \begin{bmatrix} R_2 R_1^T & R_1^T (t_2 - t_1) \\ \mathbf{0} & + 1\end{bmatrix} Arguments: - pose_1 (torch.Tensor): reference pose tensor of shape :math:`(N, 4, 4)`. - pose_2 (torch.Tensor): destination pose tensor of shape :math:`(N, 4, 4)`. + pose_1 (torch.Tensor): reference pose tensor of shape + :math:`(N, 4, 4)`. + pose_2 (torch.Tensor): destination pose tensor of shape + :math:`(N, 4, 4)`. Shape: - Output: :math:`(N, 4, 4)` From 792d32fe40ce3a0a8fca978800d76d2ba122c1fd Mon Sep 17 00:00:00 2001 From: edgarriba Date: Wed, 23 Jan 2019 21:21:09 +0100 Subject: [PATCH 4/4] refactor test_transformations --- test/test_transformations.py | 64 +++++++++++++++++++++++------------- 1 file changed, 41 insertions(+), 23 deletions(-) diff --git a/test/test_transformations.py b/test/test_transformations.py index 83b9ddddb7..2941a00eac 100644 --- a/test/test_transformations.py +++ b/test/test_transformations.py @@ -9,25 +9,30 @@ class TestTransformPose: - def test_identity(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + + def _generate_identity_matrix(self, batch_size, device_type): + eye = torch.eye(4).repeat(batch_size, 1, 1) # Nx4x4 + return eye.to(torch.device(device_type)) + + def _test_identity(self): + pose_1 = self.pose_1.clone() + pose_2 = self.pose_2.clone() pose_21 = tgm.relative_pose(pose_1, pose_2) assert utils.check_equal_torch(pose_21, torch.eye(4).unsqueeze(0)) - def test_translation(self): + def _test_translation(self): offset = 10. - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + pose_1 = self.pose_1.clone() + pose_2 = self.pose_2.clone() pose_2[..., :3, -1:] += offset # add translation # compute relative pose pose_21 = tgm.relative_pose(pose_1, pose_2) assert utils.check_equal_torch(pose_21[..., :3, -1:], offset) - def test_rotation(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.zeros(1, 4, 4) # Rz (90deg) + def _test_rotation(self): + pose_1 = self.pose_1.clone() + pose_2 = torch.zeros_like(pose_1) # Rz (90deg) pose_2[..., 0, 1] = -1.0 pose_2[..., 1, 0] = 1.0 pose_2[..., 2, 2] = 1.0 @@ -37,16 +42,14 @@ def test_rotation(self): pose_21 = tgm.relative_pose(pose_1, pose_2) assert utils.check_equal_torch(pose_21, pose_2) - @pytest.mark.parametrize("device_type", TEST_DEVICES) - @pytest.mark.parametrize("batch_size", [1, 2, 5]) - def test_integration(self, batch_size, device_type): - pose_1 = torch.eye(4).repeat(batch_size, 1, 1) - pose_1 = pose_1.to(torch.device(device_type)) + def _test_integration(self): + pose_1 = self.pose_1.clone() + pose_2 = self.pose_2.clone() - pose_2 = torch.eye(4).repeat(batch_size, 1, 1) - pose_2[..., :3, :3] = torch.rand(batch_size, 3, 3) - pose_2[..., :3, -1:] = torch.rand(batch_size, 3, 1) - pose_2 = pose_2.to(torch.device(device_type)) + # apply random rotations and translations + batch_size, device = pose_2.shape[0], pose_2.device + pose_2[..., :3, :3] = torch.rand(batch_size, 3, 3, device=device) + pose_2[..., :3, -1:] = torch.rand(batch_size, 3, 1, device=device) pose_21 = tgm.relative_pose(pose_1, pose_2) assert utils.check_equal_torch( @@ -54,19 +57,34 @@ def test_integration(self, batch_size, device_type): @pytest.mark.skip("Converting a tensor to a Python boolean ...") def test_jit(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + pose_1 = self.pose_1.clone() + pose_2 = self.pose_2.clone() pose_21 = tgm.relative_pose(pose_1, pose_2) pose_21_jit = torch.jit.trace( tgm.relative_pose, (pose_1, pose_2,))(pose_1, pose_2) assert utils.check_equal_torch(pose_21, pose_21_jit) - def test_gradcheck(self): - pose_1 = torch.eye(4).unsqueeze(0) - pose_2 = torch.eye(4).unsqueeze(0) + def _test_gradcheck(self): + pose_1 = self.pose_1.clone() + pose_2 = self.pose_2.clone() pose_1 = utils.tensor_to_gradcheck_var(pose_1) # to var pose_2 = utils.tensor_to_gradcheck_var(pose_2) # to var assert gradcheck(tgm.relative_pose, (pose_1, pose_2,), raise_exception=True) + + @pytest.mark.parametrize("device_type", TEST_DEVICES) + @pytest.mark.parametrize("batch_size", [1, 2, 5]) + def test_run_all(self, batch_size, device_type): + # generate identity matrices + self.pose_1 = self._generate_identity_matrix( + batch_size, device_type) + self.pose_2 = self.pose_1.clone() + + # run tests + self._test_identity() + self._test_translation() + self._test_rotation() + self._test_integration() + self._test_gradcheck()