diff --git a/liegroups/numpy/se2.py b/liegroups/numpy/se2.py index 0baa085..8e8cc24 100644 --- a/liegroups/numpy/se2.py +++ b/liegroups/numpy/se2.py @@ -76,7 +76,35 @@ def inv_left_jacobian(cls, xi): .. math:: \\mathcal{J}^{-1}(\\boldsymbol{\\xi}) """ - raise NotImplementedError + rho = xi[0:2] # translation part + phi = xi[2] # rotation part + + cos_phi = np.cos(phi) + sin_phi = np.sin(phi) + phi_sq = phi * phi + + jac = np.zeros((cls.dof, cls.dof)) + + 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*(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 + jac[2][2] = 1 + + return jac @classmethod def left_jacobian(cls, xi): @@ -85,7 +113,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 + + rho = xi[0:2] # translation part + phi = xi[2] # rotation part + + cos_phi = np.cos(phi) + sin_phi = np.sin(phi) + phi_sq = phi * phi + + if phi_sq < 1e-15: + A = 1 - 1./6. * phi_sq + B = 0.5 * phi - 1./24. * phi * phi_sq + else: + A = sin_phi / phi + B = (1 - cos_phi) / phi + + jac = np.zeros((cls.dof, cls.dof)) + jac[0][0] = A + jac[0][1] = -B + jac[1][0] = B + jac[1][1] = A + + 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] = ( 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 + + return jac def log(self): """Logarithmic map for :math:`SE(2)`, which computes a tangent vector from a transformation: 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/se2.py b/liegroups/torch/se2.py index dbf5169..f7130df 100644 --- a/liegroups/torch/se2.py +++ b/liegroups/torch/se2.py @@ -60,11 +60,93 @@ 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)) + + rho = xi[:, 0:2] # translation part + phi = xi[:, 2] # rotation part + + cos_phi = torch.cos(phi) + sin_phi = torch.sin(phi) + phi_sq = phi * phi + + 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() + large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) + + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) + + 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 + jac[:, 2, 2] = 1 + + return jac.squeeze_() @classmethod def 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)) + + rho = xi[:, 0:2] # translation part + phi = xi[:, 2] # rotation part + + cos_phi = torch.cos(phi) + sin_phi = torch.sin(phi) + phi_sq = phi * phi + + 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() + large_angle_inds = large_angle_mask.nonzero().squeeze_(dim=1) + + jac = torch.zeros((xi.shape[0], cls.dof, cls.dof)).to(xi.device) + + 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 + jac[:, 2, 2] = 1 + + return jac.squeeze_() def log(self): phi = self.rot.log() 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 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 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