From bec2df898a2f452b3eaee2f270b5038492339100 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 11:10:32 +0000 Subject: [PATCH 01/14] left jacobian implementation for SE2 --- liegroups/numpy/se2.py | 36 ++++++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 0baa085..1250381 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -42,7 +42,7 @@ def adjoint(self): \\end{bmatrix} \\in \\mathbb{R}^{3 \\times 3} """ - rot_part = self.rot.as_matrix() + rot_part = self.rot.as_matrix trans_part = np.array([self.trans[1], -self.trans[0]]).reshape((2, 1)) return np.vstack([np.hstack([rot_part, trans_part]), [0, 0, 1]]) @@ -85,7 +85,39 @@ def left_jacobian(cls, xi): .. math:: \\mathcal{J}(\\boldsymbol{\\xi}) """ - raise NotImplementedError + + # based on https://github.com/artivis/manif/blob/6f2c1cd3e050a2a232cc5f6c4fb0d33b74f08701/include/manif/impl/se2/SE2Tangent_base.h + + se2 = cls.exp(xi) + theta = se2.rot.to_angle() + x = se2.trans[0] + y = se2.trans[1] + + cos_theta = np.cos(theta) + sin_theta = np.sin(theta) + theta_sq = theta * theta + + if theta_sq < 1e-15: + A = 1 - 1./6. * theta_sq + B = 0.5 * theta - 1./24. * theta * theta_sq + else: + A = sin_theta / theta + B = (1 - cos_theta) / theta + + jac = np.zeros((cls.dof, cls.dof)) + jac[0][0] = A + jac[0][1] = -B + jac[1][0] = B + jac[1][1] = A + + if theta_sq < 1e-15: + jac[0][2] = y / 2. + theta * x / 6. + jac[1][2] = -x / 2. + theta * y / 6. + else: + jac[0][2] = ( y + theta*x - y*cos_theta - x*sin_theta)/theta_sq + jac[1][2] = (-x + theta*y + x*cos_theta - y*sin_theta)/theta_sq + + return jac def log(self): """Logarithmic map for :math:`SE(2)`, which computes a tangent vector from a transformation: From 8b93ad0dd884ebdcd93cf2e6643e8cc1c6e67922 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 11:18:16 +0000 Subject: [PATCH 02/14] fix bottom right element of SE2 left jacobian --- liegroups/numpy/se2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 1250381..991fe9b 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -117,6 +117,8 @@ def left_jacobian(cls, xi): jac[0][2] = ( y + theta*x - y*cos_theta - x*sin_theta)/theta_sq jac[1][2] = (-x + theta*y + x*cos_theta - y*sin_theta)/theta_sq + jac[2][2] = 1 + return jac def log(self): From 329fd1822f11e4fadf9523253997469e492bde14 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 11:19:29 +0000 Subject: [PATCH 03/14] add unit test in anticipation of inv left jacobian implementation --- tests/numpy/test_se2_numpy.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tests/numpy/test_se2_numpy.py b/tests/numpy/test_se2_numpy.py index 54c059b..bb49899 100644 --- a/tests/numpy/test_se2_numpy.py +++ b/tests/numpy/test_se2_numpy.py @@ -103,3 +103,16 @@ def test_transform_vectorized(): assert np.allclose(Tpt2, Tpts12[1]) assert np.allclose(Tpt3, Tpts34[0]) assert np.allclose(Tpt4, Tpts34[1]) + +def test_left_jacobian(): + xi1 = [1, 2, 3] + assert np.allclose( + SE2.left_jacobian(xi1).dot(SE2.inv_left_jacobian(xi1)), + np.identity(3) + ) + + xi2 = [0, 0, 0] + assert np.allclose( + SE2.left_jacobian(xi2).dot(SE2.inv_left_jacobian(xi2)), + np.identity(3) + ) \ No newline at end of file From aa70a039e4a82803dd45f3596174c2ce0bb2c3d5 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 13:33:11 +0000 Subject: [PATCH 04/14] using MATLAB symbolic toolbox to implement inverse left jacobian for SE2 group - FAILS for small tangent vecs (theta) --- liegroups/numpy/se2.py | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 991fe9b..315795c 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -76,7 +76,28 @@ def inv_left_jacobian(cls, xi): .. math:: \\mathcal{J}^{-1}(\\boldsymbol{\\xi}) """ - raise NotImplementedError + se2 = cls.exp(xi) + theta = se2.rot.to_angle() + x = se2.trans[0] + y = se2.trans[1] + + cos_theta = np.cos(theta) + sin_theta = np.sin(theta) + theta_sq = theta * theta + + jac = np.zeros((cls.dof, cls.dof)) + + jac[0][0] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[0][1] = -(theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[0][2] = (theta*(x - 2*cos_theta*x - theta*y + cos_theta**2*x + sin_theta**2*x + cos_theta*theta*y - sin_theta*theta*x))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) + jac[1][0] = (theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[1][1] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[1][2] = (theta*(y - 2*cos_theta*y + theta*x + cos_theta**2*y + sin_theta**2*y - cos_theta*theta*x - sin_theta*theta*y))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) + jac[2][0] = 0 + jac[2][1] = 0 + jac[2][2] = 1 + + return jac @classmethod def left_jacobian(cls, xi): From 9480679e7fbf13cc8f63252de41983232f621e53 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 13:51:07 +0000 Subject: [PATCH 05/14] MATLAB symbolic toolbox solves small tangent vec case - test passes --- liegroups/numpy/se2.py | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 315795c..17c2eb9 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -87,15 +87,26 @@ def inv_left_jacobian(cls, xi): jac = np.zeros((cls.dof, cls.dof)) - jac[0][0] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[0][1] = -(theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[0][2] = (theta*(x - 2*cos_theta*x - theta*y + cos_theta**2*x + sin_theta**2*x + cos_theta*theta*y - sin_theta*theta*x))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) - jac[1][0] = (theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[1][1] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[1][2] = (theta*(y - 2*cos_theta*y + theta*x + cos_theta**2*y + sin_theta**2*y - cos_theta*theta*x - sin_theta*theta*y))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) - jac[2][0] = 0 - jac[2][1] = 0 - jac[2][2] = 1 + if theta_sq > 1e-15: + jac[0][0] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[0][1] = -(theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[0][2] = (theta*(x - 2*cos_theta*x - theta*y + cos_theta**2*x + sin_theta**2*x + cos_theta*theta*y - sin_theta*theta*x))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) + jac[1][0] = (theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[1][1] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) + jac[1][2] = (theta*(y - 2*cos_theta*y + theta*x + cos_theta**2*y + sin_theta**2*y - cos_theta*theta*x - sin_theta*theta*y))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) + jac[2][0] = 0 + jac[2][1] = 0 + jac[2][2] = 1 + else: + jac[0][0] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[0][1] = -(24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[0][2] = (4*(12*theta*x - 72*y + 12*theta_sq*y - 12*theta_sq*y + theta_sq*theta_sq*y + theta_sq*theta*x))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[1][0] = (24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[1][1] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[1][2] = (4*(72*x - 12*theta_sq*x + 12*theta*y + 12*theta_sq*x - theta_sq*theta_sq*x + theta_sq*theta*y))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[2][0] = 0 + jac[2][1] = 0 + jac[2][2] = 1 return jac From 420b9046e407de35bce2ca699054634d20372dcb Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 13:57:12 +0000 Subject: [PATCH 06/14] adds torch unit tests in anticipation of left/inv jacobians for SE2 groups --- tests/torch/test_se2_torch.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/tests/torch/test_se2_torch.py b/tests/torch/test_se2_torch.py index f0980f3..a2bc9ac 100644 --- a/tests/torch/test_se2_torch.py +++ b/tests/torch/test_se2_torch.py @@ -309,3 +309,25 @@ def test_adjoint_batch(): T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6]])) assert T.adjoint().shape == (2, 3, 3) + +def test_left_jacobian(): + xi1 = torch.Tensor([1, 2, 3]) + assert utils.allclose( + torch.mm(SE2.left_jacobian(xi1), SE2.inv_left_jacobian(xi1)), + torch.eye(3) + ) + + xi2 = torch.Tensor([0, 0, 0]) + assert utils.allclose( + torch.mm(SE2.left_jacobian(xi2), SE2.inv_left_jacobian(xi2)), + torch.eye(3) + ) + + +def test_left_jacobian_batch(): + xis = torch.Tensor([[1, 2, 3], + [0, 0, 0]]) + assert utils.allclose( + SE2.left_jacobian(xis).bmm(SE2.inv_left_jacobian(xis)), + torch.eye(3).unsqueeze_(dim=0).expand(2, 3, 3) + ) \ No newline at end of file From d94f41082fbe78d3ed7b3399309e5e3792ccd370 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 14:19:42 +0000 Subject: [PATCH 07/14] left jacobian SE2 implemented in torch --- liegroups/torch/se2.py | 41 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/liegroups/torch/se2.py b/liegroups/torch/se2.py index dbf5169..acd91f3 100644 --- a/liegroups/torch/se2.py +++ b/liegroups/torch/se2.py @@ -64,7 +64,46 @@ def inv_left_jacobian(cls, xi): @classmethod def left_jacobian(cls, xi): - raise NotImplementedError + + x = xi[:, 0] # translation part + y = xi[:, 2] # translation part + theta = xi[:, 2] # rotation part + + cos_theta = torch.cos(theta) + sin_theta = torch.sin(theta) + theta_sq = theta * theta + + small_angle_mask = utils.isclose(theta_sq, 0.) + small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) + + large_angle_mask = small_angle_mask.logical_not() + large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) + + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)) + + jac[small_angle_inds, 0, 0] = 1 - 1./6. * theta_sq[small_angle_inds] + jac[large_angle_inds, 0, 0] = sin_theta[large_angle_inds] / theta[large_angle_inds] + + jac[small_angle_inds, 0, 1] = -(0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds]) + jac[large_angle_inds, 0, 1] = -(1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] + + jac[small_angle_inds, 0, 2] = y[small_angle_inds] / 2. + theta[small_angle_inds] * x[small_angle_inds] / 6. + jac[large_angle_inds, 0, 2] = ( y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] - y[large_angle_inds]*cos_theta[large_angle_inds] - x[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] + + jac[small_angle_inds, 1, 0] = 0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds] + jac[large_angle_inds, 1, 0] = (1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] + + jac[small_angle_inds, 1, 1] = 1 - 1./6. * theta_sq[small_angle_inds] + jac[large_angle_inds, 1, 1] = sin_theta[large_angle_inds] / theta[large_angle_inds] + + jac[small_angle_inds, 1, 2] = -x[small_angle_inds] / 2. + theta[small_angle_inds] * y[small_angle_inds] / 6. + jac[large_angle_inds, 1, 2] = (-x[large_angle_inds] + theta[large_angle_inds]*y[large_angle_inds] + x[large_angle_inds]*cos_theta[large_angle_inds] - y[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] + + jac[:, 2, 0] = 0 + jac[:, 2, 1] = 0 + jac[:, 2, 2] = 1 + + return jac def log(self): phi = self.rot.log() From 97d76dd60cfdc751f0a1459453a3c843a84da20c Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 14:50:38 +0000 Subject: [PATCH 08/14] torch implementations of left jacobian (and inverse) implemented and pass unit test(s) --- liegroups/numpy/se2.py | 10 +++--- liegroups/torch/se2.py | 71 ++++++++++++++++++++++++++++++++++-------- 2 files changed, 62 insertions(+), 19 deletions(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 17c2eb9..29a1166 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -94,9 +94,6 @@ def inv_left_jacobian(cls, xi): jac[1][0] = (theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) jac[1][1] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) jac[1][2] = (theta*(y - 2*cos_theta*y + theta*x + cos_theta**2*y + sin_theta**2*y - cos_theta*theta*x - sin_theta*theta*y))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) - jac[2][0] = 0 - jac[2][1] = 0 - jac[2][2] = 1 else: jac[0][0] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) jac[0][1] = -(24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) @@ -104,9 +101,10 @@ def inv_left_jacobian(cls, xi): jac[1][0] = (24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) jac[1][1] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) jac[1][2] = (4*(72*x - 12*theta_sq*x + 12*theta*y + 12*theta_sq*x - theta_sq*theta_sq*x + theta_sq*theta*y))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[2][0] = 0 - jac[2][1] = 0 - jac[2][2] = 1 + + jac[2][0] = 0 + jac[2][1] = 0 + jac[2][2] = 1 return jac diff --git a/liegroups/torch/se2.py b/liegroups/torch/se2.py index acd91f3..dae36a8 100644 --- a/liegroups/torch/se2.py +++ b/liegroups/torch/se2.py @@ -60,13 +60,62 @@ def exp(cls, xi): @classmethod def inv_left_jacobian(cls, xi): - raise NotImplementedError + + if xi.dim() < 2: + xi = xi.unsqueeze(dim=0) + + if xi.shape[1] != cls.dof: + raise ValueError( + "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof)) + + x = xi[:, 0] # translation part + y = xi[:, 1] # translation part + theta = xi[:, 2] # rotation part + + cos_theta = torch.cos(theta) + sin_theta = torch.sin(theta) + theta_sq = theta * theta + + small_angle_mask = utils.isclose(theta_sq, 0.) + small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) + + large_angle_mask = small_angle_mask.logical_not() + large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) + + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)) + + jac[small_angle_inds, 0, 0] = -(96*(theta_sq[small_angle_inds] - 6))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + jac[small_angle_inds, 0, 1] = -(24*theta[small_angle_inds]*(theta_sq[small_angle_inds] - 12))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + jac[small_angle_inds, 0, 2] = (4*(12*theta[small_angle_inds]*x[small_angle_inds] - 72*y[small_angle_inds] + 12*theta_sq[small_angle_inds]*y[small_angle_inds] - 12*theta_sq[small_angle_inds]*y[small_angle_inds] + theta_sq[small_angle_inds]*theta_sq[small_angle_inds]*y[small_angle_inds] + theta_sq[small_angle_inds]*theta[small_angle_inds]*x[small_angle_inds]))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 0] = (24*theta[small_angle_inds]*(theta_sq[small_angle_inds] - 12))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 1] = -(96*(theta_sq[small_angle_inds] - 6))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 2] = (4*(72*x[small_angle_inds] - 12*theta_sq[small_angle_inds]*x[small_angle_inds] + 12*theta[small_angle_inds]*y[small_angle_inds] + 12*theta_sq[small_angle_inds]*x[small_angle_inds] - theta_sq[small_angle_inds]*theta_sq[small_angle_inds]*x[small_angle_inds] + theta_sq[small_angle_inds]*theta[small_angle_inds]*y[small_angle_inds]))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) + + jac[large_angle_inds, 0, 0] = (sin_theta[large_angle_inds]*theta[large_angle_inds])/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) + jac[large_angle_inds, 0, 1] = -(theta[large_angle_inds]*(cos_theta[large_angle_inds] - 1))/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) + jac[large_angle_inds, 0, 2] = (theta[large_angle_inds]*(x[large_angle_inds] - 2*cos_theta[large_angle_inds]*x[large_angle_inds] - theta[large_angle_inds]*y[large_angle_inds] + cos_theta[large_angle_inds]**2*x[large_angle_inds] + sin_theta[large_angle_inds]**2*x[large_angle_inds] + cos_theta[large_angle_inds]*theta[large_angle_inds]*y[large_angle_inds] - sin_theta[large_angle_inds]*theta[large_angle_inds]*x[large_angle_inds]))/(theta_sq[large_angle_inds]*(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1)) + jac[large_angle_inds, 1, 0] = (theta[large_angle_inds]*(cos_theta[large_angle_inds] - 1))/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) + jac[large_angle_inds, 1, 1] = (sin_theta[large_angle_inds]*theta[large_angle_inds])/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) + jac[large_angle_inds, 1, 2] = (theta[large_angle_inds]*(y[large_angle_inds] - 2*cos_theta[large_angle_inds]*y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] + cos_theta[large_angle_inds]**2*y[large_angle_inds] + sin_theta[large_angle_inds]**2*y[large_angle_inds] - cos_theta[large_angle_inds]*theta[large_angle_inds]*x[large_angle_inds] - sin_theta[large_angle_inds]*theta[large_angle_inds]*y[large_angle_inds]))/(theta_sq[large_angle_inds]*(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1)) + + jac[:, 2, 0] = 0 + jac[:, 2, 1] = 0 + jac[:, 2, 2] = 1 + + return jac.squeeze_() @classmethod def left_jacobian(cls, xi): + if xi.dim() < 2: + xi = xi.unsqueeze(dim=0) + + if xi.shape[1] != cls.dof: + raise ValueError( + "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof)) + x = xi[:, 0] # translation part - y = xi[:, 2] # translation part + y = xi[:, 1] # translation part theta = xi[:, 2] # rotation part cos_theta = torch.cos(theta) @@ -82,28 +131,24 @@ def left_jacobian(cls, xi): jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)) jac[small_angle_inds, 0, 0] = 1 - 1./6. * theta_sq[small_angle_inds] - jac[large_angle_inds, 0, 0] = sin_theta[large_angle_inds] / theta[large_angle_inds] - jac[small_angle_inds, 0, 1] = -(0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds]) - jac[large_angle_inds, 0, 1] = -(1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] - jac[small_angle_inds, 0, 2] = y[small_angle_inds] / 2. + theta[small_angle_inds] * x[small_angle_inds] / 6. - jac[large_angle_inds, 0, 2] = ( y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] - y[large_angle_inds]*cos_theta[large_angle_inds] - x[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] - jac[small_angle_inds, 1, 0] = 0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds] - jac[large_angle_inds, 1, 0] = (1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] - jac[small_angle_inds, 1, 1] = 1 - 1./6. * theta_sq[small_angle_inds] - jac[large_angle_inds, 1, 1] = sin_theta[large_angle_inds] / theta[large_angle_inds] - jac[small_angle_inds, 1, 2] = -x[small_angle_inds] / 2. + theta[small_angle_inds] * y[small_angle_inds] / 6. + + jac[large_angle_inds, 0, 0] = sin_theta[large_angle_inds] / theta[large_angle_inds] + jac[large_angle_inds, 0, 1] = -(1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] + jac[large_angle_inds, 0, 2] = ( y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] - y[large_angle_inds]*cos_theta[large_angle_inds] - x[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] + jac[large_angle_inds, 1, 0] = (1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] + jac[large_angle_inds, 1, 1] = sin_theta[large_angle_inds] / theta[large_angle_inds] jac[large_angle_inds, 1, 2] = (-x[large_angle_inds] + theta[large_angle_inds]*y[large_angle_inds] + x[large_angle_inds]*cos_theta[large_angle_inds] - y[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] jac[:, 2, 0] = 0 jac[:, 2, 1] = 0 jac[:, 2, 2] = 1 - return jac + return jac.squeeze_() def log(self): phi = self.rot.log() From db18c52008630972079ce98e30304600dda31e91 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 15:23:53 +0000 Subject: [PATCH 09/14] ensure se2 jacs are on the correct device --- liegroups/torch/se2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/liegroups/torch/se2.py b/liegroups/torch/se2.py index dae36a8..4fe1044 100644 --- a/liegroups/torch/se2.py +++ b/liegroups/torch/se2.py @@ -82,7 +82,7 @@ def inv_left_jacobian(cls, xi): large_angle_mask = small_angle_mask.logical_not() large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) - jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)) + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) jac[small_angle_inds, 0, 0] = -(96*(theta_sq[small_angle_inds] - 6))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) jac[small_angle_inds, 0, 1] = -(24*theta[small_angle_inds]*(theta_sq[small_angle_inds] - 12))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) @@ -128,7 +128,7 @@ def left_jacobian(cls, xi): large_angle_mask = small_angle_mask.logical_not() large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) - jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)) + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) jac[small_angle_inds, 0, 0] = 1 - 1./6. * theta_sq[small_angle_inds] jac[small_angle_inds, 0, 1] = -(0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds]) From a391c39aa35f7caa26b36d17f4feed147a5658ca Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 16:19:35 +0000 Subject: [PATCH 10/14] fixing a few device issues --- liegroups/torch/_base.py | 4 ++-- liegroups/torch/so2.py | 18 +++++++++--------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/liegroups/torch/_base.py b/liegroups/torch/_base.py index ff095d4..f27ed88 100644 --- a/liegroups/torch/_base.py +++ b/liegroups/torch/_base.py @@ -112,11 +112,11 @@ def is_valid_matrix(cls, mat): # Determinants of each matrix in the batch should be 1 det_check = utils.isclose(mat.__class__( - np.linalg.det(mat.detach().cpu().numpy())), 1.) + np.linalg.det(mat.detach().cpu().numpy())).to(mat.device), 1.) # The transpose of each matrix in the batch should be its inverse inv_check = utils.isclose(mat.transpose(2, 1).bmm(mat), - torch.eye(cls.dim, dtype=mat.dtype)).sum(dim=1).sum(dim=1) \ + torch.eye(cls.dim, dtype=mat.dtype, device=mat.device)).sum(dim=1).sum(dim=1) \ == cls.dim * cls.dim return shape_check & det_check & inv_check diff --git a/liegroups/torch/so2.py b/liegroups/torch/so2.py index 1a282bc..6e42900 100644 --- a/liegroups/torch/so2.py +++ b/liegroups/torch/so2.py @@ -42,14 +42,14 @@ def inv_left_jacobian(cls, phi): if phi.dim() < 1: phi = phi.unsqueeze(dim=0) - jac = phi.__class__(phi.shape[0], cls.dim, cls.dim) + jac = phi.__class__(phi.shape[0], cls.dim, cls.dim).to(phi.device) # Near phi==0, use first order Taylor expansion small_angle_mask = utils.isclose(phi, 0.) small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) if len(small_angle_inds) > 0: - jac[small_angle_inds] = torch.eye(cls.dim).expand( + jac[small_angle_inds] = torch.eye(cls.dim).to(phi.device).expand( len(small_angle_inds), cls.dim, cls.dim) \ - 0.5 * cls.wedge(phi[small_angle_inds]) @@ -68,9 +68,9 @@ def inv_left_jacobian(cls, phi): dim=2).expand_as(jac[large_angle_inds]) A = hacha * \ - torch.eye(cls.dim).unsqueeze_( + torch.eye(cls.dim).to(phi.device).unsqueeze_( dim=0).expand_as(jac[large_angle_inds]) - B = -ha * cls.wedge(phi.__class__([1.])) + B = -ha * cls.wedge(phi.__class__([1.]).to(phi.device)) jac[large_angle_inds] = A + B @@ -82,14 +82,14 @@ def left_jacobian(cls, phi): if phi.dim() < 1: phi = phi.unsqueeze(dim=0) - jac = phi.__class__(phi.shape[0], cls.dim, cls.dim) + jac = phi.__class__(phi.shape[0], cls.dim, cls.dim).to(phi.device) # Near phi==0, use first order Taylor expansion small_angle_mask = utils.isclose(phi, 0.) small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) if len(small_angle_inds) > 0: - jac[small_angle_inds] = torch.eye(cls.dim).expand( + jac[small_angle_inds] = torch.eye(cls.dim).to(phi.device).expand( len(small_angle_inds), cls.dim, cls.dim) \ + 0.5 * cls.wedge(phi[small_angle_inds]) @@ -104,11 +104,11 @@ def left_jacobian(cls, phi): A = (s / angle).unsqueeze_(dim=1).unsqueeze_( dim=2).expand_as(jac[large_angle_inds]) * \ - torch.eye(cls.dim).unsqueeze_(dim=0).expand_as( + torch.eye(cls.dim).to(phi.device).unsqueeze_(dim=0).expand_as( jac[large_angle_inds]) - B = ((1. - c) / angle).unsqueeze_(dim=1).unsqueeze_( + B = ((1.0 - c) / angle).unsqueeze_(dim=1).unsqueeze_( dim=2).expand_as(jac[large_angle_inds]) * \ - cls.wedge(phi.__class__([1.])) + cls.wedge(phi.__class__([1.]).to(phi.device)) jac[large_angle_inds] = A + B From 6c87b28053d8eb3a238942599f010290c3211b29 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Sun, 3 Jan 2021 16:26:26 +0000 Subject: [PATCH 11/14] cleaning up prior PR --- liegroups/numpy/se2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 29a1166..c42c677 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -42,7 +42,7 @@ def adjoint(self): \\end{bmatrix} \\in \\mathbb{R}^{3 \\times 3} """ - rot_part = self.rot.as_matrix + rot_part = self.rot.as_matrix() trans_part = np.array([self.trans[1], -self.trans[0]]).reshape((2, 1)) return np.vstack([np.hstack([rot_part, trans_part]), [0, 0, 1]]) From 721d7fd8f0de88b89342936eb6f64cf8a7b57406 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Fri, 8 Jan 2021 15:24:41 +0000 Subject: [PATCH 12/14] take translation and rotation parts from tangent vector --- liegroups/numpy/se2.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index c42c677..4407898 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -76,10 +76,9 @@ def inv_left_jacobian(cls, xi): .. math:: \\mathcal{J}^{-1}(\\boldsymbol{\\xi}) """ - se2 = cls.exp(xi) - theta = se2.rot.to_angle() - x = se2.trans[0] - y = se2.trans[1] + x = xi[0] # translation part + y = xi[1] # translation part + theta = xi[2] # rotation part cos_theta = np.cos(theta) sin_theta = np.sin(theta) @@ -118,10 +117,9 @@ def left_jacobian(cls, xi): # based on https://github.com/artivis/manif/blob/6f2c1cd3e050a2a232cc5f6c4fb0d33b74f08701/include/manif/impl/se2/SE2Tangent_base.h - se2 = cls.exp(xi) - theta = se2.rot.to_angle() - x = se2.trans[0] - y = se2.trans[1] + x = xi[0] # translation part + y = xi[1] # translation part + theta = xi[2] # rotation part cos_theta = np.cos(theta) sin_theta = np.sin(theta) From 242f0e37c397bb078bb6bd339e8f36c971c88e88 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Fri, 8 Jan 2021 15:28:55 +0000 Subject: [PATCH 13/14] numpy implementation matches notation with SE(3) for trans/rot parts of tangent vec --- liegroups/numpy/se2.py | 68 ++++++++++++++++++++---------------------- 1 file changed, 33 insertions(+), 35 deletions(-) diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 4407898..8e8cc24 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -76,30 +76,29 @@ def inv_left_jacobian(cls, xi): .. math:: \\mathcal{J}^{-1}(\\boldsymbol{\\xi}) """ - x = xi[0] # translation part - y = xi[1] # translation part - theta = xi[2] # rotation part + rho = xi[0:2] # translation part + phi = xi[2] # rotation part - cos_theta = np.cos(theta) - sin_theta = np.sin(theta) - theta_sq = theta * theta + cos_phi = np.cos(phi) + sin_phi = np.sin(phi) + phi_sq = phi * phi jac = np.zeros((cls.dof, cls.dof)) - if theta_sq > 1e-15: - jac[0][0] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[0][1] = -(theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[0][2] = (theta*(x - 2*cos_theta*x - theta*y + cos_theta**2*x + sin_theta**2*x + cos_theta*theta*y - sin_theta*theta*x))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) - jac[1][0] = (theta*(cos_theta - 1))/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[1][1] = (sin_theta*theta)/(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1) - jac[1][2] = (theta*(y - 2*cos_theta*y + theta*x + cos_theta**2*y + sin_theta**2*y - cos_theta*theta*x - sin_theta*theta*y))/(theta_sq*(cos_theta**2 - 2*cos_theta + sin_theta**2 + 1)) + if phi_sq > 1e-15: + jac[0][0] = (sin_phi*phi)/(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1) + jac[0][1] = -(phi*(cos_phi - 1))/(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1) + jac[0][2] = (phi*(rho[0] - 2*cos_phi*rho[0] - phi*rho[1] + cos_phi**2*rho[0] + sin_phi**2*rho[0] + cos_phi*phi*rho[1] - sin_phi*phi*rho[0]))/(phi_sq*(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1)) + jac[1][0] = (phi*(cos_phi - 1))/(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1) + jac[1][1] = (sin_phi*phi)/(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1) + jac[1][2] = (phi*(rho[1] - 2*cos_phi*rho[1] + phi*rho[0] + cos_phi**2*rho[1] + sin_phi**2*rho[1] - cos_phi*phi*rho[0] - sin_phi*phi*rho[1]))/(phi_sq*(cos_phi**2 - 2*cos_phi + sin_phi**2 + 1)) else: - jac[0][0] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[0][1] = -(24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[0][2] = (4*(12*theta*x - 72*y + 12*theta_sq*y - 12*theta_sq*y + theta_sq*theta_sq*y + theta_sq*theta*x))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[1][0] = (24*theta*(theta_sq - 12))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[1][1] = -(96*(theta_sq - 6))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) - jac[1][2] = (4*(72*x - 12*theta_sq*x + 12*theta*y + 12*theta_sq*x - theta_sq*theta_sq*x + theta_sq*theta*y))/(theta_sq**2*theta_sq + 16*theta_sq**2 - 24*theta_sq*theta_sq - 192*theta_sq + 144*theta_sq + 576) + jac[0][0] = -(96*(phi_sq - 6))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) + jac[0][1] = -(24*phi*(phi_sq - 12))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) + jac[0][2] = (4*(12*phi*rho[0] - 72*rho[1] + 12*phi_sq*rho[1] - 12*phi_sq*rho[1] + phi_sq*phi_sq*rho[1] + phi_sq*phi*rho[0]))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) + jac[1][0] = (24*phi*(phi_sq - 12))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) + jac[1][1] = -(96*(phi_sq - 6))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) + jac[1][2] = (4*(72*rho[0] - 12*phi_sq*rho[0] + 12*phi*rho[1] + 12*phi_sq*rho[0] - phi_sq*phi_sq*rho[0] + phi_sq*phi*rho[1]))/(phi_sq**2*phi_sq + 16*phi_sq**2 - 24*phi_sq*phi_sq - 192*phi_sq + 144*phi_sq + 576) jac[2][0] = 0 jac[2][1] = 0 @@ -117,20 +116,19 @@ def left_jacobian(cls, xi): # based on https://github.com/artivis/manif/blob/6f2c1cd3e050a2a232cc5f6c4fb0d33b74f08701/include/manif/impl/se2/SE2Tangent_base.h - x = xi[0] # translation part - y = xi[1] # translation part - theta = xi[2] # rotation part + rho = xi[0:2] # translation part + phi = xi[2] # rotation part - cos_theta = np.cos(theta) - sin_theta = np.sin(theta) - theta_sq = theta * theta + cos_phi = np.cos(phi) + sin_phi = np.sin(phi) + phi_sq = phi * phi - if theta_sq < 1e-15: - A = 1 - 1./6. * theta_sq - B = 0.5 * theta - 1./24. * theta * theta_sq + if phi_sq < 1e-15: + A = 1 - 1./6. * phi_sq + B = 0.5 * phi - 1./24. * phi * phi_sq else: - A = sin_theta / theta - B = (1 - cos_theta) / theta + A = sin_phi / phi + B = (1 - cos_phi) / phi jac = np.zeros((cls.dof, cls.dof)) jac[0][0] = A @@ -138,12 +136,12 @@ def left_jacobian(cls, xi): jac[1][0] = B jac[1][1] = A - if theta_sq < 1e-15: - jac[0][2] = y / 2. + theta * x / 6. - jac[1][2] = -x / 2. + theta * y / 6. + if phi_sq < 1e-15: + jac[0][2] = rho[1] / 2. + phi * rho[0] / 6. + jac[1][2] = -rho[0] / 2. + phi * rho[1] / 6. else: - jac[0][2] = ( y + theta*x - y*cos_theta - x*sin_theta)/theta_sq - jac[1][2] = (-x + theta*y + x*cos_theta - y*sin_theta)/theta_sq + jac[0][2] = ( rho[1] + phi*rho[0] - rho[1]*cos_phi - rho[0]*sin_phi)/phi_sq + jac[1][2] = (-rho[0] + phi*rho[1] + rho[0]*cos_phi - rho[1]*sin_phi)/phi_sq jac[2][2] = 1 From ca637bd461300d70d70f90bff7a18462d06f5f82 Mon Sep 17 00:00:00 2001 From: Matt Gadd Date: Fri, 8 Jan 2021 15:34:53 +0000 Subject: [PATCH 14/14] torch implementation matches notation with SE(3) for trans/rot parts of tangent vec --- liegroups/torch/se2.py | 78 ++++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 40 deletions(-) diff --git a/liegroups/torch/se2.py b/liegroups/torch/se2.py index 4fe1044..f7130df 100644 --- a/liegroups/torch/se2.py +++ b/liegroups/torch/se2.py @@ -68,15 +68,14 @@ def inv_left_jacobian(cls, xi): raise ValueError( "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof)) - x = xi[:, 0] # translation part - y = xi[:, 1] # translation part - theta = xi[:, 2] # rotation part + rho = xi[:, 0:2] # translation part + phi = xi[:, 2] # rotation part - cos_theta = torch.cos(theta) - sin_theta = torch.sin(theta) - theta_sq = theta * theta + cos_phi = torch.cos(phi) + sin_phi = torch.sin(phi) + phi_sq = phi * phi - small_angle_mask = utils.isclose(theta_sq, 0.) + small_angle_mask = utils.isclose(phi_sq, 0.) small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) large_angle_mask = small_angle_mask.logical_not() @@ -84,19 +83,19 @@ def inv_left_jacobian(cls, xi): jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) - jac[small_angle_inds, 0, 0] = -(96*(theta_sq[small_angle_inds] - 6))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - jac[small_angle_inds, 0, 1] = -(24*theta[small_angle_inds]*(theta_sq[small_angle_inds] - 12))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - jac[small_angle_inds, 0, 2] = (4*(12*theta[small_angle_inds]*x[small_angle_inds] - 72*y[small_angle_inds] + 12*theta_sq[small_angle_inds]*y[small_angle_inds] - 12*theta_sq[small_angle_inds]*y[small_angle_inds] + theta_sq[small_angle_inds]*theta_sq[small_angle_inds]*y[small_angle_inds] + theta_sq[small_angle_inds]*theta[small_angle_inds]*x[small_angle_inds]))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - jac[small_angle_inds, 1, 0] = (24*theta[small_angle_inds]*(theta_sq[small_angle_inds] - 12))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - jac[small_angle_inds, 1, 1] = -(96*(theta_sq[small_angle_inds] - 6))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - jac[small_angle_inds, 1, 2] = (4*(72*x[small_angle_inds] - 12*theta_sq[small_angle_inds]*x[small_angle_inds] + 12*theta[small_angle_inds]*y[small_angle_inds] + 12*theta_sq[small_angle_inds]*x[small_angle_inds] - theta_sq[small_angle_inds]*theta_sq[small_angle_inds]*x[small_angle_inds] + theta_sq[small_angle_inds]*theta[small_angle_inds]*y[small_angle_inds]))/(theta_sq[small_angle_inds]**2*theta_sq[small_angle_inds] + 16*theta_sq[small_angle_inds]**2 - 24*theta_sq[small_angle_inds]*theta_sq[small_angle_inds] - 192*theta_sq[small_angle_inds] + 144*theta_sq[small_angle_inds] + 576) - - jac[large_angle_inds, 0, 0] = (sin_theta[large_angle_inds]*theta[large_angle_inds])/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) - jac[large_angle_inds, 0, 1] = -(theta[large_angle_inds]*(cos_theta[large_angle_inds] - 1))/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) - jac[large_angle_inds, 0, 2] = (theta[large_angle_inds]*(x[large_angle_inds] - 2*cos_theta[large_angle_inds]*x[large_angle_inds] - theta[large_angle_inds]*y[large_angle_inds] + cos_theta[large_angle_inds]**2*x[large_angle_inds] + sin_theta[large_angle_inds]**2*x[large_angle_inds] + cos_theta[large_angle_inds]*theta[large_angle_inds]*y[large_angle_inds] - sin_theta[large_angle_inds]*theta[large_angle_inds]*x[large_angle_inds]))/(theta_sq[large_angle_inds]*(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1)) - jac[large_angle_inds, 1, 0] = (theta[large_angle_inds]*(cos_theta[large_angle_inds] - 1))/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) - jac[large_angle_inds, 1, 1] = (sin_theta[large_angle_inds]*theta[large_angle_inds])/(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1) - jac[large_angle_inds, 1, 2] = (theta[large_angle_inds]*(y[large_angle_inds] - 2*cos_theta[large_angle_inds]*y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] + cos_theta[large_angle_inds]**2*y[large_angle_inds] + sin_theta[large_angle_inds]**2*y[large_angle_inds] - cos_theta[large_angle_inds]*theta[large_angle_inds]*x[large_angle_inds] - sin_theta[large_angle_inds]*theta[large_angle_inds]*y[large_angle_inds]))/(theta_sq[large_angle_inds]*(cos_theta[large_angle_inds]**2 - 2*cos_theta[large_angle_inds] + sin_theta[large_angle_inds]**2 + 1)) + jac[small_angle_inds, 0, 0] = -(96*(phi_sq[small_angle_inds] - 6))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + jac[small_angle_inds, 0, 1] = -(24*phi[small_angle_inds]*(phi_sq[small_angle_inds] - 12))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + jac[small_angle_inds, 0, 2] = (4*(12*phi[small_angle_inds]*rho[small_angle_inds,0] - 72*rho[small_angle_inds,1] + 12*phi_sq[small_angle_inds]*rho[small_angle_inds,1] - 12*phi_sq[small_angle_inds]*rho[small_angle_inds,1] + phi_sq[small_angle_inds]*phi_sq[small_angle_inds]*rho[small_angle_inds,1] + phi_sq[small_angle_inds]*phi[small_angle_inds]*rho[small_angle_inds,0]))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 0] = (24*phi[small_angle_inds]*(phi_sq[small_angle_inds] - 12))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 1] = -(96*(phi_sq[small_angle_inds] - 6))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + jac[small_angle_inds, 1, 2] = (4*(72*rho[small_angle_inds,0] - 12*phi_sq[small_angle_inds]*rho[small_angle_inds,0] + 12*phi[small_angle_inds]*rho[small_angle_inds,1] + 12*phi_sq[small_angle_inds]*rho[small_angle_inds,0] - phi_sq[small_angle_inds]*phi_sq[small_angle_inds]*rho[small_angle_inds,0] + phi_sq[small_angle_inds]*phi[small_angle_inds]*rho[small_angle_inds,1]))/(phi_sq[small_angle_inds]**2*phi_sq[small_angle_inds] + 16*phi_sq[small_angle_inds]**2 - 24*phi_sq[small_angle_inds]*phi_sq[small_angle_inds] - 192*phi_sq[small_angle_inds] + 144*phi_sq[small_angle_inds] + 576) + + jac[large_angle_inds, 0, 0] = (sin_phi[large_angle_inds]*phi[large_angle_inds])/(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1) + jac[large_angle_inds, 0, 1] = -(phi[large_angle_inds]*(cos_phi[large_angle_inds] - 1))/(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1) + jac[large_angle_inds, 0, 2] = (phi[large_angle_inds]*(rho[large_angle_inds,0] - 2*cos_phi[large_angle_inds]*rho[large_angle_inds,0] - phi[large_angle_inds]*rho[large_angle_inds,1] + cos_phi[large_angle_inds]**2*rho[large_angle_inds,0] + sin_phi[large_angle_inds]**2*rho[large_angle_inds,0] + cos_phi[large_angle_inds]*phi[large_angle_inds]*rho[large_angle_inds,1] - sin_phi[large_angle_inds]*phi[large_angle_inds]*rho[large_angle_inds,0]))/(phi_sq[large_angle_inds]*(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1)) + jac[large_angle_inds, 1, 0] = (phi[large_angle_inds]*(cos_phi[large_angle_inds] - 1))/(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1) + jac[large_angle_inds, 1, 1] = (sin_phi[large_angle_inds]*phi[large_angle_inds])/(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1) + jac[large_angle_inds, 1, 2] = (phi[large_angle_inds]*(rho[large_angle_inds,1] - 2*cos_phi[large_angle_inds]*rho[large_angle_inds,1] + phi[large_angle_inds]*rho[large_angle_inds,0] + cos_phi[large_angle_inds]**2*rho[large_angle_inds,1] + sin_phi[large_angle_inds]**2*rho[large_angle_inds,1] - cos_phi[large_angle_inds]*phi[large_angle_inds]*rho[large_angle_inds,0] - sin_phi[large_angle_inds]*phi[large_angle_inds]*rho[large_angle_inds,1]))/(phi_sq[large_angle_inds]*(cos_phi[large_angle_inds]**2 - 2*cos_phi[large_angle_inds] + sin_phi[large_angle_inds]**2 + 1)) jac[:, 2, 0] = 0 jac[:, 2, 1] = 0 @@ -114,15 +113,14 @@ def left_jacobian(cls, xi): raise ValueError( "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof)) - x = xi[:, 0] # translation part - y = xi[:, 1] # translation part - theta = xi[:, 2] # rotation part + rho = xi[:, 0:2] # translation part + phi = xi[:, 2] # rotation part - cos_theta = torch.cos(theta) - sin_theta = torch.sin(theta) - theta_sq = theta * theta + cos_phi = torch.cos(phi) + sin_phi = torch.sin(phi) + phi_sq = phi * phi - small_angle_mask = utils.isclose(theta_sq, 0.) + small_angle_mask = utils.isclose(phi_sq, 0.) small_angle_inds = small_angle_mask.nonzero().squeeze_(dim=1) large_angle_mask = small_angle_mask.logical_not() @@ -130,19 +128,19 @@ def left_jacobian(cls, xi): jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) - jac[small_angle_inds, 0, 0] = 1 - 1./6. * theta_sq[small_angle_inds] - jac[small_angle_inds, 0, 1] = -(0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds]) - jac[small_angle_inds, 0, 2] = y[small_angle_inds] / 2. + theta[small_angle_inds] * x[small_angle_inds] / 6. - jac[small_angle_inds, 1, 0] = 0.5 * theta[small_angle_inds] - 1./24. * theta[small_angle_inds] * theta_sq[small_angle_inds] - jac[small_angle_inds, 1, 1] = 1 - 1./6. * theta_sq[small_angle_inds] - jac[small_angle_inds, 1, 2] = -x[small_angle_inds] / 2. + theta[small_angle_inds] * y[small_angle_inds] / 6. - - jac[large_angle_inds, 0, 0] = sin_theta[large_angle_inds] / theta[large_angle_inds] - jac[large_angle_inds, 0, 1] = -(1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] - jac[large_angle_inds, 0, 2] = ( y[large_angle_inds] + theta[large_angle_inds]*x[large_angle_inds] - y[large_angle_inds]*cos_theta[large_angle_inds] - x[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] - jac[large_angle_inds, 1, 0] = (1 - cos_theta[large_angle_inds]) / theta[large_angle_inds] - jac[large_angle_inds, 1, 1] = sin_theta[large_angle_inds] / theta[large_angle_inds] - jac[large_angle_inds, 1, 2] = (-x[large_angle_inds] + theta[large_angle_inds]*y[large_angle_inds] + x[large_angle_inds]*cos_theta[large_angle_inds] - y[large_angle_inds]*sin_theta[large_angle_inds])/theta_sq[large_angle_inds] + jac[small_angle_inds, 0, 0] = 1 - 1./6. * phi_sq[small_angle_inds] + jac[small_angle_inds, 0, 1] = -(0.5 * phi[small_angle_inds] - 1./24. * phi[small_angle_inds] * phi_sq[small_angle_inds]) + jac[small_angle_inds, 0, 2] = rho[small_angle_inds,1] / 2. + phi[small_angle_inds] * rho[small_angle_inds,0] / 6. + jac[small_angle_inds, 1, 0] = 0.5 * phi[small_angle_inds] - 1./24. * phi[small_angle_inds] * phi_sq[small_angle_inds] + jac[small_angle_inds, 1, 1] = 1 - 1./6. * phi_sq[small_angle_inds] + jac[small_angle_inds, 1, 2] = -rho[small_angle_inds,0] / 2. + phi[small_angle_inds] * rho[small_angle_inds,1] / 6. + + jac[large_angle_inds, 0, 0] = sin_phi[large_angle_inds] / phi[large_angle_inds] + jac[large_angle_inds, 0, 1] = -(1 - cos_phi[large_angle_inds]) / phi[large_angle_inds] + jac[large_angle_inds, 0, 2] = ( rho[large_angle_inds,1] + phi[large_angle_inds]*rho[large_angle_inds,0] - rho[large_angle_inds,1]*cos_phi[large_angle_inds] - rho[large_angle_inds,0]*sin_phi[large_angle_inds])/phi_sq[large_angle_inds] + jac[large_angle_inds, 1, 0] = (1 - cos_phi[large_angle_inds]) / phi[large_angle_inds] + jac[large_angle_inds, 1, 1] = sin_phi[large_angle_inds] / phi[large_angle_inds] + jac[large_angle_inds, 1, 2] = (-rho[large_angle_inds,0] + phi[large_angle_inds]*rho[large_angle_inds,1] + rho[large_angle_inds,0]*cos_phi[large_angle_inds] - rho[large_angle_inds,1]*sin_phi[large_angle_inds])/phi_sq[large_angle_inds] jac[:, 2, 0] = 0 jac[:, 2, 1] = 0