948 add support to create coordinate systems from homogeneous transfo…

…rmation matrices (#949) * initial changes without tests and .get_homogenous(?!) methods * added as_homogeneous_matrix method to LCS * added squeeze to get rid of excess dimension and fixed shape * added dimeonsionality. Maybe a better way * put changes into changelog * better docstring from as_homogeneous_matrix * added first test * works with xarray now * removed xarray support?! * wrong indexing * What is mypys problem? * changed npt.ArrayLike to np.ndarray for homogeneous transformation matrices * changed typing for translation unit * fixed typo * more typo * excluding time series * Update CHANGELOG.md * Update CHANGELOG.md * fixed typo in "homogeneous" ... --------- Co-authored-by: Çağtay Fabry <[email protected]>
BAMWelDX · Nov 6, 2024 · e0e3a65 · e0e3a65
1 parent 8a34f2d
commit e0e3a65
Show file tree

Hide file tree

Showing 5 changed files with 156 additions and 2 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,6 +2,13 @@
 
 ## 0.6.9 (unreleased)
 
+### Changes
+
+- added support for homogeneous transformation matrices \[{pull}`949`\]:
+  - added `create_cs_from_homogeneous_transformation` to `CoordinateSystemManager`
+  - added `from_homogeneous_transformation` to `LocalCoordinateSystem`
+  - added `as_homogeneous_matrix` to `LocalCoordinateSystem`
+
 ### Fixes
 
 - rename (fix typo) argument to `lcs_child_in_parent` in `CoordinateSystemManager.add_cs`  \[{pull}`936`\].

diff --git a/weldx/tests/transformations/test_cs_manager.py b/weldx/tests/transformations/test_cs_manager.py
@@ -2749,6 +2749,10 @@ def test_coordinate_system_manager_create_coordinate_system():
     orientations = np.matmul(rot_mat_x, rot_mat_y)
     coords = Q_([[1, 0, 0], [-1, 0, 2], [3, 5, 7], [-4, -5, -6]], "mm")
 
+    transformation_matrix = np.resize(np.identity(4), (4, 4, 4))
+    transformation_matrix[:, :3, :3] = orientations
+    transformation_matrix[:, :3, 3] = coords.m
+
     csm = tf.CoordinateSystemManager("root")
     lcs_default = tf.LocalCoordinateSystem()
 
@@ -2790,6 +2794,18 @@ def test_coordinate_system_manager_create_coordinate_system():
         time=time,
     )
 
+    # from homogeneous transformation ---------------------
+    csm.create_cs_from_homogeneous_transformation(
+        "lcs_homogeneous_default", "root", transformation_matrix, coords.u, time
+    )
+    check_coordinate_system(
+        csm.get_cs("lcs_homogeneous_default"),
+        orientations,
+        coords,
+        True,
+        time=time,
+    )
+
 
 def test_coordinate_system_manager_transform_data():
     """Test the coordinate system managers transform_data function."""

diff --git a/weldx/transformations/cs_manager.py b/weldx/transformations/cs_manager.py
@@ -17,10 +17,11 @@
 from weldx.exceptions import WeldxDeprecationWarning, WeldxException
 from weldx.geometry import SpatialData
 from weldx.time import Time, types_time_like, types_timestamp_like
+from weldx.types import UnitLike
 from weldx.util import check_matplotlib_available, dataclass_nested_eq
 
 from .local_cs import LocalCoordinateSystem
-from .types import types_coordinates, types_orientation
+from .types import types_coordinates, types_homogeneous, types_orientation
 
 # only import heavy-weight packages on type checking
 if TYPE_CHECKING:  # pragma: no cover
@@ -830,6 +831,51 @@ def create_cs_from_axis_vectors(
             coordinate_system_name, reference_system_name, lcs, lcs_child_in_parent
         )
 
+    def create_cs_from_homogeneous_transformation(
+        self,
+        coordinate_system_name: str,
+        reference_system_name: str,
+        transformation_matrix: types_homogeneous,
+        translation_unit: UnitLike,
+        time: types_time_like = None,
+        time_ref: types_timestamp_like = None,
+        lcs_child_in_parent: bool = True,
+    ):
+        """Create a coordinate system from a homogeneous transformation matrix and add
+        it to the coordinate system manager.
+
+        This function uses the `LocalCoordinateSystem.from_homogeneous_transformation`
+        method of the `LocalCoordinateSystem` class.
+
+        Parameters
+        ----------
+        coordinate_system_name :
+            Name of the new coordinate system.
+        reference_system_name :
+            Name of the parent system. This must have been already added.
+        transformation_matrix :
+            Describes the homogeneous transformation matrix that includes the rotation
+            and the translation (coordinates).
+        translation_unit :
+            Unit describing the value of the translation. Necessary, because the
+            homogeneous transformation matrix is unitless.
+        time :
+            Time data for time dependent coordinate systems.
+        time_ref :
+            Reference time for time dependent coordinate systems
+        lcs_child_in_parent :
+            If set to `True`, the passed `LocalCoordinateSystem` instance describes
+            the new system orientation towards is parent. If `False`, it describes
+            how the parent system is positioned in its new child system.
+
+        """
+        lcs = LocalCoordinateSystem.from_homogeneous_transformation(
+            transformation_matrix, translation_unit, time, time_ref
+        )
+        self.add_cs(
+            coordinate_system_name, reference_system_name, lcs, lcs_child_in_parent
+        )
+
     def delete_cs(self, coordinate_system_name: str, delete_children: bool = False):
         """Delete a coordinate system from the coordinate system manager.
 

diff --git a/weldx/transformations/local_cs.py b/weldx/transformations/local_cs.py
@@ -18,8 +18,13 @@
 from weldx.core import TimeSeries
 from weldx.exceptions import WeldxException
 from weldx.time import Time, TimeDependent, types_time_like, types_timestamp_like
-from weldx.transformations.types import types_coordinates, types_orientation
+from weldx.transformations.types import (
+    types_coordinates,
+    types_homogeneous,
+    types_orientation,
+)
 from weldx.transformations.util import normalize
+from weldx.types import UnitLike
 
 __all__ = ("LocalCoordinateSystem",)
 
@@ -540,6 +545,45 @@ def from_axis_vectors(
         t_axes = (1, 0) if mat.ndim == 2 else (1, 2, 0)
         return cls(mat.transpose(t_axes), coordinates, time, time_ref)
 
+    @classmethod
+    def from_homogeneous_transformation(
+        cls,
+        transformation_matrix: types_homogeneous,
+        translation_unit: UnitLike,
+        time: types_time_like = None,
+        time_ref: types_timestamp_like = None,
+    ) -> LocalCoordinateSystem:
+        """Construct a local coordinate system from a homogeneous transformation matrix.
+
+        Parameters
+        ----------
+        transformation_matrix :
+            Describes the homogeneous transformation matrix that includes the rotation
+            and the translation (coordinates).
+        translation_unit :
+            Unit describing the value of the translation. Necessary, because the
+            homogeneous transformation matrix is unitless.
+        time :
+            Time data for time dependent coordinate systems (Default value = None)
+        time_ref :
+            Optional reference timestamp if ``time`` is a time delta.
+
+        Returns
+        -------
+        LocalCoordinateSystem
+            Local coordinate system
+
+        """
+        if isinstance(transformation_matrix, xr.DataArray):
+            transformation_matrix = np.array(transformation_matrix.data)
+        if transformation_matrix.ndim == 3:
+            orientation = transformation_matrix[:, :3, :3]
+            coordinates = Q_(transformation_matrix[:, :3, 3], translation_unit)
+        else:
+            orientation = transformation_matrix[:3, :3]
+            coordinates = Q_(transformation_matrix[:3, 3], translation_unit)
+        return cls(orientation, coordinates=coordinates, time=time, time_ref=time_ref)
+
     @property
     def orientation(self) -> xr.DataArray:
         """Get the coordinate systems orientation matrix.
@@ -690,6 +734,44 @@ def as_rotation(self) -> Rot:  # pragma: no cover
         """
         return Rot.from_matrix(self.orientation.values)
 
+    def as_homogeneous_matrix(self, translation_unit: UnitLike) -> np.ndarray:
+        """Get a homogeneous transformation matrix from the coordinate system
+        orientation.
+
+        Parameters
+        ----------
+        translation_unit : UnitLike
+            Unit the translation part of the homogeneous transformation matrix
+            should represent.
+
+        Returns
+        -------
+        numpy.ndarray
+            Numpy array representing the homogeneous transformation matrix.
+
+        """
+
+        if self.is_time_dependent:
+            time_dim = self.time.shape[0]
+        else:
+            time_dim = 1
+
+        rotation = np.resize(self.orientation.data, (time_dim, 3, 3))
+        coordinates = self.coordinates
+        if not isinstance(coordinates, TimeSeries):
+            translation = np.resize(
+                coordinates.data.to(translation_unit).m, (time_dim, 3)
+            )
+            homogeneous_matrix = np.resize(np.identity(4), (time_dim, 4, 4))
+            homogeneous_matrix[:, :3, :3] = rotation
+            homogeneous_matrix[:, :3, 3] = translation
+
+            return np.squeeze(homogeneous_matrix)
+        else:
+            raise NotImplementedError(
+                "Cannot convert LCS with `TimeSeries` coordinates to homogeneous matrix"
+            )
+
     def _interp_time_orientation(self, time: Time) -> xr.DataArray:
         """Interpolate the orientation in time."""
         if "time" not in self.orientation.dims:  # don't interpolate static

diff --git a/weldx/transformations/types.py b/weldx/transformations/types.py
@@ -2,16 +2,19 @@
 
 from typing import Union
 
+import numpy as np
 import numpy.typing as npt
 import pint
 import xarray as xr
 from scipy.spatial.transform import Rotation
 
 types_coordinates = Union[xr.DataArray, npt.ArrayLike, pint.Quantity]
 types_orientation = Union[xr.DataArray, npt.ArrayLike, Rotation]
+types_homogeneous = Union[xr.DataArray, np.ndarray]
 
 
 __all__ = [
     "types_coordinates",
     "types_orientation",
+    "types_homogeneous",
 ]