schema: add reference coordinate system to poses (#147)

feat: add reference coordinate system to poses

Co-authored-by: David Torres Ocana (TME) <[email protected]>
Co-authored-by: yuta-tsuzuki-woven <[email protected]>

dgp/proto/geometry.proto

@@ -32,6 +32,11 @@ message Quaternion {
 message Pose {
   Vector3 translation = 1;
   Quaternion rotation = 2;
+  // Indicate the name of the coordinate system the above transform is from:
+  //   i.e. let x_ref be a vector in reference_coordinate_system and x_pose a vector in this Pose's coordinate system (e.g. sensor's),
+  //   then x_ref = rotation . x_pose + translation
+  //   TODO: Coordinate systems Naming Convention
+  string reference_coordinate_system = 3;
 }
 
 message CameraIntrinsics {

dgp/utils/pose.py

@@ -11,7 +11,9 @@ class Pose:
     """SE(3) rigid transform class that allows compounding of 6-DOF poses
     and provides common transformations that are commonly seen in geometric problems.
     """
-    def __init__(self, wxyz=np.float32([1., 0., 0., 0.]), tvec=np.float32([0., 0., 0.])):
+    def __init__(
+        self, wxyz=np.float32([1., 0., 0., 0.]), tvec=np.float32([0., 0., 0.]), reference_coordinate_system=""
+    ):
         """Initialize a Pose with Quaternion and 3D Position
 
         Parameters
@@ -21,30 +23,33 @@ def __init__(self, wxyz=np.float32([1., 0., 0., 0.]), tvec=np.float32([0., 0., 0
 
         tvec: np.float32 (default: np.float32([0,0,0]))
             Translation (xyz)
+        reference_coordinate_system: str
+            Reference coordinate system this Pose (Transform) is expressed with respect to
         """
         assert isinstance(wxyz, (np.ndarray, Quaternion))
         assert isinstance(tvec, np.ndarray)
 
         if isinstance(wxyz, np.ndarray):
             assert np.abs(1.0 - np.linalg.norm(wxyz)) < 1.0e-3
 
+        self.reference_coordinate_system = reference_coordinate_system
         self.quat = Quaternion(wxyz)
         self.tvec = tvec
 
     def __repr__(self):
         formatter = {'float_kind': lambda x: '%.2f' % x}
         tvec_str = np.array2string(self.tvec, formatter=formatter)
-        return 'wxyz: {}, tvec: ({})'.format(self.quat, tvec_str)
+        return 'wxyz: {}, tvec: ({}) wrt. `{}`'.format(self.quat, tvec_str, self.reference_coordinate_system)
 
     def copy(self):
         """Return a copy of this pose object.
 
         Returns
-        ----------
-        result: Pose
+        -------
+        Pose
             Copied pose object.
         """
-        return self.__class__(Quaternion(self.quat), self.tvec.copy())
+        return self.__class__(Quaternion(self.quat), self.tvec.copy(), self.reference_coordinate_system)
 
     def __mul__(self, other):
         """Left-multiply Pose with another Pose or 3D-Points.
@@ -58,8 +63,8 @@ def __mul__(self, other):
                (i.e. X' = self_pose * X)
 
         Returns
-        ----------
-        result: Pose or np.ndarray
+        -------
+        Pose or np.ndarray
             Transformed pose or point cloud
         """
         if isinstance(other, Pose):
@@ -77,24 +82,31 @@ def __mul__(self, other):
     def __rmul__(self, other):
         raise NotImplementedError('Right multiply not implemented yet!')
 
-    def inverse(self):
+    def inverse(self, new_reference_coordinate_system=""):
         """Returns a new Pose that corresponds to the
         inverse of this one.
 
-        Returns
+        Parameters
         ----------
-        result: Pose
-            Inverted pose
+        new_reference_coordinate_system: str
+            The reference coordinate system the inverse Pose (Transform) is expressed with respect to. I.e. the name of the current Pose
+
+        Returns
+        -------
+        Pose
+            new_reference_coordinate_system pose
         """
         qinv = self.quat.inverse
-        return self.__class__(qinv, qinv.rotate(-self.tvec))
+        return self.__class__(
+            qinv, qinv.rotate(-self.tvec), reference_coordinate_system=new_reference_coordinate_system
+        )
 
     @property
     def matrix(self):
         """Returns a 4x4 homogeneous matrix of the form [R t; 0 1]
 
         Returns
-        ----------
+        -------
         result: np.ndarray
             4x4 homogeneous matrix
         """
@@ -107,8 +119,8 @@ def rotation_matrix(self):
         """Returns the 3x3 rotation matrix (R)
 
         Returns
-        ----------
-        result: np.ndarray
+        -------
+        np.ndarray
             3x3 rotation matrix
         """
         result = self.quat.transformation_matrix
@@ -119,8 +131,8 @@ def rotation(self):
         """Return the rotation component of the pose as a Quaternion object.
 
         Returns
-        ----------
-        self.quat: Quaternion
+        -------
+        Quaternion
             Rotation component of the Pose object.
         """
         return self.quat
@@ -130,29 +142,35 @@ def translation(self):
         """Return the translation component of the pose as a np.ndarray.
 
         Returns
-        ----------
-        self.tvec: np.ndarray
+        -------
+        np.ndarray
             Translation component of the Pose object.
         """
         return self.tvec
 
     @classmethod
-    def from_matrix(cls, transformation_matrix):
+    def from_matrix(cls, transformation_matrix, reference_coordinate_system=""):
         """Initialize pose from 4x4 transformation matrix
 
         Parameters
         ----------
         transformation_matrix: np.ndarray
             4x4 containing rotation/translation
+        reference_coordinate_system: str
+            Reference coordinate system this Pose (Transform) is expressed with respect to
 
         Returns
         -------
         Pose
         """
-        return cls(wxyz=Quaternion(matrix=transformation_matrix[:3, :3]), tvec=np.float32(transformation_matrix[:3, 3]))
+        return cls(
+            wxyz=Quaternion(matrix=transformation_matrix[:3, :3]),
+            tvec=np.float32(transformation_matrix[:3, 3]),
+            reference_coordinate_system=reference_coordinate_system
+        )
 
     @classmethod
-    def from_rotation_translation(cls, rotation_matrix, tvec):
+    def from_rotation_translation(cls, rotation_matrix, tvec, reference_coordinate_system=""):
         """Initialize pose from rotation matrix and translation vector.
 
         Parameters
@@ -161,8 +179,14 @@ def from_rotation_translation(cls, rotation_matrix, tvec):
             3x3 rotation matrix
         tvec : np.ndarray
             length-3 translation vector
+        reference_coordinate_system: str
+            Reference coordinate system this Pose (Transform) is expressed with respect to
         """
-        return cls(wxyz=Quaternion(matrix=rotation_matrix), tvec=np.float64(tvec))
+        return cls(
+            wxyz=Quaternion(matrix=rotation_matrix),
+            tvec=np.float64(tvec),
+            reference_coordinate_system=reference_coordinate_system
+        )
 
     @classmethod
     def load(cls, pose_proto):
@@ -190,7 +214,9 @@ def load(cls, pose_proto):
             pose_proto.translation.y,
             pose_proto.translation.z,
         ])
-        return cls(wxyz=rotation, tvec=translation)
+        reference_coordinate_system = pose_proto.reference_coordinate_system
+
+        return cls(wxyz=rotation, tvec=translation, reference_coordinate_system=reference_coordinate_system)
 
     def to_proto(self):
         """Convert Pose into pb object.
@@ -209,7 +235,11 @@ def to_proto(self):
         pose_0S.translation.x = self.tvec[0]
         pose_0S.translation.y = self.tvec[1]
         pose_0S.translation.z = self.tvec[2]
+        pose_0S.reference_coordinate_system = self.reference_coordinate_system
         return pose_0S
 
     def __eq__(self, other):
-        return self.quat == other.quat and (self.tvec == other.tvec).all()
+        return (
+            self.quat == other.quat and (self.tvec == other.tvec).all()
+            and self.reference_coordinate_system == other.reference_coordinate_system
+        )