diff --git a/dgp/proto/geometry.proto b/dgp/proto/geometry.proto index ded4131..cd30c81 100644 --- a/dgp/proto/geometry.proto +++ b/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 { diff --git a/dgp/proto/geometry_pb2.py b/dgp/proto/geometry_pb2.py index 89b9f81..71872ca 100644 --- a/dgp/proto/geometry_pb2.py +++ b/dgp/proto/geometry_pb2.py @@ -19,7 +19,7 @@ syntax='proto3', serialized_options=None, create_key=_descriptor._internal_create_key, - serialized_pb=b'\n\x18\x64gp/proto/geometry.proto\x12\tdgp.proto\"*\n\x07Point3D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"*\n\x07Vector3\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"<\n\nQuaternion\x12\n\n\x02qx\x18\x01 \x01(\x01\x12\n\n\x02qy\x18\x02 \x01(\x01\x12\n\n\x02qz\x18\x03 \x01(\x01\x12\n\n\x02qw\x18\x04 \x01(\x01\"X\n\x04Pose\x12\'\n\x0btranslation\x18\x01 \x01(\x0b\x32\x12.dgp.proto.Vector3\x12\'\n\x08rotation\x18\x02 \x01(\x0b\x32\x15.dgp.proto.Quaternion\"\xce\x02\n\x10\x43\x61meraIntrinsics\x12\n\n\x02\x66x\x18\x01 \x01(\x01\x12\n\n\x02\x66y\x18\x02 \x01(\x01\x12\n\n\x02\x63x\x18\x03 \x01(\x01\x12\n\n\x02\x63y\x18\x04 \x01(\x01\x12\x0c\n\x04skew\x18\x05 \x01(\x01\x12\x0b\n\x03\x66ov\x18\x06 \x01(\x01\x12\x0f\n\x07\x66isheye\x18\x07 \x01(\x05\x12\n\n\x02k1\x18\x08 \x01(\x01\x12\n\n\x02k2\x18\t \x01(\x01\x12\n\n\x02k3\x18\n \x01(\x01\x12\n\n\x02k4\x18\x0b \x01(\x01\x12\n\n\x02k5\x18\x0c \x01(\x01\x12\n\n\x02k6\x18\r \x01(\x01\x12\n\n\x02p1\x18\x0e \x01(\x01\x12\n\n\x02p2\x18\x0f \x01(\x01\x12\n\n\x02s1\x18\x10 \x01(\x01\x12\n\n\x02s2\x18\x11 \x01(\x01\x12\n\n\x02s3\x18\x12 \x01(\x01\x12\n\n\x02s4\x18\x13 \x01(\x01\x12\x0c\n\x04taux\x18\x14 \x01(\x01\x12\x0c\n\x04tauy\x18\x15 \x01(\x01\x12\r\n\x05\x61lpha\x18\x16 \x01(\x01\x12\x0c\n\x04\x62\x65ta\x18\x17 \x01(\x01\x12\t\n\x01w\x18\x18 \x01(\x01\x12\n\n\x02xi\x18\x19 \x01(\x01\x62\x06proto3' + serialized_pb=b'\n\x18\x64gp/proto/geometry.proto\x12\tdgp.proto\"*\n\x07Point3D\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"*\n\x07Vector3\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"<\n\nQuaternion\x12\n\n\x02qx\x18\x01 \x01(\x01\x12\n\n\x02qy\x18\x02 \x01(\x01\x12\n\n\x02qz\x18\x03 \x01(\x01\x12\n\n\x02qw\x18\x04 \x01(\x01\"}\n\x04Pose\x12\'\n\x0btranslation\x18\x01 \x01(\x0b\x32\x12.dgp.proto.Vector3\x12\'\n\x08rotation\x18\x02 \x01(\x0b\x32\x15.dgp.proto.Quaternion\x12#\n\x1breference_coordinate_system\x18\x03 \x01(\t\"\xce\x02\n\x10\x43\x61meraIntrinsics\x12\n\n\x02\x66x\x18\x01 \x01(\x01\x12\n\n\x02\x66y\x18\x02 \x01(\x01\x12\n\n\x02\x63x\x18\x03 \x01(\x01\x12\n\n\x02\x63y\x18\x04 \x01(\x01\x12\x0c\n\x04skew\x18\x05 \x01(\x01\x12\x0b\n\x03\x66ov\x18\x06 \x01(\x01\x12\x0f\n\x07\x66isheye\x18\x07 \x01(\x05\x12\n\n\x02k1\x18\x08 \x01(\x01\x12\n\n\x02k2\x18\t \x01(\x01\x12\n\n\x02k3\x18\n \x01(\x01\x12\n\n\x02k4\x18\x0b \x01(\x01\x12\n\n\x02k5\x18\x0c \x01(\x01\x12\n\n\x02k6\x18\r \x01(\x01\x12\n\n\x02p1\x18\x0e \x01(\x01\x12\n\n\x02p2\x18\x0f \x01(\x01\x12\n\n\x02s1\x18\x10 \x01(\x01\x12\n\n\x02s2\x18\x11 \x01(\x01\x12\n\n\x02s3\x18\x12 \x01(\x01\x12\n\n\x02s4\x18\x13 \x01(\x01\x12\x0c\n\x04taux\x18\x14 \x01(\x01\x12\x0c\n\x04tauy\x18\x15 \x01(\x01\x12\r\n\x05\x61lpha\x18\x16 \x01(\x01\x12\x0c\n\x04\x62\x65ta\x18\x17 \x01(\x01\x12\t\n\x01w\x18\x18 \x01(\x01\x12\n\n\x02xi\x18\x19 \x01(\x01\x62\x06proto3' ) @@ -192,6 +192,13 @@ message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='reference_coordinate_system', full_name='dgp.proto.Pose.reference_coordinate_system', index=2, + number=3, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -205,7 +212,7 @@ oneofs=[ ], serialized_start=189, - serialized_end=277, + serialized_end=314, ) @@ -404,8 +411,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=280, - serialized_end=614, + serialized_start=317, + serialized_end=651, ) _POSE.fields_by_name['translation'].message_type = _VECTOR3 diff --git a/dgp/utils/pose.py b/dgp/utils/pose.py index bacf4d9..41086fc 100644 --- a/dgp/utils/pose.py +++ b/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,6 +23,8 @@ 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) @@ -28,23 +32,24 @@ def __init__(self, wxyz=np.float32([1., 0., 0., 0.]), tvec=np.float32([0., 0., 0 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 + )