Error with exponential map and logarithm map

[marcoleg]

Hi,
first of all thanks for your work.
My problem is linked to transformation of SE3 matrix to xi vector and vice versa. This code section below is a test that shows that the original SE3 matrix has different values compare to output matrix, but this happens with just values that have small module.

import torch
from liegroups.torch import SE3

print("Test 1:")
test_SE3_matrix = torch.tensor([[0.9999957085, -0.002008878859, -0.002134637441, 1.187580466], [0.002002746798, 0.9999938607, -0.002870822791, 0.06579889357], [0.002140391385, 0.002866535215, 0.9999936223, -0.05322432518], [0., 0., 0., 1.]])
print("Testing SE3 matrix:\n", test_SE3_matrix)
test_xi = SE3.from_matrix(test_SE3_matrix).log()
print("Output xi value:", test_xi)
test_original_value_SE3_matrix = SE3.exp(test_xi).as_matrix()
print("It should be the same as the start test matrix:\n", test_original_value_SE3_matrix)

print("\nTest 2:")
test_SE3_matrix_2 = torch.tensor([[1.000000e-00, 9.043681e-12, 2.326810e-11, -5.551115e-17], [9.043682e-12, 1.000000e+00, 2.392370e-10, 0.000000e+00], [2.326809e-11, 2.392370e-10, 9.999999e-01, -2.220446e-16], [0., 0., 0., 1.]])
print("Testing SE3 matrix:\n", test_SE3_matrix_2)
test_xi_2 = SE3.from_matrix(test_SE3_matrix_2).log()
print("Output xi value:", test_xi_2)
test_original_value_SE3_matrix_2 = SE3.exp(test_xi_2).as_matrix()
print("It should be the same as the start test matrix:\n", test_original_value_SE3_matrix_2)

And this is the output:

Test 1:
Testing SE3 matrix:
tensor([[ 1.0000, -0.0020, -0.0021, 1.1876], [ 0.0020, 1.0000, -0.0029, 0.0658], [ 0.0021, 0.0029, 1.0000, -0.0532], [ 0.0000, 0.0000, 0.0000, 1.0000]])
Output xi value: tensor([ 1.1876, 0.0645, -0.0546, 0.0029, -0.0021, 0.0020])
It should be the same as the start test matrix:
tensor([[ 1.0000, -0.0020, -0.0021, 1.1876], [ 0.0020, 1.0000, -0.0029, 0.0658], [ 0.0021, 0.0029, 1.0000, -0.0532], [ 0.0000, 0.0000, 0.0000, 1.0000]])

Test 2:
Testing SE3 matrix:
tensor([[ 1.0000e+00, 9.0437e-12, 2.3268e-11, -5.5511e-17], [ 9.0437e-12, 1.0000e+00, 2.3924e-10, 0.0000e+00], [ 2.3268e-11, 2.3924e-10, 1.0000e+00, -2.2204e-16], [ 0.0000e+00, 0.0000e+00, 0.0000e+00, 1.0000e+00]])
Output xi value: tensor([-5.5511e-17, -2.6310e-26, -2.2204e-16, 2.3924e-10, 2.3268e-11, 9.0437e-12])
It should be the same as the start test matrix:
tensor([[ 1.0000e+00, -9.0437e-12, 2.3268e-11, -5.5511e-17], [ 9.0437e-12, 1.0000e+00, -2.3924e-10, 0.0000e+00], [-2.3268e-11, 2.3924e-10, 1.0000e+00, -2.2204e-16], [ 0.0000e+00, 0.0000e+00, 0.0000e+00, 1.0000e+00]])

As you can see, in Test 2, the element (3,1), (1,2) and (2,3) have wrong sign.