SE3 from Transformation Matrix

[VitorGuizilini-TRI]

Hi, I'm wondering if it's possible to initialize a SE3 instance from a 4x4 transformation matrix, instead of quaternion + translation. Thank you!

[nischal-sanil]

where you able to figure this out?

[JonathonLuiten]

I am using the following:

def matrix_to_lie(matrix):
    quat = pytorch3d.transforms.matrix_to_quaternion(matrix[:3, :3])
    quat = torch.cat((quat[1:], quat[0][None]), 0)  # swap real first to real last
    trans = matrix[:3, 3]
    vec = torch.cat((trans, quat), 0)
    Ps = SE3.InitFromVec(vec)
    return Ps

I don't know if it's differentiable (I don't need this atm so haven't checked), but it should be as long as the pytorch3d function used is (which I can't see any reason why it shouldn't be).

[zhigangjiang]

from lietorch import SE3
from scipy.spatial.transform import Rotation

pose_mat = data['poses']  # [n, 4, 4]
quat = Rotation.from_matrix(pose_mat[:, :3, :3]).as_quat()
trans = pose_mat[:, :3, 3]
pose_data = np.concatenate((trans, quat), axis=-1)
T = SE3.InitFromVec(torch.tensor(pose_data))
error = (T.matrix() - torch.tensor(pose_mat)).sum()
print(error)

tensor(-4.0434e-07, dtype=torch.float64)

[Willyzw]

from lietorch import SE3
from scipy.spatial.transform import Rotation

pose_mat = data['poses']  # [n, 4, 4]
quat = Rotation.from_matrix(pose_mat[:, :3, :3]).as_quat()
trans = pose_mat[:, :3, 3]
pose_data = np.concatenate((trans, quat), axis=-1)
T = SE3.InitFromVec(torch.tensor(pose_data))
error = (T.matrix() - torch.tensor(pose_mat)).sum()
print(error)

tensor(-4.0434e-07, dtype=torch.float64)

Hi, isn't the error about 4e-7 too large? If you also wondering like I did, here is what I found out, it turned out the SciPy Rotation class do orthogonalize the input rotation matrix when is not strictly orthogonal. And this is often the case as pose matrces has precision loss when saving and loading.
Reference: https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.from_matrix.html