"""Spatial pose and point utilities.
This module defines spatial utility models and transformation helpers for points and poses.
It provides typed pose representations, conversion between homogeneous matrices and structured pose models,
and point-transformation utilities that work directly with NumPy arrays. The module streamlines common spatial
operations used by camera and calibration workflows while preserving a user-friendly API.
Key functionalities include:
- Structured 3D pose models for position and Euler-angle orientation.
- Conversion between homogeneous transforms and typed pose objects.
- Homogeneous-coordinate helpers for batched point transformations.
- Utilities for transforming single points and point sets with 4x4 matrices.
- Input-shape normalization for robust NumPy-based spatial computations.
- Lightweight abstractions optimized for internal camera workflow usage.
Copyright (c) 2026 Jozef Stefan Institute
Authors: Leon Zlajpah.
"""
from __future__ import annotations
import numpy as np
from numpy.typing import ArrayLike
from pydantic import BaseModel
from robotblockset.rbs_typing import HomogeneousMatrixType, Vector3DArrayType, Vectors3DType
from robotblockset.transformations import map_pose
[docs]
class Position(BaseModel):
"""Position in 3D space, all units are in meters."""
x: float
y: float
z: float
[docs]
class EulerAngles(BaseModel):
"""Extrinsic XYZ Euler angles in radians."""
roll: float
pitch: float
yaw: float
[docs]
class Pose(BaseModel):
"""Position and extrinsic XYZ orientation of an object in 3D space.
Position values are expressed in meters and Euler angles in radians. The
rotations are about the fixed X, Y, and Z axes of the reference frame.
"""
position_in_meters: Position
rotation_euler_xyz_in_radians: EulerAngles
[docs]
@classmethod
def from_homogeneous_matrix(cls, matrix: HomogeneousMatrixType) -> Pose:
"""Construct a pose from a homogeneous transformation matrix.
Parameters
----------
matrix : HomogeneousMatrixType
Homogeneous transformation matrix with shape ``(4, 4)``.
Returns
-------
Pose
Structured position and Euler-angle representation.
"""
se3_pose = map_pose(T=matrix, out="pRPY")
position = se3_pose[:3]
euler_angles = se3_pose[-1:-4:-1] # yaw, pitch, roll
position_model = Position(x=position[0], y=position[1], z=position[2])
euler_angles_model = EulerAngles(roll=euler_angles[0], pitch=euler_angles[1], yaw=euler_angles[2])
pose = cls(position_in_meters=position_model, rotation_euler_xyz_in_radians=euler_angles_model)
return pose
[docs]
def as_homogeneous_matrix(self) -> HomogeneousMatrixType:
"""Return the pose as a homogeneous transformation matrix.
Returns
-------
HomogeneousMatrixType
Homogeneous transformation matrix with shape ``(4, 4)``.
"""
position = self.position_in_meters
euler_angles = self.rotation_euler_xyz_in_radians
position_array = np.array([position.x, position.y, position.z])
RPY_array = np.array([euler_angles.yaw, euler_angles.pitch, euler_angles.roll])
pose_matrix = map_pose(RPY=RPY_array, p=position_array, out="T")
return pose_matrix
class _HomogeneousPoints:
"""Represent one or more 3D points in homogeneous coordinates.
The helper accepts a single point with shape ``(3,)`` or a point batch with
shape ``(N, 3)`` and preserves that distinction when returning Cartesian
points.
"""
# TODO: extend to generic dimensions (1D,2D,3D).
def __init__(self, points: ArrayLike) -> None:
"""Create homogeneous points from real, finite array-like values.
Parameters
----------
points : numpy.typing.ArrayLike
Single 3D point with shape ``(3,)`` or point batch with shape
``(N, 3)``.
Raises
------
TypeError
If ``points`` is not array-like or does not contain real numeric
values.
ValueError
If ``points`` has an unsupported shape or contains non-finite
values.
"""
try:
points_array = np.asarray(points)
except (TypeError, ValueError) as error:
raise TypeError("points must be a numeric array-like object") from error
if not self.is_valid_points_type(points_array):
raise ValueError(f"points must have shape (3,) or (N, 3), received {points_array.shape}")
if not np.issubdtype(points_array.dtype, np.number) or np.issubdtype(points_array.dtype, np.complexfloating):
raise TypeError("points must contain real numeric values")
if not np.all(np.isfinite(points_array)):
raise ValueError("points must contain only finite values")
self._single_point = points_array.ndim == 1
points_array = self.ensure_array_2d(points_array)
self._homogeneous_points = np.concatenate(
[points_array, np.ones((points_array.shape[0], 1), dtype=np.float32)], axis=1
)
@staticmethod
def is_valid_points_type(points: ArrayLike) -> bool:
"""Return whether ``points`` has shape ``(3,)`` or ``(N, 3)``."""
try:
points_array = np.asarray(points)
except (TypeError, ValueError):
return False
return points_array.shape == (3,) or (points_array.ndim == 2 and points_array.shape[1] == 3)
@staticmethod
def ensure_array_2d(points: Vectors3DType) -> Vector3DArrayType:
"""Return points as a two-dimensional array.
A single point with shape ``(3,)`` is reshaped to ``(1, 3)``. An
existing point batch is returned unchanged.
Parameters
----------
points : Vectors3DType
Single point or point batch represented by a NumPy array.
Returns
-------
Vector3DArrayType
Point array with shape ``(N, 3)``.
Raises
------
ValueError
If a one-dimensional input does not contain exactly three values.
"""
if len(points.shape) == 1:
if len(points) != 3:
raise ValueError("points has only one dimension, but it's length is not 3")
points = points.reshape((1, 3))
return points
@property
def homogeneous_points(self) -> np.ndarray:
"""Return the internal homogeneous point array with shape ``(N, 4)``."""
return self._homogeneous_points
@property
def points(self) -> Vectors3DType:
"""Return normalized Cartesian points while preserving input rank.
Returns
-------
Vectors3DType
A vector with shape ``(3,)`` for a single-point input, or an array
with shape ``(N, 3)`` for a batched input.
Raises
------
ValueError
If a homogeneous scale is zero or near zero.
"""
# normalize points (for safety, should never be necessary with affine transforms)
# but we've had bugs of this type with projection operations, so better safe than sorry?
scalars = self._homogeneous_points[:, 3][:, np.newaxis]
if np.any(np.isclose(scalars, 0.0)):
raise ValueError("Cannot normalize homogeneous points with zero or near-zero scale values")
points = self.homogeneous_points[:, :3] / scalars
if self._single_point:
return points[0]
return points
def apply_transform(self, homogeneous_transform_matrix: HomogeneousMatrixType) -> None:
"""Apply a homogeneous transformation matrix in place.
Parameters
----------
homogeneous_transform_matrix : HomogeneousMatrixType
Transformation matrix with shape ``(4, 4)``.
"""
self._homogeneous_points = (homogeneous_transform_matrix @ self.homogeneous_points.transpose()).transpose()