Transforms in 3D

Functions for converting SE(3) poses (x, y, z, qw, qx, qy, qz) and 3D points (x, y, z) between coordinate frames. Rotations are represented as unit quaternions. For the 2D counterpart see Transforms in 2D.

Convert SE3 poses between frames

Convert between absolute and relative coordinates:

• abs_to_rel_se3_array() / abs_to_rel_se3() – absolute → relative: \(T_\text{rel} = T_\text{origin}^{-1} \cdot T_\text{abs}\)

• rel_to_abs_se3_array() / rel_to_abs_se3() – relative → absolute: \(T_\text{abs} = T_\text{origin} \cdot T_\text{rel}\)

• reframe_se3_array() / reframe_se3() – re-express poses from one reference frame to another.

py123d.geometry.transform.abs_to_rel_se3_array(origin, pose_se3_array)

Convert an SE3 array from absolute to relative coordinates.

Computes \(T_\text{rel} = T_\text{origin}^{-1} \cdot T_\text{abs}\) for each pose in the array. Positions are transformed via the rotation matrix and orientations via quaternion multiplication.

Example:

>>> origin = PoseSE3.from_R_t(EulerAngles(0.0, 0.0, np.pi / 2), np.array([1.0, 0.0, 0.0]))
>>> poses = np.zeros((1, len(PoseSE3Index)), dtype=np.float64)
>>> poses[0, PoseSE3Index.QW] = 1.0  # identity rotation
>>> rel = abs_to_rel_se3_array(origin, poses)

Parameters:

• origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – Origin pose of the relative coordinate system.

• pose_se3_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of SE3 poses with shape (..., 7), indexed by PoseSE3Index in the last dimension.

Raises:

TypeError – If origin is not a PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

SE3 array in relative coordinates with the same shape as pose_se3_array.

py123d.geometry.transform.abs_to_rel_se3(origin, pose_se3)

Convert a single SE3 pose from absolute to relative coordinates.

Typed wrapper around abs_to_rel_se3_array().

Computes \(T_\text{rel} = T_\text{origin}^{-1} \cdot T_\text{abs}\).

Parameters:

• origin (PoseSE3) – The origin pose of the relative coordinate system.

• pose_se3 (PoseSE3) – The absolute SE3 pose to convert.

Return type:

PoseSE3

Returns:

The SE3 pose in relative coordinates.

py123d.geometry.transform.rel_to_abs_se3_array(origin, pose_se3_array)

Convert an SE3 array from relative to absolute coordinates.

Computes \(T_\text{abs} = T_\text{origin} \cdot T_\text{rel}\) for each pose in the array.

Parameters:

• origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – Origin pose of the relative coordinate system.

• pose_se3_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of SE3 poses with shape (..., 7), indexed by PoseSE3Index in the last dimension.

Raises:

TypeError – If origin is not a PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

SE3 array in absolute coordinates with the same shape as pose_se3_array.

py123d.geometry.transform.rel_to_abs_se3(origin, pose_se3)

Convert a single SE3 pose from relative to absolute coordinates.

Typed wrapper around rel_to_abs_se3_array().

Computes \(T_\text{abs} = T_\text{origin} \cdot T_\text{rel}\).

Parameters:

• origin (PoseSE3) – The origin pose of the relative coordinate system.

• pose_se3 (PoseSE3) – The relative SE3 pose to convert.

Return type:

PoseSE3

Returns:

The SE3 pose in absolute coordinates.

py123d.geometry.transform.reframe_se3_array(from_origin, to_origin, pose_se3_array)

Convert an SE3 array from one reference frame to another.

Equivalent to converting from the source frame to absolute coordinates, then from absolute coordinates to the target frame: abs_to_rel(to_origin, rel_to_abs(from_origin, poses)), but computed more efficiently as a single relative transformation.

Example:

>>> frame_a = PoseSE3.from_R_t(EulerAngles(0.0, 0.0, 0.0), np.array([1.0, 0.0, 0.0]))
>>> frame_b = PoseSE3.from_R_t(EulerAngles(0.0, 0.0, np.pi / 2), np.array([0.0, 1.0, 0.0]))
>>> poses_in_a = np.zeros((1, len(PoseSE3Index)), dtype=np.float64)
>>> poses_in_a[0, PoseSE3Index.QW] = 1.0
>>> poses_in_b = reframe_se3_array(frame_a, frame_b, poses_in_a)

Parameters:

• from_origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – The source origin state in the absolute frame.

• to_origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – The target origin state in the absolute frame.

• pose_se3_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of SE3 poses in the source frame with shape (..., 7).

Raises:

TypeError – If the origins are not PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

The SE3 array in the target frame, indexed by PoseSE3Index.

py123d.geometry.transform.reframe_se3(from_origin, to_origin, pose_se3)

Convert a single SE3 pose from one reference frame to another.

Typed wrapper around reframe_se3_array().

Parameters:

• from_origin (PoseSE3) – The source origin state in the absolute frame.

• to_origin (PoseSE3) – The target origin state in the absolute frame.

• pose_se3 (PoseSE3) – The SE3 pose in the source frame.

Return type:

PoseSE3

Returns:

The SE3 pose in the target frame.

Convert 3D points between frames

The same absolute/relative/reframe operations, applied to 3D points instead of SE3 poses:

• abs_to_rel_points_3d_array() / abs_to_rel_point_3d()

• rel_to_abs_points_3d_array() / rel_to_abs_point_3d()

• reframe_points_3d_array() / reframe_point_3d()

py123d.geometry.transform.abs_to_rel_points_3d_array(origin, points_3d_array)

Convert 3D points from absolute to relative coordinates.

Computes \(p_\text{rel} = R_\text{origin}^T \cdot (p_\text{abs} - t_\text{origin})\) for each point in the array.

Example:

>>> origin = PoseSE3.from_R_t(EulerAngles(0.0, 0.0, 0.0), np.array([1.0, 2.0, 3.0]))
>>> points = np.array([[2.0, 3.0, 4.0]], dtype=np.float64)
>>> rel_points = abs_to_rel_points_3d_array(origin, points)  # [[1.0, 1.0, 1.0]]

Parameters:

• origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – Origin pose of the relative coordinate system.

• points_3d_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of 3D points with shape (..., 3), indexed by Point3DIndex in the last dimension.

Raises:

TypeError – If origin is not a PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

3D points in relative coordinates with the same shape as points_3d_array.

py123d.geometry.transform.abs_to_rel_point_3d(origin, point_3d)

Convert a single 3D point from absolute to relative coordinates.

Typed wrapper around abs_to_rel_points_3d_array().

Computes \(p_\text{rel} = R_\text{origin}^T \cdot (p_\text{abs} - t_\text{origin})\).

Parameters:

• origin (PoseSE3) – The origin pose of the relative coordinate system.

• point_3d (Point3D) – The absolute 3D point to convert.

Return type:

Point3D

Returns:

The 3D point in relative coordinates.

py123d.geometry.transform.rel_to_abs_points_3d_array(origin, points_3d_array)

Convert 3D points from relative to absolute coordinates.

Computes \(p_\text{abs} = R_\text{origin} \cdot p_\text{rel} + t_\text{origin}\) for each point in the array.

Parameters:

• origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – Origin pose of the relative coordinate system.

• points_3d_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of 3D points with shape (..., 3), indexed by Point3DIndex in the last dimension.

Raises:

TypeError – If origin is not a PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

3D points in absolute coordinates with the same shape as points_3d_array.

py123d.geometry.transform.rel_to_abs_point_3d(origin, point_3d)

Convert a single 3D point from relative to absolute coordinates.

Typed wrapper around rel_to_abs_points_3d_array().

Computes \(p_\text{abs} = R_\text{origin} \cdot p_\text{rel} + t_\text{origin}\).

Parameters:

• origin (PoseSE3) – The origin pose of the relative coordinate system.

• point_3d (Point3D) – The relative 3D point to convert.

Return type:

Point3D

Returns:

The 3D point in absolute coordinates.

py123d.geometry.transform.reframe_points_3d_array(from_origin, to_origin, points_3d_array)

Convert 3D points from one reference frame to another.

Equivalent to converting from the source frame to absolute coordinates, then from absolute coordinates to the target frame, but computed more efficiently as a single relative transformation.

Parameters:

• from_origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – The source origin state in the absolute frame.

• to_origin (Union[PoseSE3, ndarray[tuple[Any, ...], dtype[float64]]]) – The target origin state in the absolute frame.

• points_3d_array (ndarray[tuple[Any, ...], dtype[float64]]) – Array of 3D points in the source frame with shape (..., 3).

Raises:

TypeError – If the origins are not PoseSE3 or np.ndarray.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

The 3D points in the target frame, indexed by Point3DIndex.

py123d.geometry.transform.reframe_point_3d(from_origin, to_origin, point_3d)

Convert a single 3D point from one reference frame to another.

Typed wrapper around reframe_points_3d_array().

Parameters:

• from_origin (PoseSE3) – The source origin state in the absolute frame.

• to_origin (PoseSE3) – The target origin state in the absolute frame.

• point_3d (Point3D) – The 3D point in the source frame.

Return type:

Point3D

Returns:

The 3D point in the target frame.

Translation along body-frame axes

Translate SE3 poses or 3D points along their local (body) coordinate axes. The orientation is preserved.

py123d.geometry.transform.translate_se3_along_body_frame(pose_se3, translation)

Translate an SE3 state along a vector in its body frame.

The translation vector is rotated from the body frame into the world frame and added to the position. The orientation is unchanged.

Parameters:

• pose_se3 (PoseSE3) – The SE3 state to translate.

• translation (Vector3D) – The 3D translation vector in the body frame.

Return type:

PoseSE3

Returns:

The translated SE3 state.

py123d.geometry.transform.translate_se3_along_x(pose_se3, distance)

Translate an SE3 state along its local X-axis (forward direction).

Shorthand for translate_se3_along_body_frame(pose_se3, Vector3D(distance, 0.0, 0.0)).

Parameters:

• pose_se3 (PoseSE3) – The SE3 state to translate.

• distance (float) – The distance to translate along the local X-axis.

Return type:

PoseSE3

Returns:

The translated SE3 state.

py123d.geometry.transform.translate_se3_along_y(pose_se3, distance)

Translate an SE3 state along its local Y-axis (left direction).

Shorthand for translate_se3_along_body_frame(pose_se3, Vector3D(0.0, distance, 0.0)).

Parameters:

• pose_se3 (PoseSE3) – The SE3 state to translate.

• distance (float) – The distance to translate along the local Y-axis.

Return type:

PoseSE3

Returns:

The translated SE3 state.

py123d.geometry.transform.translate_se3_along_z(pose_se3, distance)

Translate an SE3 state along its local Z-axis (up direction).

Shorthand for translate_se3_along_body_frame(pose_se3, Vector3D(0.0, 0.0, distance)).

Parameters:

• pose_se3 (PoseSE3) – The SE3 state to translate.

• distance (float) – The distance to translate along the local Z-axis.

Return type:

PoseSE3

Returns:

The translated SE3 state.

py123d.geometry.transform.translate_3d_along_body_frame(points_3d, quaternions, translation)

Translate 3D points along their respective body frames defined by quaternions.

Unlike translate_se3_along_body_frame(), this function operates on raw arrays and supports per-point translations via broadcasting.

Parameters:

• points_3d (ndarray[tuple[Any, ...], dtype[float64]]) – Array of 3D points, indexed by Point3DIndex.

• quaternions (ndarray[tuple[Any, ...], dtype[float64]]) – Array of quaternions, indexed by QuaternionIndex.

• translation (ndarray[tuple[Any, ...], dtype[float64]]) – Array of translation vectors, indexed by Vector3DIndex.

Return type:

ndarray[tuple[Any, ...], dtype[float64]]

Returns:

Translated 3D points in the world frame, indexed by Point3DIndex.