def test_log_SO3(self):
C = tr.tensor(se3.exp_SO3([-100., 200., -80]), dtype=tr.float32)
log_C_scipy = torch_se3.unskew3(
tr.tensor(np.real(slinalg.logm(C)), dtype=tr.float32))
log_C = torch_se3.log_SO3(C)
self.assertTrue(tr.allclose(log_C, log_C_scipy, atol=1e-6))
C = tr.tensor(se3.exp_SO3([-1., 2., -0.8]), dtype=tr.float32)
log_C_scipy = torch_se3.unskew3(
tr.tensor(np.real(slinalg.logm(C)), dtype=tr.float32))
log_C = torch_se3.log_SO3(C)
self.assertTrue(tr.allclose(log_C, log_C_scipy, atol=1e-8))
C = tr.tensor(se3.exp_SO3([1e-8, -2e-8, 3e-8]), dtype=tr.float32)
log_C = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(log_C, tr.tensor([1e-8, -2e-8, 3e-8]), atol=1e-10))
C = tr.tensor(se3.exp_SO3([1e-9, -2e-9, 3e-9]), dtype=tr.float32)
log_C = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(log_C, tr.tensor([1e-9, -2e-9, 3e-9]), atol=1e-10))
C = tr.tensor(se3.exp_SO3([0., 0., 0.]), dtype=tr.float32)
log_C = torch_se3.log_SO3(C)
self.assertTrue(tr.allclose(log_C, tr.zeros(3, ), atol=1e-10))
def test_cuda_device_and_grad(self):
C = torch_se3.exp_SO3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(C.requires_grad)
self.assertTrue(C.is_cuda)
C = torch_se3.exp_SO3(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(C.requires_grad)
self.assertTrue(C.is_cuda)
phi = torch_se3.log_SO3(C)
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3(tr.eye(3, 3, requires_grad=True).cuda())
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3_eigen(C)
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3_eigen(tr.eye(3, 3, requires_grad=True).cuda())
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
v_skewed = torch_se3.skew3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(v_skewed.requires_grad)
self.assertTrue(v_skewed.is_cuda)
v = torch_se3.unskew3(v_skewed)
self.assertTrue(v.requires_grad)
self.assertTrue(v.is_cuda)
J = torch_se3.J_left_SO3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3_inv(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3_inv(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
def test_log_SO3_eigen(self):
C = tr.tensor(se3.exp_SO3([-100., 200., -80]), dtype=tr.float32)
log_C_scipy = torch_se3.unskew3(
tr.tensor(np.real(slinalg.logm(C)), dtype=tr.float32))
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(tr.allclose(log_C, log_C_scipy, atol=1e-6))
C = tr.tensor(se3.exp_SO3([-1., 2., -0.8]), dtype=tr.float32)
log_C_scipy = torch_se3.unskew3(
tr.tensor(np.real(slinalg.logm(C)), dtype=tr.float32))
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(tr.allclose(log_C, log_C_scipy, atol=1e-8))
C = tr.tensor(se3.exp_SO3([1e-3, -2e-3, 3e-3]), dtype=tr.float32)
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(
tr.allclose(log_C, tr.tensor([1e-3, -2e-3, 3e-3]), atol=1e-5))
C = tr.tensor(se3.exp_SO3([0., 0., 0.]), dtype=tr.float32)
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(tr.allclose(log_C, tr.zeros(3, ), atol=1e-10))
C = tr.tensor(se3.exp_SO3([0.1, -0.2, 3.133624802220163]),
dtype=tr.float32)
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(
tr.allclose(
log_C, tr.tensor([0.1, -0.2, 3.133624802220163]), atol=1e-6)
or tr.allclose(
-log_C, tr.tensor([0.1, -0.2, 3.133624802220163]), atol=1e-6))
C = tr.tensor(se3.exp_SO3([-np.pi, 0, 0]), dtype=tr.float32)
log_C = torch_se3.log_SO3_eigen(C)
self.assertTrue(
tr.allclose(log_C, tr.tensor([-np.pi, 0, 0]), atol=1e-6)
or tr.allclose(-log_C, tr.tensor([-np.pi, 0, 0]), atol=1e-6))
def test_skew_unskew(self):
self.assertTrue(
tr.allclose(torch_se3.unskew3(
torch_se3.skew3(tr.tensor([1., 2., 3.]))),
tr.tensor([1., 2., 3.]),
atol=1e-8))
self.assertTrue(
tr.allclose(torch_se3.skew3(tr.tensor([1., 2.,
3.])).transpose(0, 1),
-torch_se3.skew3(tr.tensor([1., 2., 3.])),
atol=1e-8))
self.assertTrue(
tr.allclose(tr.mm(torch_se3.skew3(tr.tensor([1., 2., 3.])),
tr.tensor([[1.], [2.], [3.]])),
tr.tensor([0., 0., 0.]),
atol=1e-8))
def test_simple_grad(self):
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.0, 0.0, 0.0], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.unskew3(
tr.mm(C - tr.eye(3, 3), torch_se3.J_left_SO3_inv(v)))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.unskew3(
tr.mm(C - tr.eye(3, 3),
torch_se3.J_left_SO3(v).inverse()))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))