Add quaternion convert

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 实例.