diff --git a/doc/source/api.rst b/doc/source/api.rst index 9c62ed63..a2294874 100644 --- a/doc/source/api.rst +++ b/doc/source/api.rst @@ -27,6 +27,7 @@ Rotation Matrix ~matrix_requires_renormalization ~norm_matrix + ~polar_decomposition ~random_matrix diff --git a/pytransform3d/rotations/__init__.py b/pytransform3d/rotations/__init__.py index 32d5570f..0b238cbf 100644 --- a/pytransform3d/rotations/__init__.py +++ b/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" ] diff --git a/pytransform3d/rotations/_polar_decomp.py b/pytransform3d/rotations/_polar_decomp.py new file mode 100644 index 00000000..2096c574 --- /dev/null +++ b/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) diff --git a/pytransform3d/rotations/_utils.py b/pytransform3d/rotations/_utils.py index f76cab6b..3b219ec7 100644 --- a/pytransform3d/rotations/_utils.py +++ b/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])