Ryan Fitz-Gerald
51a8c0e923
Added Recorder pose outputs to compare calculations Added dataset parsing library
25 lines
801 B
Python
25 lines
801 B
Python
import json
|
|
from os import PathLike
|
|
from pathlib import Path
|
|
|
|
from scipy.spatial.transform import RigidTransform, Rotation
|
|
import numpy as np
|
|
|
|
|
|
def relative_transform(world_to_a: RigidTransform, world_to_b: RigidTransform) -> RigidTransform:
|
|
""" Computes the relative transform between two poses represented by a transform from the same
|
|
world coordinates
|
|
|
|
Args:
|
|
world_to_a (RigidTransform): transform from world origin to the first pose
|
|
world_to_b (RigidTransform): transform from world origin to the second pose
|
|
|
|
Returns:
|
|
RigidTransform: transform to second pose from first pose b_to_a
|
|
"""
|
|
|
|
Twa = world_to_a.as_matrix()
|
|
Tbw = np.linalg.inv(world_to_b.as_matrix())
|
|
Tab = np.linalg.matmul(Tbw,Twa)
|
|
|
|
return RigidTransform.from_matrix(Tab) |