diff --git a/tf/transform.py b/tf/transform.py index 539c9f7..33d71bc 100644 --- a/tf/transform.py +++ b/tf/transform.py @@ -105,6 +105,39 @@ def roll(self) -> float: """物体绕 X 轴旋转的欧拉角, 单位度, 由变换矩阵计算得到""" return np.rad2deg(np.arctan2(self.matrix[2, 1], self.matrix[2, 2])).item() + @property + def quaternion(self) -> np.ndarray: + """物体的四元数表示, 4x1 的矩阵""" + m = self.matrix[:3, :3] + trace = np.trace(m) + if trace > 0: + s = 0.5 / np.sqrt(trace + 1.0) + qw = 0.25 / s + qx = (m[2, 1] - m[1, 2]) * s + qy = (m[0, 2] - m[2, 0]) * s + qz = (m[1, 0] - m[0, 1]) * s + else: + if m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = 2.0 * np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) + qw = (m[2, 1] - m[1, 2]) / s + qx = 0.25 * s + qy = (m[0, 1] + m[1, 0]) / s + qz = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = 2.0 * np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) + qw = (m[0, 2] - m[2, 0]) / s + qx = (m[0, 1] + m[1, 0]) / s + qy = 0.25 * s + qz = (m[1, 2] + m[2, 1]) / s + else: + s = 2.0 * np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) + qw = (m[1, 0] - m[0, 1]) / s + qx = (m[0, 2] + m[2, 0]) / s + qy = (m[1, 2] + m[2, 1]) / s + qz = 0.25 * s + + return np.array([qw, qx, qy, qz]) + @classmethod def from_carla_transform_obj(cls, transform: carla.Transform) -> 'Transform': """从 CARLA 的变换对象创建 Transform 实例.