Added motion estimation with outputs

Added Recorder pose outputs to compare calculations
Added dataset parsing library

util.py

@@ -0,0 +1,25 @@
+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)