145 lines
4.7 KiB
Python
145 lines
4.7 KiB
Python
"""SE(3) helpers: compose, invert, conversions rvec/quat/euler/matrix."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
|
|
def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
|
|
import cv2
|
|
R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=np.float64).reshape(3, 1))
|
|
return R
|
|
|
|
|
|
def R_to_rvec(R: np.ndarray) -> np.ndarray:
|
|
import cv2
|
|
rvec, _ = cv2.Rodrigues(np.asarray(R, dtype=np.float64))
|
|
return rvec.flatten()
|
|
|
|
|
|
def quat_xyzw_to_R(q: np.ndarray) -> np.ndarray:
|
|
x, y, z, w = [float(v) for v in q]
|
|
n = x * x + y * y + z * z + w * w
|
|
if n == 0.0:
|
|
return np.eye(3)
|
|
s = 2.0 / n
|
|
return np.array([
|
|
[1 - s * (y * y + z * z), s * (x * y - z * w), s * (x * z + y * w)],
|
|
[s * (x * y + z * w), 1 - s * (x * x + z * z), s * (y * z - x * w)],
|
|
[s * (x * z - y * w), s * (y * z + x * w), 1 - s * (x * x + y * y)],
|
|
])
|
|
|
|
|
|
def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
|
|
m = np.asarray(R, dtype=np.float64)
|
|
t = m[0, 0] + m[1, 1] + m[2, 2]
|
|
if t > 0:
|
|
s = np.sqrt(t + 1.0) * 2
|
|
w = 0.25 * s
|
|
x = (m[2, 1] - m[1, 2]) / s
|
|
y = (m[0, 2] - m[2, 0]) / s
|
|
z = (m[1, 0] - m[0, 1]) / s
|
|
elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
|
|
s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2
|
|
w = (m[2, 1] - m[1, 2]) / s
|
|
x = 0.25 * s
|
|
y = (m[0, 1] + m[1, 0]) / s
|
|
z = (m[0, 2] + m[2, 0]) / s
|
|
elif m[1, 1] > m[2, 2]:
|
|
s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2
|
|
w = (m[0, 2] - m[2, 0]) / s
|
|
x = (m[0, 1] + m[1, 0]) / s
|
|
y = 0.25 * s
|
|
z = (m[1, 2] + m[2, 1]) / s
|
|
else:
|
|
s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2
|
|
w = (m[1, 0] - m[0, 1]) / s
|
|
x = (m[0, 2] + m[2, 0]) / s
|
|
y = (m[1, 2] + m[2, 1]) / s
|
|
z = 0.25 * s
|
|
return np.array([x, y, z, w])
|
|
|
|
|
|
def normalize_quaternion(q: list[float]) -> list[float]:
|
|
"""Normalizza un quaternion a lunghezza unitaria. Solleva ValueError se
|
|
la norma e' troppo piccola per essere normalizzata in modo sensato
|
|
(es. quaternion zero, sotto 1e-6)."""
|
|
n = math.sqrt(sum(float(x)*float(x) for x in q))
|
|
if n < 1e-6:
|
|
raise ValueError(f"quaternion is degenerate (norm={n:.3e})")
|
|
return [float(x) / n for x in q]
|
|
|
|
|
|
def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
|
|
T = np.eye(4)
|
|
T[:3, :3] = R
|
|
T[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
|
|
return T
|
|
|
|
|
|
def invert_T(T: np.ndarray) -> np.ndarray:
|
|
R = T[:3, :3]
|
|
t = T[:3, 3]
|
|
Ti = np.eye(4)
|
|
Ti[:3, :3] = R.T
|
|
Ti[:3, 3] = -R.T @ t
|
|
return Ti
|
|
|
|
|
|
def T_from_pos_quat(pos: np.ndarray, quat_xyzw: np.ndarray) -> np.ndarray:
|
|
return make_T(quat_xyzw_to_R(np.asarray(quat_xyzw)), np.asarray(pos))
|
|
|
|
|
|
def pos_quat_from_T(T: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
return T[:3, 3].copy(), R_to_quat_xyzw(T[:3, :3])
|
|
|
|
|
|
def T_from_rvec_tvec(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
|
|
return make_T(rvec_to_R(rvec), np.asarray(tvec).flatten())
|
|
|
|
|
|
def R_to_euler_xyz(R: np.ndarray) -> np.ndarray:
|
|
"""Extrinsic XYZ euler (roll, pitch, yaw) in radians."""
|
|
sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
|
|
singular = sy < 1e-6
|
|
if not singular:
|
|
x = np.arctan2(R[2, 1], R[2, 2])
|
|
y = np.arctan2(-R[2, 0], sy)
|
|
z = np.arctan2(R[1, 0], R[0, 0])
|
|
else:
|
|
x = np.arctan2(-R[1, 2], R[1, 1])
|
|
y = np.arctan2(-R[2, 0], sy)
|
|
z = 0.0
|
|
return np.array([x, y, z])
|
|
|
|
|
|
def transform_points(T: np.ndarray, pts: np.ndarray) -> np.ndarray:
|
|
"""Apply T (4x4) to Nx3 points."""
|
|
pts = np.asarray(pts, dtype=np.float64)
|
|
R = T[:3, :3]
|
|
t = T[:3, 3]
|
|
return pts @ R.T + t
|
|
|
|
|
|
def rpy_deg_to_quaternion_xyzw(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
|
|
"""Extrinsic XYZ Euler (deg) -> quaternion xyzw. Inverse of `quaternion_xyzw_to_rpy_deg`.
|
|
Convenzione coerente con `R_to_euler_xyz` (XYZ extrinsic, in radianti)."""
|
|
cr, sr = np.cos(np.radians(roll_deg) * 0.5), np.sin(np.radians(roll_deg) * 0.5)
|
|
cp, sp = np.cos(np.radians(pitch_deg) * 0.5), np.sin(np.radians(pitch_deg) * 0.5)
|
|
cy, sy = np.cos(np.radians(yaw_deg) * 0.5), np.sin(np.radians(yaw_deg) * 0.5)
|
|
qw = cr * cp * cy + sr * sp * sy
|
|
qx = sr * cp * cy - cr * sp * sy
|
|
qy = cr * sp * cy + sr * cp * sy
|
|
qz = cr * cp * sy - sr * sp * cy
|
|
return np.array([qx, qy, qz, qw], dtype=np.float64)
|
|
|
|
|
|
def quaternion_xyzw_to_rpy_deg(quat_xyzw) -> tuple[float, float, float]:
|
|
"""Quaternion xyzw -> extrinsic XYZ Euler (deg). Convention matches `R_to_euler_xyz`."""
|
|
R = quat_xyzw_to_R(np.asarray(quat_xyzw, dtype=np.float64))
|
|
rpy_rad = R_to_euler_xyz(R)
|
|
rpy_deg = np.degrees(rpy_rad)
|
|
return float(rpy_deg[0]), float(rpy_deg[1]), float(rpy_deg[2])
|