Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

SE2 left jacobians #12

Open
wants to merge 14 commits into
base: master
Choose a base branch
from
Open

SE2 left jacobians #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
bec2df8
left jacobian implementation for SE2
Jan 3, 2021
8b93ad0
fix bottom right element of SE2 left jacobian
Jan 3, 2021
329fd18
add unit test in anticipation of inv left jacobian implementation
Jan 3, 2021
aa70a03
using MATLAB symbolic toolbox to implement inverse left jacobian for …
Jan 3, 2021
9480679
MATLAB symbolic toolbox solves small tangent vec case - test passes
Jan 3, 2021
420b904
adds torch unit tests in anticipation of left/inv jacobians for SE2 g…
Jan 3, 2021
d94f410
left jacobian SE2 implemented in torch
Jan 3, 2021
97d76dd
torch implementations of left jacobian (and inverse) implemented and …
Jan 3, 2021
db18c52
ensure se2 jacs are on the correct device
Jan 3, 2021
a391c39
fixing a few device issues
Jan 3, 2021
6c87b28
cleaning up prior PR
Jan 3, 2021
721d7fd
take translation and rotation parts from tangent vector
Jan 8, 2021
242f0e3
numpy implementation matches notation with SE(3) for trans/rot parts …
Jan 8, 2021
ca637bd
torch implementation matches notation with SE(3) for trans/rot parts …
Jan 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 62 additions & 2 deletions liegroups/numpy/se2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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:
Expand Down
4 changes: 2 additions & 2 deletions liegroups/torch/_base.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
86 changes: 84 additions & 2 deletions liegroups/torch/se2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
18 changes: 9 additions & 9 deletions liegroups/torch/so2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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])

Expand All @@ -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

Expand All @@ -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:
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if len(small_angle_inds): is enough

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

Expand All @@ -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

Expand Down
13 changes: 13 additions & 0 deletions tests/numpy/test_se2_numpy.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)
22 changes: 22 additions & 0 deletions tests/torch/test_se2_torch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)