| |
| """C10 geometry (sweep and ego-motion parts): rigid transforms in float64, Autoware's float32 sweep transform, and |
| the yaw / quaternion conventions of the LiDAR detectors. |
| |
| Naming: ``T_a_from_b`` maps points of frame ``b`` into frame ``a`` (4x4, metres, row-major, translation in the last |
| column), so ``p_a = T_a_from_b @ p_b`` and ``T_a_from_c = T_a_from_b @ T_b_from_c``. |
| |
| Two precisions on purpose: |
| |
| - **float64** for composing poses (``compose``, ``invert_rigid``): what tf2 does, and what every port uses unless its |
| Autoware node does otherwise (S:streampetr:386; S:pointpainting:212-215; S:bevformer:437). |
| - **float32 as Autoware's lidar_centerpoint does it**: the node casts each TF to ``Eigen::Affine3f``, inverts and |
| multiplies in float32 (``pointcloud_densification.cpp:47-51,82-86``; ``voxel_generator.cpp:63``) and the CUDA |
| kernel ``generateSweepPoints_kernel`` applies the float32 affine as ``m0*x + m4*y + m8*z + m12`` (column-major, |
| ``preprocess_kernel.cu:58-90``). :func:`invert_affine_f32` and :func:`transform_points_f32` reproduce that |
| arithmetic in numpy float32 without fused multiply-adds, so they agree with the GPU to the last bit except where |
| nvcc contracts a multiply-add (an FMA rounds once): a sub-micrometre difference, irrelevant to 0.32 m pillars. |
| |
| Yaw conventions (S:centerpoint:305-307): the deployed CenterPoint ONNX regresses the "mmdet3d" yaw; Autoware |
| publishes ``yaw_ros = -yaw_net - pi/2`` counter-clockwise from +x of ``base_link`` (``ros_utils.cpp:55-60``). |
| |
| numpy only; no side effects on import. |
| """ |
| from __future__ import annotations |
|
|
| import math |
| from typing import Any, Sequence |
|
|
| import numpy as np |
|
|
| from .io import parse_transform |
|
|
| __all__ = [ |
| "as_transform", |
| "compose", |
| "invert_rigid", |
| "invert_affine_f32", |
| "compose_f32", |
| "transform_points", |
| "transform_points_f32", |
| "rotation_from_rpy", |
| "transform_from_xyz_rpy", |
| "quaternion_wxyz_from_yaw", |
| "yaw_from_quaternion_wxyz", |
| "yaw_from_rotation", |
| "wrap_angle", |
| "mmdet_yaw_to_ros", |
| "ros_yaw_to_mmdet", |
| ] |
|
|
|
|
| |
|
|
| def as_transform(obj: Any, *, field: str = "transform") -> np.ndarray: |
| """Any spelling :func:`ttaw.io.parse_transform` accepts (4x4 / 3x4 array, ``{matrix}``, |
| ``{translation, rotation_wxyz | rotation_xyzw | rotation}``, ``{x, y, z, roll, pitch, yaw}``) -> a validated |
| (4, 4) float64 rigid transform. A malformed value raises :class:`ttaw.io.InputError`.""" |
| return parse_transform(obj, field=field) |
|
|
|
|
| def compose(*transforms: np.ndarray) -> np.ndarray: |
| """``T_0 @ T_1 @ ... @ T_n`` in float64 (``compose(T_a_from_b, T_b_from_c) == T_a_from_c``).""" |
| if not transforms: |
| return np.eye(4) |
| out = np.asarray(transforms[0], dtype=np.float64) |
| for t in transforms[1:]: |
| out = out @ np.asarray(t, dtype=np.float64) |
| return out |
|
|
|
|
| def invert_rigid(T: np.ndarray) -> np.ndarray: |
| """Exact inverse of a rigid transform in float64: ``[R^T, -R^T t]``.""" |
| T = np.asarray(T, dtype=np.float64) |
| out = np.eye(4) |
| r = T[:3, :3] |
| out[:3, :3] = r.T |
| out[:3, 3] = -(r.T @ T[:3, 3]) |
| return out |
|
|
|
|
| def invert_affine_f32(T: np.ndarray) -> np.ndarray: |
| """``Eigen::Affine3f::inverse()`` (TransformTraits ``Affine``) in float32: the 3x3 linear part inverted through |
| its cofactors and the column-0 determinant (Eigen's ``compute_inverse<..., 3>``), translation ``-L^-1 t``, |
| every operation rounded to float32. Used by the Autoware densification emulation |
| (``pointcloud_densification.cpp:84``).""" |
| m = np.asarray(T, dtype=np.float32) |
| a = m[:3, :3] |
| f = np.float32 |
|
|
| def cofactor(i: int, j: int) -> np.float32: |
| i1, i2, j1, j2 = (i + 1) % 3, (i + 2) % 3, (j + 1) % 3, (j + 2) % 3 |
| return f(f(a[i1, j1] * a[i2, j2]) - f(a[i1, j2] * a[i2, j1])) |
|
|
| det = f(f(cofactor(0, 0) * a[0, 0]) + f(cofactor(1, 0) * a[1, 0])) + f(cofactor(2, 0) * a[2, 0]) |
| if det == 0 or not np.isfinite(det): |
| raise ValueError("singular affine transform") |
| inv_det = f(f(1.0) / det) |
| inv = np.empty((3, 3), np.float32) |
| for i in range(3): |
| for j in range(3): |
| inv[i, j] = f(cofactor(j, i) * inv_det) |
| out = np.eye(4, dtype=np.float32) |
| out[:3, :3] = inv |
| t = m[:3, 3] |
| for i in range(3): |
| out[i, 3] = -(f(f(inv[i, 0] * t[0]) + f(inv[i, 1] * t[1])) + f(inv[i, 2] * t[2])) |
| return out |
|
|
|
|
| def compose_f32(A: np.ndarray, B: np.ndarray) -> np.ndarray: |
| """``A @ B`` of two float32 affine matrices with float32 rounding after every product and sum, summed in index |
| order (Eigen's lazy product of two ``Affine3f``).""" |
| a = np.asarray(A, dtype=np.float32) |
| b = np.asarray(B, dtype=np.float32) |
| out = np.zeros((4, 4), np.float32) |
| for k in range(4): |
| out = (out + a[:, k:k + 1] * b[k:k + 1, :]).astype(np.float32) |
| return out |
|
|
|
|
| def transform_points(points: np.ndarray, T: np.ndarray) -> np.ndarray: |
| """``T @ [x, y, z, 1]`` for (N, >=3) points in float64 (extra columns are passed through unchanged).""" |
| p = np.asarray(points, dtype=np.float64) |
| T = np.asarray(T, dtype=np.float64) |
| out = p.copy() |
| out[:, :3] = p[:, :3] @ T[:3, :3].T + T[:3, 3] |
| return out |
|
|
|
|
| def transform_points_f32(xyz: np.ndarray, T: np.ndarray) -> np.ndarray: |
| """Autoware ``generateSweepPoints_kernel`` (``preprocess_kernel.cu:58-90``): the affine is cast to float32 and |
| each output coordinate is ``((m[r,0]*x + m[r,1]*y) + m[r,2]*z) + m[r,3]`` in float32, left to right (no FMA). |
| ``xyz``: (N, >=3); returns (N, 3) float32.""" |
| p = np.asarray(xyz, dtype=np.float32) |
| m = np.asarray(T, dtype=np.float32) |
| x, y, z = p[:, 0], p[:, 1], p[:, 2] |
| out = np.empty((len(p), 3), np.float32) |
| for r in range(3): |
| out[:, r] = ((m[r, 0] * x + m[r, 1] * y) + m[r, 2] * z) + m[r, 3] |
| return out |
|
|
|
|
| |
|
|
| def rotation_from_rpy(roll: float, pitch: float, yaw: float) -> np.ndarray: |
| """tf2 ``setRPY``: ``R = Rz(yaw) @ Ry(pitch) @ Rx(roll)`` (float64).""" |
| cr, sr, cp, sp, cy, sy = (math.cos(roll), math.sin(roll), math.cos(pitch), math.sin(pitch), math.cos(yaw), |
| math.sin(yaw)) |
| return np.array([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], |
| [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], |
| [-sp, cp * sr, cp * cr]], dtype=np.float64) |
|
|
|
|
| def transform_from_xyz_rpy(x: float, y: float, z: float, roll: float = 0.0, pitch: float = 0.0, |
| yaw: float = 0.0) -> np.ndarray: |
| """A (4, 4) float64 transform from a translation and tf2 RPY angles (Autoware's calibration yaml spelling).""" |
| out = np.eye(4) |
| out[:3, :3] = rotation_from_rpy(roll, pitch, yaw) |
| out[:3, 3] = (x, y, z) |
| return out |
|
|
|
|
| def quaternion_wxyz_from_yaw(yaw: float) -> np.ndarray: |
| """``autoware_utils::create_quaternion_from_yaw``: rotation about +z, as (w, x, y, z) float64.""" |
| return np.array([math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)], dtype=np.float64) |
|
|
|
|
| def yaw_from_quaternion_wxyz(q: Sequence[float]) -> float: |
| """``tf2::getYaw`` of a (w, x, y, z) quaternion.""" |
| w, x, y, z = (float(v) for v in q) |
| return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) |
|
|
|
|
| def yaw_from_rotation(r: np.ndarray) -> float: |
| """Heading of a rotation matrix (rotation of +x about +z, ``atan2(R[1,0], R[0,0])``).""" |
| r = np.asarray(r, dtype=np.float64) |
| return math.atan2(r[1, 0], r[0, 0]) |
|
|
|
|
| def wrap_angle(a: Any) -> Any: |
| """Wrap angles to [-pi, pi).""" |
| return (np.asarray(a, dtype=np.float64) + math.pi) % (2.0 * math.pi) - math.pi |
|
|
|
|
| def mmdet_yaw_to_ros(yaw_net: Any) -> np.ndarray: |
| """``ros_utils.cpp:55``: ``const float yaw = -box3d.yaw - pi / 2`` (float input, double arithmetic, float |
| result). Returns float32 radians, not wrapped (as Autoware).""" |
| y = np.asarray(yaw_net, dtype=np.float32).astype(np.float64) |
| return (-y - math.pi / 2.0).astype(np.float32) |
|
|
|
|
| def ros_yaw_to_mmdet(yaw_ros: Any) -> np.ndarray: |
| """Inverse of :func:`mmdet_yaw_to_ros` (float64): ``yaw_net = -yaw_ros - pi/2``.""" |
| return -np.asarray(yaw_ros, dtype=np.float64) - math.pi / 2.0 |
|
|
