def test_compare_with_precomputed(self):
"""
Compare the outputs against precomputed results.
"""
self.assertClose(
se3_log_map(self.precomputed_transform),
self.precomputed_log_transform,
atol=1e-4,
)
self.assertClose(
self.precomputed_transform,
se3_exp_map(self.precomputed_log_transform),
atol=1e-4,
)
def test_se3_exp_to_log_to_exp(self, batch_size: int = 10000):
"""
Check that `se3_exp_map(se3_log_map(A))==A` for
a batch of randomly generated SE(3) matrices `A`.
"""
transform = TestSE3.init_transform(batch_size=batch_size)
# Limit test transforms to those not around the singularity where
# the rotation angle~=pi.
nonsingular = so3_rotation_angle(transform[:, :3, :3]) < 3.134
transform = transform[nonsingular]
transform_ = se3_exp_map(se3_log_map(transform,
eps=1e-8,
cos_bound=0.0),
eps=1e-8)
self.assertClose(transform, transform_, atol=0.02)
def test_se3_exp_zero_translation(self, batch_size: int = 100):
"""
Check that `se3_exp_map` with zero translation gives
the same result as corresponding `so3_exp_map`.
"""
log_transform = TestSE3.init_log_transform(batch_size=batch_size)
log_transform[:, :3] *= 0.0
transform = se3_exp_map(log_transform, eps=1e-8)
transform_so3 = so3_exp_map(log_transform[:, 3:], eps=1e-8)
self.assertClose(transform[:, :3, :3],
transform_so3.permute(0, 2, 1),
atol=1e-4)
self.assertClose(transform[:, 3, :3],
torch.zeros_like(transform[:, :3, 3]),
atol=1e-4)
def test_se3_exp_singularity(self, batch_size: int = 100):
"""
Tests whether the `se3_exp_map` is robust to the input vectors
with low L2 norms, where the algorithm is numerically unstable.
"""
# generate random log-rotations with a tiny angle
log_rot = TestSE3.init_log_transform(batch_size=batch_size)
log_rot_small = log_rot * 1e-6
log_rot_small.requires_grad = True
transforms = se3_exp_map(log_rot_small)
# tests whether all outputs are finite
self.assertTrue(torch.isfinite(transforms).all())
# tests whether all gradients are finite and not None
loss = transforms.sum()
loss.backward()
self.assertIsNotNone(log_rot_small.grad)
self.assertTrue(torch.isfinite(log_rot_small.grad).all())
def test_se3_exp_output_format(self, batch_size: int = 100):
"""
Check that the output of `se3_exp_map` is a valid SE3 matrix.
"""
transform = se3_exp_map(
TestSE3.init_log_transform(batch_size=batch_size))
R = transform[:, :3, :3]
T = transform[:, 3, :3]
rest = transform[:, :, 3]
Rdet = R.det()
# check det(R)==1
self.assertClose(Rdet, torch.ones_like(Rdet), atol=1e-4)
# check that the translation is a finite vector
self.assertTrue(torch.isfinite(T).all())
# check last column == [0,0,0,1]
last_col = rest.new_zeros(batch_size, 4)
last_col[:, -1] = 1.0
self.assertClose(rest, last_col)
def compute_transforms():
se3_exp_map(log_transform)
torch.cuda.synchronize()