Open VitorGuizilini-TRI opened 2 years ago
where you able to figure this out?
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).
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)
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
Hi, I'm wondering if it's possible to initialize a SE3 instance from a 4x4 transformation matrix, instead of quaternion + translation. Thank you!