cgse-coordinates 0.17.3__py3-none-any.whl → 0.18.0__py3-none-any.whl

egse/coordinates/transform3d_addon.py

@@ -1,65 +1,64 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-Created on Mon Jun 25 16:25:33 2018
-
-@author: pierre
-"""
-
-import numpy
 import numpy as np
 import math
 import transforms3d as t3
 
-from egse.coordinates.rotationMatrix import RotationMatrix
+from egse.coordinates.rotation_matrix import RotationMatrix
 
 
-def affine_isEuclidian(matrix):
-    """
-    Tests if a matrix is a pure solid-body euclidian rotation + translation (no shear or scaling)
+def affine_is_euclidian(matrix: np.ndarray) -> bool:
+    """Checks if the given matrix is a pure solid-body Euclidian rotation + translation (no shear or scaling).
 
-    We only need to check that
-    . the rotation part is orthogonal : R @ R.T = I
-    . the det(R) = 1  (=> this is not a reflexion)
+    We only need to check that:
+        - The rotation part is orthogonal : R @ R.T = I
+        - The det(R) = 1  (to check that the matrix does not represent a reflection)
+
+    Args:
+        matrix (np.ndarray): Matrix to check.
+
+    Returns:
+        True if the matrix is a pure solid-body Euclidian rotation + translation, False otherwise.
     """
+
     rotation = matrix[:3, :3]
+
     return np.allclose((rotation @ rotation.T), np.identity(3)) & np.allclose(np.linalg.det(matrix), 1)
 
 
 def affine_inverse(matrix):
+    """Returns the inverse of the given affine matrix.
+
+    We assume that the given matrix is an affine transformation matrix that only involves rotation and translation,
+    no zoom, no shear!  That is why we can invert it by simply transposing the rotation part and negating the
+    translation part.
+
+    Args:
+        matrix (np.ndarray): Augmented matrix to invert.
+
+    Returns:
+        Inverted affine matrix.
     """
-    affine_inverse(matrix)
 
-    WARNING:
-    This is NOT a generic inversion of an affine transformation matrix
+    if affine_is_euclidian(matrix):
+        # Separate the given augmented matrix into rotation and translation
 
-    This returns the affine transformation inverting that produced by the input matrix,
+        rotation = matrix[:3, :3]
+        translation = matrix[:3, 3]
 
-    ASSSUMING that only rotation and translation were involved
-    in the affine transformation, no zoom, no shear!
+        # Invert the rotation and the translation
 
-    That preserves the fact that the orthogonal property of the part of the input matrix
-    corresponding to rotation => the inverse is simply the transpose.
+        inverse_rotation = rotation.T
+        inverse_translation = -translation
 
-    Pierre Royer
-    """
-    # import numpy as np
-
-    # Extract Rotation matrix and translation vector from input affine transformation
-    R = matrix[:3, :3]
-    t = matrix[:3, 3]
-    #
-    # Invert the rotation and the translation
-    Rinv = R.T
-    tinv = -t
-    #
-    # The inverse affine is composed from R^-1 for the rotation and -(R^-1 . t) for the translation
-    result = np.identity(4)
-    result[:3, :3] = Rinv
-    result[:3, 3] = np.dot(Rinv, tinv)
+        # The inverse affine matrix is composed of
+        #   - Rotation: R^-1
+        #   - Translation: -(R^-1 . t)
+
+        result = np.identity(4)
+        result[:3, :3] = inverse_rotation
+        result[:3, 3] = np.dot(inverse_rotation, inverse_translation)
 
-    if affine_isEuclidian(result):
         return result
+
     else:
         print("WARNING: This is not a rigid-body transformation matrix")
         # print(f"R.T-based  (.6f) = \n {np.round(result,6)}")
@@ -67,9 +66,12 @@ def affine_inverse(matrix):
         return np.linalg.inv(matrix)
 
 
-def affine_matrix_from_points(v0, v1, shear=False, scale=False, usesvd=True):
-    """affine_matrix_from_points(v0, v1, shear=False, scale=False, usesvd=True)
-    Return affine transform matrix to register two point sets.
+def affine_matrix_from_points(
+    v0: np.ndarray, v1: np.ndarray, shear: bool = False, scale: bool = False, use_svd: bool = True
+) -> np.ndarray:
+    """Computes the homogeneous affine transform matrix that best maps one set of points to another.
+
+    Returns affine transform matrix to register two point sets.
 
     v0 and v1 are shape (ndims, \*) arrays of at least ndims non-homogeneous
     coordinates, where ndims is the dimensionality of the coordinate space.
@@ -78,7 +80,7 @@ def affine_matrix_from_points(v0, v1, shear=False, scale=False, usesvd=True):
     If also scale is False, a rigid/Euclidean transformation matrix
     is returned.
 
-    By default the algorithm by Hartley and Zissermann [15] is used.
+    By default, the algorithm by Hartley and Zissermann [15] is used.
     If usesvd is True, similarity and Euclidean transformation matrices
     are calculated by minimizing the weighted sum of squared deviations
     (RMSD) according to the algorithm by Kabsch [8].
@@ -94,76 +96,89 @@ def affine_matrix_from_points(v0, v1, shear=False, scale=False, usesvd=True):
     array([[   0.14549,    0.00062,  675.50008],
            [   0.00048,    0.14094,   53.24971],
            [   0.     ,    0.     ,    1.     ]])
-    >>> T = translation_matrix(numpy.random.random(3)-0.5)
-    >>> R = random_rotation_matrix(numpy.random.random(3))
+    >>> T = translation_matrix(np.random.random(3)-0.5)
+    >>> R = random_rotation_matrix(np.random.random(3))
     >>> S = scale_matrix(random.random())
     >>> M = concatenate_matrices(T, R, S)
-    >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20
+    >>> v0 = (np.random.rand(4, 100) - 0.5) * 20
     >>> v0[3] = 1
-    >>> v1 = numpy.dot(M, v0)
-    >>> v0[:3] += numpy.random.normal(0, 1e-8, 300).reshape(3, -1)
+    >>> v1 = np.dot(M, v0)
+    >>> v0[:3] += np.random.normal(0, 1e-8, 300).reshape(3, -1)
     >>> M = affine_matrix_from_points(v0[:3], v1[:3])
-    >>> numpy.allclose(v1, numpy.dot(M, v0))
+    >>> np.allclose(v1, np.dot(M, v0))
     True
 
     More examples in superimposition_matrix()
 
-    Author: this function was extracted from the original transformations.py
-            written by Christoph Golke:
-            https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
+    References: This function was extracted from the original transformations.py written by Christoph Golke:
+                https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
 
-    usesvd controls the use of a method based on Singular Value Decomposition (SVD)
-    --> when True, it is equivalent to rigid_transform_3D (see below)
+    Args:
+        v0 (np.ndarray): Set of points to transform.
+        v1 (np.ndarray): Set of points to transform to.
+        shear (bool): Indicates whether a full affine transform is allowed (i.e. incl. shear).
+        scale (bool): Indicates whether a full affine transform is allowed (i.e. incl. zoom).
+        use_svd (bool): Indicates whether to use SVD-base solution (Singular Value Decomposition) for
+                        rigid/similarity transformations.  If False and ndims = 3, the quaternion-based solution is used.
 
+    Returns:
+        Best affine transformation matrix to map `v0` to `v1`.
     """
-    import numpy
 
-    v0 = numpy.array(v0, dtype=numpy.float64, copy=True)
-    v1 = numpy.array(v1, dtype=numpy.float64, copy=True)
+    v0 = np.array(v0, dtype=np.float64, copy=True)
+    v1 = np.array(v1, dtype=np.float64, copy=True)
 
-    ndims = v0.shape[0]
-    if ndims < 2 or v0.shape[1] < ndims or v0.shape != v1.shape:
-        print(f"ndims {ndims} v0/1.shape {v0.shape} {v1.shape} v0/1 class {v0.__class__} {v1.__class__}")
+    num_dimensions = v0.shape[0]
+    if num_dimensions < 2 or v0.shape[1] < num_dimensions or v0.shape != v1.shape:
+        print(
+            f"num_dimensions {num_dimensions} v0/1.shape {v0.shape} {v1.shape} v0/1 class {v0.__class__} {v1.__class__}"
+        )
         raise ValueError("input arrays are of wrong shape or type")
 
-    # move centroids to origin
-    t0 = -numpy.mean(v0, axis=1)
-    M0 = numpy.identity(ndims + 1)
-    M0[:ndims, ndims] = t0
-    v0 += t0.reshape(ndims, 1)
-    t1 = -numpy.mean(v1, axis=1)
-    M1 = numpy.identity(ndims + 1)
-    M1[:ndims, ndims] = t1
-    v1 += t1.reshape(ndims, 1)
+    # First set of coordinates
+
+    t0 = -np.mean(v0, axis=1)  # Move centroids to origin
+    v0 += t0.reshape(num_dimensions, 1)
+    matrix_0 = np.identity(num_dimensions + 1)
+    matrix_0[:num_dimensions, num_dimensions] = t0  # (I | t0)
+
+    # Second set of coordinates
+
+    t1 = -np.mean(v1, axis=1)  # Move centroids to origin
+    v1 += t1.reshape(num_dimensions, 1)
+    matrix_1 = np.identity(num_dimensions + 1)
+    matrix_1[:num_dimensions, num_dimensions] = t1  # (I | t1)
 
     if shear:
         # Affine transformation
-        A = numpy.concatenate((v0, v1), axis=0)
-        u, s, vh = numpy.linalg.svd(A.T)
-        vh = vh[:ndims].T
-        B = vh[:ndims]
-        C = vh[ndims : 2 * ndims]
-        t = numpy.dot(C, numpy.linalg.pinv(B))
-        t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1)
-        M = numpy.vstack((t, ((0.0,) * ndims) + (1.0,)))
-    elif usesvd or ndims != 3:
+        A = np.concatenate((v0, v1), axis=0)
+        svd_u, svd_s, svd_vh = np.linalg.svd(A.T)  # Singular Value Decomposition -> U, S, Vh
+        svd_vh = svd_vh[:num_dimensions].T
+        B = svd_vh[:num_dimensions]
+        C = svd_vh[num_dimensions : 2 * num_dimensions]
+        t = np.dot(C, np.linalg.pinv(B))
+        t = np.concatenate((t, np.zeros((num_dimensions, 1))), axis=1)
+        M = np.vstack((t, ((0.0,) * num_dimensions) + (1.0,)))
+
+    elif use_svd or num_dimensions != 3:
         # Rigid transformation via SVD of covariance matrix
-        u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T))
+        svd_u, svd_s, svd_vh = np.linalg.svd(np.dot(v1, v0.T))
         # rotation matrix from SVD orthonormal bases
-        R = numpy.dot(u, vh)
-        if numpy.linalg.det(R) < 0.0:
-            # R does not constitute right handed system
-            R -= numpy.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0)
-            s[-1] *= -1.0
+        R = np.dot(svd_u, svd_vh)
+        if np.linalg.det(R) < 0.0:
+            # R does not constitute right-handed system
+            R -= np.outer(svd_u[:, num_dimensions - 1], svd_vh[num_dimensions - 1, :] * 2.0)
+            svd_s[-1] *= -1.0
         # homogeneous transformation matrix
-        M = numpy.identity(ndims + 1)
-        M[:ndims, :ndims] = R
+        M = np.identity(num_dimensions + 1)
+        M[:num_dimensions, :num_dimensions] = R
+
     else:
         # Rigid transformation matrix via quaternion
         # compute symmetric matrix N
-        xx, yy, zz = numpy.sum(v0 * v1, axis=1)
-        xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1)
-        xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1)
+        xx, yy, zz = np.sum(v0 * v1, axis=1)
+        xy, yz, zx = np.sum(v0 * np.roll(v1, -1, axis=0), axis=1)
+        xz, yx, zy = np.sum(v0 * np.roll(v1, -2, axis=0), axis=1)
         N = [
             [xx + yy + zz, 0.0, 0.0, 0.0],
             [yz - zy, xx - yy - zz, 0.0, 0.0],
@@ -171,42 +186,43 @@ def affine_matrix_from_points(v0, v1, shear=False, scale=False, usesvd=True):
             [xy - yx, zx + xz, yz + zy, zz - xx - yy],
         ]
         # quaternion: eigenvector corresponding to most positive eigenvalue
-        w, V = numpy.linalg.eigh(N)
-        q = V[:, numpy.argmax(w)]
-        q /= _vector_norm(q)  # unit quaternion
-        # homogeneous transformation matrix
-        M = _quaternion_matrix(q)
+        eigenvalues, eigenvectors = np.linalg.eigh(N)
+        quaternion = eigenvectors[:, np.argmax(eigenvalues)]
+        quaternion /= _vector_norm(quaternion)  # Normalised quaternion
+        # Quaternion -> Homogeneous rotation matrix
+        M = quaternion_matrix(quaternion)
 
     if scale and not shear:
         # Affine transformation; scale is ratio of RMS deviations from centroid
         v0 *= v0
         v1 *= v1
-        M[:ndims, :ndims] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0))
+        M[:num_dimensions, :num_dimensions] *= math.sqrt(np.sum(v1) / np.sum(v0))
+
+    # Move centroids back
+    M = np.dot(np.linalg.inv(matrix_1), np.dot(M, matrix_0))
+    M /= M[num_dimensions, num_dimensions]
 
-    # move centroids back
-    M = numpy.dot(numpy.linalg.inv(M1), numpy.dot(M, M0))
-    M /= M[ndims, ndims]
     return M
 
 
-def _vector_norm(data, axis=None, out=None):
-    """Return length, i.e. Euclidean norm, of ndarray along axis.
+def _vector_norm(data: np.ndarray, axis: str | None = None, out=None):
+    """Returns the length, i.e. Euclidean norm, of the given array along the given axis.
 
-    >>> v = numpy.random.random(3)
+    >>> v = np.random.random(3)
     >>> n = vector_norm(v)
-    >>> numpy.allclose(n, numpy.linalg.norm(v))
+    >>> np.allclose(n, np.linalg.norm(v))
     True
-    >>> v = numpy.random.rand(6, 5, 3)
+    >>> v = np.random.rand(6, 5, 3)
     >>> n = vector_norm(v, axis=-1)
-    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
+    >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=2)))
     True
     >>> n = vector_norm(v, axis=1)
-    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
+    >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
     True
-    >>> v = numpy.random.rand(5, 4, 3)
-    >>> n = numpy.empty((5, 3))
+    >>> v = np.random.rand(5, 4, 3)
+    >>> n = np.empty((5, 3))
     >>> vector_norm(v, axis=1, out=n)
-    >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
+    >>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
     True
     >>> vector_norm([])
     0.0
@@ -215,53 +231,63 @@ def _vector_norm(data, axis=None, out=None):
 
     This function is called by affine_matrix_from_points when usesvd=False
 
-    Author: this function was extracted from the original transformations.py
-            written by Christoph Golke:
-            https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
+    References: This function was extracted from the original transformations.py written by Christoph Golke:
+                https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
 
     """
-    data = numpy.array(data, dtype=numpy.float64, copy=True)
+
+    data = np.array(data, dtype=np.float64, copy=True)
+
     if out is None:
         if data.ndim == 1:
-            return math.sqrt(numpy.dot(data, data))
+            return math.sqrt(np.dot(data, data))
         data *= data
-        out = numpy.atleast_1d(numpy.sum(data, axis=axis))
-        numpy.sqrt(out, out)
+        out = np.atleast_1d(np.sum(data, axis=axis))
+        np.sqrt(out, out)
+
         return out
     else:
         data *= data
-        numpy.sum(data, axis=axis, out=out)
-        numpy.sqrt(out, out)
+        np.sum(data, axis=axis, out=out)
+        return np.sqrt(out, out)
 
 
-def _quaternion_matrix(quaternion):
-    """Return homogeneous rotation matrix from quaternion.
+def quaternion_matrix(quaternion: np.ndarray):
+    """Returns the homogeneous rotation matrix from the given quaternion.
 
     >>> M = quaternion_matrix([0.99810947, 0.06146124, 0, 0])
-    >>> numpy.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
+    >>> np.allclose(M, rotation_matrix(0.123, [1, 0, 0]))
     True
     >>> M = quaternion_matrix([1, 0, 0, 0])
-    >>> numpy.allclose(M, numpy.identity(4))
+    >>> np.allclose(M, np.identity(4))
     True
     >>> M = quaternion_matrix([0, 1, 0, 0])
-    >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1]))
+    >>> np.allclose(M, np.diag([1, -1, -1, 1]))
     True
 
     This function is called by affine_matrix_from_points when usesvd=False
 
-    Author: this function was extracted from the original transformations.py
-            written by Christoph Golke:
-            https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
+    References: This function was extracted from the original transformations.py written by Christoph Golke:
+                https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
+
+    Args:
+        quaternion (np.ndarray): Quaternion to convert to a rotation matrix.
+
+    Returns:
+        Homogeneous rotation matrix corresponding to the given quaternion.
     """
+
     _EPS = np.finfo(float).eps * 5
 
-    q = numpy.array(quaternion, dtype=numpy.float64, copy=True)
-    n = numpy.dot(q, q)
+    q = np.array(quaternion, dtype=np.float64, copy=True)
+    n = np.dot(q, q)
+
     if n < _EPS:
-        return numpy.identity(4)
+        return np.identity(4)
+
     q *= math.sqrt(2.0 / n)
-    q = numpy.outer(q, q)
-    return numpy.array(
+    q = np.outer(q, q)
+    return np.array(
         [
             [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], 0.0],
             [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], 0.0],
@@ -271,167 +297,205 @@ def _quaternion_matrix(quaternion):
     )
 
 
-def rigid_transform_3D(fromA, toB, verbose=True):
-    """rigid_transform_3D(fromA, toB, verbose=True)
-
-    INPUT
-    Afrom, Bto 3xn arrays = xyz coords of n points to be registered
+def rigid_transform_3d(dataset_a: np.ndarray, dataset_b: np.ndarray):
+    """Returns best translation and rotation to align points in dataset A to points in dataset B.
 
-    OUTPUT
-    Rotation + translation transformation matrix registering fromA into toB
+    Args:
+        dataset_a (np.ndarray): First 3xn dataset of points (dataset A).
+        dataset_b (np.ndarray): Second 3xn dataset of points (dataset B).
 
-    Author : Nghia Ho - 2013 - http://nghiaho.com/?page_id=671
-            "Finding optimal rotation and translation between corresponding 3D points"
-             Based on "A Method for Registration of 3-D Shapes", by Besl and McKay, 1992.
+    References: Nghia Ho - 2013 - http://nghiaho.com/?page_id=671
+                "Finding optimal rotation and translation between corresponding 3D points"
+                Based on "A Method for Registration of 3-D Shapes", by Besl and McKay, 1992.
 
     This is based on Singular Value Decomposition (SVD)
-    --> it is equivalent to affine_matrix_from_points with parameter usesvd=True
+    -> It is equivalent to affine_matrix_from_points with parameter use_svd=True
+
+    Returns:
+        Transformation matrix to align points in dataset A to points in dataset B.
     """
-    A = fromA.T
-    B = toB.T
 
-    assert len(A) == len(B)
+    dataset_a_transposed = dataset_a.T
+    dataset_b_transposed = dataset_b.T
 
-    N = A.shape[0]  # total points
+    assert len(dataset_a_transposed) == len(dataset_b_transposed)
 
-    centroid_A = np.mean(A, axis=0)
-    centroid_B = np.mean(B, axis=0)
+    num_points = dataset_a_transposed.shape[0]  # Total points
 
-    # centre the points
-    AA = A - np.tile(centroid_A, (N, 1))
-    BB = B - np.tile(centroid_B, (N, 1))
+    centroid_a = np.mean(dataset_a_transposed, axis=0)
+    centroid_b = np.mean(dataset_b_transposed, axis=0)
+
+    # Centre the points
+    a_centered = dataset_a_transposed - np.tile(centroid_a, (num_points, 1))
+    b_centered = dataset_b_transposed - np.tile(centroid_b, (num_points, 1))
 
     # @ is matrix multiplication for array
-    H = np.transpose(AA) @ BB
+    covariance_matrix = np.transpose(a_centered) @ b_centered  # Covariance matrix H
+
+    svd_u, svd_s, svd_vh = np.linalg.svd(covariance_matrix)  # SVD(H) = [U, S, V]
 
-    U, S, Vt = np.linalg.svd(H)
+    rotation = svd_vh.T @ svd_u.T  # Rotation matrix R
 
-    R = Vt.T @ U.T
+    # Special reflection case
+    # There’s a special case when finding the rotation matrix that you have to take care of. Sometimes the SVD will
+    # return a "reflection" matrix, which is numerically correct but is actually nonsense in real life. This is
+    # addressed by checking the determinant of R (from SVD above) and seeing if it’s negative (-1). If it is then the
+    # 3rd column of V is multiplied by -1.
 
-    # special reflection case
-    if np.linalg.det(R) < 0:
+    if np.linalg.det(rotation) < 0:
         print("Reflection detected")
-        Vt[2, :] *= -1
-        R = Vt.T @ U.T
+        svd_vh[2, :] *= -1
+        rotation = svd_vh.T @ svd_u.T
 
-    t = -R @ centroid_A.T + centroid_B.T
+    translation = -rotation @ centroid_a.T + centroid_b.T
 
     result = np.identity(4)
-    result[:3, :3] = R
-    result[:3, 3] = t
+    result[:3, :3] = rotation
+    result[:3, 3] = translation
 
     return result
 
 
-def translationRotationToTransformation(
-    translation, rotation, rot_config="sxyz", active=True, degrees=True, translationFirst=False
+def translation_rotation_to_transformation(
+    translation: np.ndarray,
+    rotation: np.ndarray,
+    rotation_config: str = "sxyz",
+    active: bool = True,
+    degrees: bool = True,
+    translation_first: bool = False,
 ):
+    """Returns the transformation matrix from the given translation and rotation.
+
+    Args:
+        translation (np.ndarray): Translation vector.
+        rotation (np.ndarray): Rotation vector.
+        rotation_config (str): Order in which the rotation about the three axes are chained.
+        active (bool): Indicates if the rotation is active (object rotates IN a fixed coordinate system) or passive
+                           (coordinate system rotates AROUND a fixed object).  Even if two angles are zero, the match
+                           between angle orders and rot_config is still critical
+        degrees (bool): Indicates whether the rotation angles are specified in degrees, rather than radians.
+        translation_first (bool): Indicates the order of the translation and rotation in the transformation matrix.
+                                  False if the first three rows of the transformation matrix are (R t). This is the
+                                  usual convention.
+                                  True if the first three rows of the transformation matrix are (R Rt).  This is used
+                                  in the hexapod.
+
+    Returns:
+        Transformation matrix corresponding to the given translation and rotation.
     """
-    translationRotationToTransformation(translation,rotation,rot_config="sxyz",active=True,degrees=True,translationFirst=False)
 
-    translationFirst : translation first
-             False first 3 rows of transformation matrix = (R   t) [usual convention and default here]
-             True  first 3 rows of transformation matrix = (R  Rt) [used in the hexapod]
-    """
-    import transforms3d as t3
-    import numpy as np
-
-    # Zoom - unit
-    zdef = np.array([1, 1, 1])
-    # Shear
-    sdef = np.array([0, 0, 0])
+    zoom = np.array([1, 1, 1])
+    shear = np.array([0, 0, 0])
     translation = np.array(translation)
-    # if degrees: rotation = np.deg2rad(np.array(rotation))
     if degrees:
         rotation = np.array([np.deg2rad(item) for item in rotation])
-    rotx, roty, rotz = rotation
-    rmat = RotationMatrix(rotx, roty, rotz, rot_config=rot_config, active=active)
-    #
-    if translationFirst:
+    rotation_x, rotation_y, rotation_z = rotation
+    rotation_matrix = RotationMatrix(rotation_x, rotation_y, rotation_z, rotation_config=rotation_config, active=active)
+
+    if translation_first:
         result = np.identity(4)
-        result[:3, :3] = rmat.R
-        result[:3, 3] = rmat.R @ translation
+        result[:3, :3] = rotation_matrix.rotation_matrix
+        result[:3, 3] = rotation_matrix.rotation_matrix @ translation
     else:
-        result = t3.affines.compose(translation, rmat.R, Z=zdef, S=sdef)
+        result = t3.affines.compose(translation, rotation_matrix.rotation_matrix, Z=zoom, S=shear)
+
     return result
 
 
-def translationRotationFromTransformation(
-    transformation, rot_config="sxyz", active=True, degrees=True, translationFirst=False
+def translation_rotation_from_transformation(
+    transformation: np.ndarray,
+    rotation_config: str = "sxyz",
+    active: bool = True,
+    degrees: bool = True,
+    translation_first: bool = False,
 ):
+    """Extracts the translation and rotation vector from the given transformation matrix.
+
+    Args:
+    transformation (np.ndarray): Transformation matrix.
+    rotation_config (str): Order in which the rotation about the three axes are chained.
+    active (bool): Indicates if the rotation is active (object rotates IN a fixed coordinate system) or passive
+                       (coordinate system rotates AROUND a fixed object).  Even if two angles are zero, the match
+                       between angle orders and rot_config is still critical
+    degrees (bool): Indicates whether the rotation angles are specified in degrees, rather than radians.
+    translation_first (bool): Indicates the order of the translation and rotation in the transformation matrix.
+                              False if the first three rows of the transformation matrix are (R t). This is the
+                              usual convention.
+                              True if the first three rows of the transformation matrix are (R Rt).  This is used
+                              in the hexapod.
+
+    Returns:
+        Translation and rotation vector for the given transformation matrix.
     """
-    translationRotationFromTransformation(transformation,rot_config="sxyz",active=True,degrees=True,translationFirst=False)
 
-    translationFirst : translation first
-             False first 3 rows of transformation matrix = (R   t) [usual convention and default here]
-             True  first 3 rows of transformation matrix = (R  Rt) [used in the hexapod]
-    """
     translation = transformation[:3, 3]
-    rotation = t3.euler.mat2euler(transformation, axes=rot_config)
+    rotation = t3.euler.mat2euler(transformation, axes=rotation_config)
     if degrees:
         rotation = np.array([np.rad2deg(item) for item in rotation])
-    if translationFirst:
+    if translation_first:
         translation = transformation[:3, :3].T @ translation
+
     return translation, rotation
 
 
-tr2T = translationRotationToTransformation
-T2tr = translationRotationFromTransformation
+tr2t = translation_rotation_to_transformation
+t2tr = translation_rotation_from_transformation
 
 
-def vectorPlaneIntersection(pt, frame, epsilon=1.0e-6):
-    """
-    return the coordinates of the intersection of a vector with a plane.
+def vector_plane_intersection(vector, frame, epsilon=1.0e-6):
+    """Returns the coordinates of the insection of a vector with a plane.
+
+    The origin of the input vector is:
+        vector.reference_frame.get_origin().coordinates[:3]
+
+    The direction of the input vector is:
+        vector.coordinates[:3]
 
-    pt = input vector. Point object, expressing the vector
-         vector origin = pt.ref.getOrigin().coordinates[:3]
-         vector direction = pt.coordinates[:3]
-    frame = input plane. ReferenceFrame object whose x-y plane is the target plane for intersection
+    In all cases, the coordinates of the intersection point are provided as a Point object, in "frame" coordinates
 
-    If the vector's own reference frame is 'frame', the problem is trivial
+    Args:
+        vector (Vector): Input vector.
+        frame (ReferenceFrame): Reference frame for which the xy-plane is the target plane for intersection.
+        epsilon (float):
 
-    In all cases, the coordinates of the interesection point are provided as a Point object, in "frame" coordinates
 
-    Ref:
-    https://stackoverflow.com/questions/5666222/3d-line-plane-intersection
+    References:
+        https://stackoverflow.com/questions/5666222/3d-line-plane-intersection
     """
 
     from egse.coordinates.point import Point
 
-    if pt.ref == frame:
+    if vector.reference_frame == frame:
         # The point is defined in frame => the origin of the vector is the origin of the target plane.
         return np.array([0, 0, 0])
     else:
-        # Express all inputs in 'frame'
+        # Express all inputs in the given reference frame
 
-        # Vector Origin (p0)
-        vec_orig = Point(pt.ref.getOrigin().coordinates[:3], ref=pt.ref, name="ptorig").expressIn(frame)[:3]
-        # Vector End (p1)
-        vec_end = pt.expressIn(frame)[:3]
-        # Vector (u)
-        vec = vec_end - vec_orig
+        vector_origin = Point(
+            vector.reference_frame.get_origin().coordinates[:3], reference_frame=vector.reference_frame, name="ptorig"
+        ).express_in(frame)[:3]  # Vector Origin (p0)
+        vector_end = vector.express_in(frame)[:3]  # Vector end (p1)
 
-        # A point in Plane (pco)
-        # plane_orig = np.array([0,0,0],dtype=float)
-        plane_orig = frame.getOrigin().coordinates[:3]
-        # Normal to the plane (pno)
-        plane_normal = frame.getAxis("z").coordinates[:3]
+        vector_u = vector_end - vector_origin  # Vector (u)
+
+        plane_origin = frame.get_origin().coordinates[:3]  # Origin of the reference frame (p_co)
+        plane_normal = frame.get_axis("z").coordinates[:3]  # Normal to the plane (p_no)
 
         # Vector to normal 'angle'
-        vec_x_normal = np.dot(vec, plane_normal)
+        dot = np.dot(vector_u, plane_normal)  # dot = p_no * u
 
         # Test if there is an intersection (and if it's unique)
-        # --> input vector and normal mustn't be perpendicular, else the vector is // to the plane or inside it
-        #
-        if np.allclose(vec_x_normal, 0.0, atol=epsilon):
+        # -> input vector and normal mustn't be perpendicular, else the vector is // to the plane or inside it
+
+        if np.allclose(dot, 0.0, atol=epsilon):
             print("The input vector is // to the plane normal (or inside the plane)")
-            print("--> there exists no intersection (or an infinity of them)")
+            print("-> There exists no intersection (or an infinity of them)")
             return None
         else:
             # Vector from the point in the plane to the origin of the vector (w)
-            plane_to_vec = vec_orig - plane_orig
+            plane_to_vector = vector_origin - plane_origin  # w = p0 - p_co
 
-            # Solution  ("how many 'vectors' away is the interesection ?")
-            vec_multiplicator = -np.dot(plane_normal, plane_to_vec) / vec_x_normal
+            # Solution  ("how many 'vectors' away is the intersection ?")
+            factor = -np.dot(plane_normal, plane_to_vector) / dot  # fac = -(plane * w) / fac
 
-            return Point(vec_orig + (vec * vec_multiplicator), ref=frame)
+            return Point(vector_origin + (vector_u * factor), reference_frame=frame)