pytorchse3

Numerically stable implementations of batched SE(3) exp and log maps

Install

pip install pytorchse3

How to use

import torch

from pytorchse3.se3 import se3_exp_map, se3_log_map

Here are two transformation matrices for which PyTorch3D recovers the wrong log map (see this issue).

T = torch.Tensor(
    [
        [
            [-0.7384057045, 0.3333132863, -0.5862244964, 0.0000000000],
            [0.3520625532, -0.5508944392, -0.7566816807, 0.0000000000],
            [-0.5751599669, -0.7651259303, 0.2894364297, 0.0000000000],
            [-0.1840534210, -0.1836946011, 0.9952554703, 1.0000000000],
        ],
        [
            [-0.7400283217, 0.5210028887, -0.4253400862, 0.0000000000],
            [0.5329059958, 0.0683888718, -0.8434065580, 0.0000000000],
            [-0.4103286564, -0.8508108258, -0.3282552958, 0.0000000000],
            [-0.1197679043, 0.1799146235, 0.5538908839, 1.0000000000],
        ],
    ],
).transpose(-1, -2)

pytorchse3 computes the correct log map.

log_T_vee = se3_log_map(T)
log_T_vee
tensor([[ 1.1319,  1.4831, -2.5131, -0.8503, -0.1170,  0.7346],
        [ 1.1288,  2.2886, -1.8147, -0.8812,  0.0367, -0.1004]])

Exponentiating the log map recovers the original transformation matrix with 1e-4 absolute error.

eq_T = se3_exp_map(log_T_vee)
assert torch.allclose(T, eq_T, atol=1e-4)
T - eq_T
tensor([[[-9.2983e-06, -2.3842e-07,  1.1504e-05,  2.9802e-08],
         [-5.1558e-06,  8.5235e-06, -8.6427e-06, -2.9802e-08],
         [ 8.6427e-06, -6.4373e-06,  4.4703e-07,  0.0000e+00],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00]],

        [[ 8.0466e-06,  1.6212e-05,  6.0201e-06, -3.7253e-08],
         [ 4.5896e-06,  8.6352e-06,  3.3975e-06,  2.9802e-08],
         [-8.5831e-06,  1.0610e-05, -1.6809e-05,  0.0000e+00],
         [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  0.0000e+00]]])

References