Plot Polar Decomposition

Robust polar decomposition orthonormalizes basis vectors (i.e., rotation matrices). It is more expensive than standard Gram-Schmidt orthonormalization, but it spreads the error more evenly over all basis vectors. The top row of these plots shows the unnormalized bases that were obtained by randomly rotating one of the columns of the identity matrix. The middle row shows Gram-Schmidt orthonormalization and the bottom row shows orthonormalization through robust polar decomposition. For comparison, we show the unnormalized basis with dashed lines in the last two rows.

/home/afabisch/Data/Projects/pytransform3d/pytransform3d/pytransform3d/rotations/_plot.py:54: UserWarning: Expected rotation matrix, but it failed the test for inversion by transposition. np.dot(R, R.T) gives array([[ 1.05720187, -0.21547284, -0.08660988],
       [-0.21547284,  0.81166132,  0.32624941],
       [-0.08660988,  0.32624941,  1.13113681]])
  R = check_matrix(R, strict_check=strict_check)
/home/afabisch/Data/Projects/pytransform3d/pytransform3d/pytransform3d/rotations/_plot.py:54: UserWarning: Expected rotation matrix, but it failed the test for inversion by transposition. np.dot(R, R.T) gives array([[ 1.18100749, -0.14921234, -0.35493584],
       [-0.14921234,  1.12300223,  0.29258905],
       [-0.35493584,  0.29258905,  0.69599029]])
  R = check_matrix(R, strict_check=strict_check)
/home/afabisch/Data/Projects/pytransform3d/pytransform3d/pytransform3d/rotations/_plot.py:54: UserWarning: Expected rotation matrix, but it failed the test for inversion by transposition. np.dot(R, R.T) gives array([[ 1.02770255, -0.00322832,  0.16408747],
       [-0.00322832,  1.00037621, -0.01912193],
       [ 0.16408747, -0.01912193,  0.97192124]])
  R = check_matrix(R, strict_check=strict_check)
/home/afabisch/Data/Projects/pytransform3d/pytransform3d/pytransform3d/rotations/_plot.py:54: UserWarning: Expected rotation matrix, but it failed the test for inversion by transposition. np.dot(R, R.T) gives array([[ 1.07023019, -0.04169216, -0.25211044],
       [-0.04169216,  1.02475055,  0.14966538],
       [-0.25211044,  0.14966538,  0.90501926]])
  R = check_matrix(R, strict_check=strict_check)

import matplotlib.pyplot as plt
import numpy as np

from pytransform3d import rotations as pr

n_cases = 4
fig, axes = plt.subplots(
    3, n_cases, subplot_kw={"projection": "3d"}, figsize=(8, 8)
)
ax_s = 1.0
plot_center = np.array([-0.2, -0.2, -0.2])
for ax in axes.flat:
    ax.set_xticks([])
    ax.set_yticks([])
    ax.set_zticks([])
    ax.set_xlim(-ax_s, ax_s)
    ax.set_ylim(-ax_s, ax_s)
    ax.set_zlim(-ax_s, ax_s)

titles = ["Unnormalized Bases", "Gram-Schmidt", "Polar Decomposition"]
for ax, title in zip(axes[:, 0], titles):
    ax.set_title(title)

rng = np.random.default_rng(46)
for i in range(n_cases):
    random_axis = rng.integers(0, 3)
    R_unnormalized = np.eye(3)
    R_unnormalized[:, random_axis] = np.dot(
        pr.random_matrix(rng, cov=0.1 * np.eye(3)),
        R_unnormalized[:, random_axis],
    )
    pr.plot_basis(axes[0, i], R_unnormalized, p=plot_center, strict_check=False)

    R_gs = pr.norm_matrix(R_unnormalized)
    pr.plot_basis(
        axes[1, i], R_unnormalized, p=plot_center, strict_check=False, ls="--"
    )
    pr.plot_basis(axes[1, i], R_gs, p=plot_center)

    R_pd = pr.robust_polar_decomposition(R_unnormalized)
    pr.plot_basis(
        axes[2, i], R_unnormalized, p=plot_center, strict_check=False, ls="--"
    )
    pr.plot_basis(axes[2, i], R_pd, p=plot_center)

plt.tight_layout()
plt.show()