MAINT: extract polar decomposition

doc/source/api.rst

@@ -27,6 +27,7 @@ Rotation Matrix
 
    ~matrix_requires_renormalization
    ~norm_matrix
+   ~polar_decomposition
 
    ~random_matrix
 

pytransform3d/rotations/__init__.py

@@ -111,6 +111,7 @@
 from ._jacobians import (
     left_jacobian_SO3, left_jacobian_SO3_series, left_jacobian_SO3_inv,
     left_jacobian_SO3_inv_series)
+from ._polar_decomp import polar_decomposition
 
 __all__ = [
     "eps",
@@ -273,5 +274,6 @@
     "left_jacobian_SO3",
     "left_jacobian_SO3_series",
     "left_jacobian_SO3_inv",
-    "left_jacobian_SO3_inv_series"
+    "left_jacobian_SO3_inv_series",
+    "polar_decomposition"
 ]

pytransform3d/rotations/_polar_decomp.py

@@ -0,0 +1,57 @@
+import numpy as np
+from ._conversions import (matrix_from_quaternion,
+                           quaternion_from_compact_axis_angle)
+from ._quaternions import concatenate_quaternions
+
+
+def polar_decomposition(R, n_iter=20, eps=np.finfo(float).eps):
+    r"""Orthonormalize rotation matrix with polar decomposition.
+
+    Use polar decomposition [1] [2] to normalize rotation matrix. This is a
+    computationally more costly method, but it spreads the error more
+    evenly between the basis vectors.
+
+    Parameters
+    ----------
+    R : array-like, shape (3, 3)
+        Rotation matrix with small numerical errors.
+
+    TODO
+
+    Returns
+    -------
+    R : array, shape (3, 3)
+        Orthonormalized rotation matrix.
+
+    See Also
+    --------
+    norm_matrix
+        The cheaper default orthonormalization method that uses Gram-Schmidt
+        orthonormalization optimized for 3 dimensions.
+
+    References
+    ----------
+    .. [1] Selstad, J. (2019). Orthonormalization.
+       https://zalo.github.io/blog/polar-decomposition/
+
+    .. [2] Müller, M., Bender, J., Chentanez, N., Macklin, M. (2016).
+       A Robust Method to Extract the Rotational Part of Deformations.
+       In MIG '16: Proceedings of the 9th International Conference on Motion in
+       Games, pp. 55-60, doi: 10.1145/2994258.2994269.
+    """
+    current_q = np.array([1.0, 0.0, 0.0, 0.0])
+    for _ in range(n_iter):
+        current_R = matrix_from_quaternion(current_q)
+        omega = ((np.cross(current_R[:, 0], R[:, 0])
+                  + np.cross(current_R[:, 1], R[:, 1])
+                  + np.cross(current_R[:, 2], R[:, 2]))
+                 /
+                 (abs(np.dot(current_R[:, 0], R[:, 0])
+                      + np.dot(current_R[:, 1], R[:, 1])
+                      + np.dot(current_R[:, 2], R[:, 2]))
+                  + eps))
+        if np.linalg.norm(omega) < eps:
+            break
+        current_q = concatenate_quaternions(
+            quaternion_from_compact_axis_angle(omega), current_q)
+    return matrix_from_quaternion(current_q)

pytransform3d/rotations/_utils.py

@@ -52,7 +52,7 @@ def matrix_requires_renormalization(R, tolerance=1e-6):
     return not np.allclose(RRT, np.eye(3), atol=tolerance)
 
 
-def norm_matrix(R, polar_decomposition=False):
+def norm_matrix(R):
     r"""Orthonormalize rotation matrix.
 
     A rotation matrix is defined as
@@ -76,7 +76,8 @@ def norm_matrix(R, polar_decomposition=False):
     :math:`\boldsymbol R^T \boldsymbol R = \boldsymbol I`.
 
     Because of numerical problems, a rotation matrix might not satisfy the
-    constraints anymore. This function will enforce them.
+    constraints anymore. This function will enforce them with Gram-Schmidt
+    orthonormalization optimized for 3 dimensions.
 
     Parameters
     ----------
@@ -100,48 +101,10 @@ def norm_matrix(R, polar_decomposition=False):
     check_matrix : Checks orthonormality of a rotation matrix.
     matrix_requires_renormalization
         Checks if a rotation matrix needs renormalization.
-
-    References
-    ----------
-    .. [1] Selstad, J. (2019). Orthonormalization.
-       https://zalo.github.io/blog/polar-decomposition/
-
-    .. [2] Müller, M., Bender, J., Chentanez, N., Macklin, M. (2016).
-       A Robust Method to Extract the Rotational Part of Deformations.
-       In MIG '16: Proceedings of the 9th International Conference on Motion in
-       Games, pp. 55-60, doi: 10.1145/2994258.2994269.
+    polar_decomposition
+        A more expensive orthonormalization method that spreads the error more
+        evenly between the basis vectors.
     """
-    if polar_decomposition:
-        return _polar_decomposition(R)
-    else:
-        return _gram_schmidt_orthonormalization_3d(R)
-
-
-def _polar_decomposition(R, n_iter=20, eps=np.finfo(float).eps):
-    # to avoid circular import
-    from ._conversions import (matrix_from_quaternion,
-                               quaternion_from_compact_axis_angle)
-    from ._quaternions import concatenate_quaternions
-
-    current_q = np.array([1.0, 0.0, 0.0, 0.0])
-    for _ in range(n_iter):
-        current_R = matrix_from_quaternion(current_q)
-        omega = ((np.cross(current_R[:, 0], R[:, 0])
-                  + np.cross(current_R[:, 1], R[:, 1])
-                  + np.cross(current_R[:, 2], R[:, 2]))
-                 /
-                 (abs(np.dot(current_R[:, 0], R[:, 0])
-                      + np.dot(current_R[:, 1], R[:, 1])
-                      + np.dot(current_R[:, 2], R[:, 2]))
-                  + eps))
-        if np.linalg.norm(omega) < eps:
-            break
-        current_q = concatenate_quaternions(
-            quaternion_from_compact_axis_angle(omega), current_q)
-    return matrix_from_quaternion(current_q)
-
-
-def _gram_schmidt_orthonormalization_3d(R):
     R = np.asarray(R)
     c2 = R[:, 1]
     c3 = norm_vector(R[:, 2])