103 lines
3.3 KiB
Python
103 lines
3.3 KiB
Python
from __future__ import annotations
|
|
|
|
from typing import Optional, Tuple
|
|
|
|
import numpy as np
|
|
|
|
|
|
def to_world_points(points_sensor: np.ndarray, pose: Optional[np.ndarray]) -> np.ndarray:
|
|
"""
|
|
Convert sensor-frame points to world frame using pose (world_T_sensor).
|
|
Falls back to input points when pose is unavailable/invalid.
|
|
"""
|
|
if pose is None:
|
|
return np.asarray(points_sensor, dtype=np.float32)
|
|
try:
|
|
pts = np.asarray(points_sensor, dtype=np.float32)
|
|
pose_np = np.asarray(pose, dtype=np.float32)
|
|
if pts.ndim != 2 or pts.shape[1] != 3 or pose_np.shape != (4, 4):
|
|
return pts
|
|
rot = pose_np[:3, :3]
|
|
trans = pose_np[:3, 3]
|
|
return (pts @ rot.T) + trans
|
|
except Exception:
|
|
return np.asarray(points_sensor, dtype=np.float32)
|
|
|
|
|
|
def apply_transform_points(points: Optional[np.ndarray], transform: np.ndarray) -> Optional[np.ndarray]:
|
|
if points is None:
|
|
return None
|
|
pts = np.asarray(points, dtype=np.float32)
|
|
if len(pts) == 0:
|
|
return pts
|
|
try:
|
|
tf = np.asarray(transform, dtype=np.float32)
|
|
if tf.shape != (4, 4):
|
|
return pts
|
|
rot = tf[:3, :3]
|
|
trans = tf[:3, 3]
|
|
return (pts @ rot.T) + trans
|
|
except Exception:
|
|
return pts
|
|
|
|
|
|
def tf_delta(prev_tf: np.ndarray, new_tf: np.ndarray) -> Tuple[float, float]:
|
|
try:
|
|
a = np.asarray(prev_tf, dtype=np.float64)
|
|
b = np.asarray(new_tf, dtype=np.float64)
|
|
if a.shape != (4, 4) or b.shape != (4, 4):
|
|
return 0.0, 0.0
|
|
dp = b[:3, 3] - a[:3, 3]
|
|
trans = float(np.linalg.norm(dp))
|
|
r = b[:3, :3] @ a[:3, :3].T
|
|
ang = float(np.degrees(np.arccos(np.clip((np.trace(r) - 1.0) * 0.5, -1.0, 1.0))))
|
|
return trans, ang
|
|
except Exception:
|
|
return 0.0, 0.0
|
|
|
|
|
|
def blend_rigid_tf(prev_tf: np.ndarray, new_tf: np.ndarray, alpha_t: float, alpha_r: float) -> np.ndarray:
|
|
try:
|
|
a = np.asarray(prev_tf, dtype=np.float64)
|
|
b = np.asarray(new_tf, dtype=np.float64)
|
|
if a.shape != (4, 4) or b.shape != (4, 4):
|
|
return np.asarray(new_tf, dtype=np.float64)
|
|
at = float(np.clip(alpha_t, 0.0, 1.0))
|
|
ar = float(np.clip(alpha_r, 0.0, 1.0))
|
|
out = np.eye(4, dtype=np.float64)
|
|
out[:3, 3] = ((1.0 - at) * a[:3, 3]) + (at * b[:3, 3])
|
|
rm = ((1.0 - ar) * a[:3, :3]) + (ar * b[:3, :3])
|
|
u, _, vt = np.linalg.svd(rm, full_matrices=False)
|
|
r = u @ vt
|
|
if np.linalg.det(r) < 0:
|
|
u[:, -1] *= -1.0
|
|
r = u @ vt
|
|
out[:3, :3] = r
|
|
return out
|
|
except Exception:
|
|
return np.asarray(new_tf, dtype=np.float64)
|
|
|
|
|
|
def yaw_deg_from_tf(tf: np.ndarray) -> float:
|
|
try:
|
|
m = np.asarray(tf, dtype=np.float64)
|
|
if m.shape != (4, 4):
|
|
return 0.0
|
|
yaw = np.degrees(np.arctan2(float(m[1, 0]), float(m[0, 0])))
|
|
return float(yaw)
|
|
except Exception:
|
|
return 0.0
|
|
|
|
|
|
def tf_from_xyzyaw(x: float, y: float, z: float, yaw_deg: float) -> np.ndarray:
|
|
yaw = np.deg2rad(float(yaw_deg))
|
|
cy = float(np.cos(yaw))
|
|
sy = float(np.sin(yaw))
|
|
tf = np.eye(4, dtype=np.float64)
|
|
tf[:3, :3] = np.array(
|
|
[[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]],
|
|
dtype=np.float64,
|
|
)
|
|
tf[:3, 3] = np.array([float(x), float(y), float(z)], dtype=np.float64)
|
|
return tf
|