Imprecision with rotation matrix to align a vector to an axis

Question

I've been banging my head against the wall with this for several hours and I can't seem to figure out what I'm doing wrong.

I'm trying to generate a rotation matrix which will align a vector with a particular axis (I'll ultimately be transforming more data, so having the rotation matrix is important).

I feel like my method is right, and if I test it on a variety of vectors, it works pretty well, but the transformed vectors are always a little off.

Here's a full code sample I'm using to test the method:

import numpy as np
import matplotlib.pyplot as plt
import mpl_toolkits.mplot3d
import matplotlib as mpl


def get_rotation_matrix(i_v, unit=None):
    # From http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38
    if unit is None:
        unit = [1.0, 0.0, 0.0]
    # Normalize vector length
    i_v = np.divide(i_v, np.sqrt(np.dot(i_v, i_v)))
    # Get axis
    u, v, w = np.cross(i_v, unit)
    # Get angle
    phi = np.arccos(np.dot(i_v, unit))
    # Precompute trig values
    rcos = np.cos(phi)
    rsin = np.sin(phi)
    # Compute rotation matrix
    matrix = np.zeros((3, 3))
    matrix[0][0] = rcos + u * u * (1.0 - rcos)
    matrix[1][0] = w * rsin + v * u * (1.0 - rcos)
    matrix[2][0] = -v * rsin + w * u * (1.0 - rcos)
    matrix[0][1] = -w * rsin + u * v * (1.0 - rcos)
    matrix[1][1] = rcos + v * v * (1.0 - rcos)
    matrix[2][1] = u * rsin + w * v * (1.0 - rcos)
    matrix[0][2] = v * rsin + u * w * (1.0 - rcos)
    matrix[1][2] = -u * rsin + v * w * (1.0 - rcos)
    matrix[2][2] = rcos + w * w * (1.0 - rcos)
    return matrix

# Example Vector
origv = np.array([0.47404573,  0.78347482,  0.40180573])

# Compute the rotation matrix
R = get_rotation_matrix(origv)

# Apply the rotation matrix to the vector
newv = np.dot(origv.T, R.T)

# Get the 3D figure
fig = plt.figure()
ax = fig.gca(projection='3d')

# Plot the original and rotated vector
ax.plot(*np.transpose([[0, 0, 0], origv]), label="original vector", color="r")
ax.plot(*np.transpose([[0, 0, 0], newv]), label="rotated vector", color="b")

# Plot some axes for reference
ax.plot([0, 1], [0, 0], [0, 0], color='k')
ax.plot([0, 0], [0, 1], [0, 0], color='k')
ax.plot([0, 0], [0, 0], [0, 1], color='k')

# Show the plot and legend
ax.legend()
plt.show()

I've linked found the method here. Why is the transform this produces always just a little bit off???

Wow... such a simple solution. I'm not sure how I missed that. Thank you so much! — user986122, Apr 19 '17 at 23:40

Eric · Accepted Answer · 2017-10-25T08:29:41.303

3

You need to norm uvw for that to work. So replace

u, v, w = np.cross(i_v, unit)

With

uvw = np.cross(i_v, unit)
uvw /= np.linalg.norm(uvw)

Which is basically the same as the i_v = np.divide(i_v, np.sqrt(np.dot(i_v, i_v))) line you already had.

You can do better though, and avoid trig entirely:

def get_rotation_matrix(i_v, unit=None):
    # From http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q38
    if unit is None:
        unit = [1.0, 0.0, 0.0]
    # Normalize vector length
    i_v /= np.linalg.norm(i_v)

    # Get axis
    uvw = np.cross(i_v, unit)

    # compute trig values - no need to go through arccos and back
    rcos = np.dot(i_v, unit)
    rsin = np.linalg.norm(uvw)

    #normalize and unpack axis
    if not np.isclose(rsin, 0):
        uvw /= rsin
    u, v, w = uvw

    # Compute rotation matrix - re-expressed to show structure
    return (
        rcos * np.eye(3) +
        rsin * np.array([
            [ 0, -w,  v],
            [ w,  0, -u],
            [-v,  u,  0]
        ]) +
        (1.0 - rcos) * uvw[:,None] * uvw[None,:]
    )

That last expression is this equation from the wikipedia page:

edited Oct 25 '17 at 08:29

answered Apr 19 '17 at 23:39

Eric

95,302
53
242
374

This is pretty close to what I had originally in the algorithm, but my uvw was still incorrect I guess - so I ended up with this other solution as I debugged. I'll definitely used the faster solution going forward. Thanks again! – user986122 Apr 19 '17 at 23:43
Note that composing the matrix like this is probably not faster than doing it elementwise - I just did that for clarity. Avoiding `arccos` is worthwhile though, as that and `cos` together will lose precision. – Eric Apr 19 '17 at 23:50
Also, you should skip the normalizing step when `rsin == 0` – Eric Apr 19 '17 at 23:50
1

It looks like the calculation of uvw is incorrect. It should be uvw=np.cross(unit,i_v) not the other way around. What is there now yields the opposite angle. I've been banging my head against the wall working with this, with 3D volume phantoms. With the change I outline it works for me. – kabammi Oct 23 '17 at 21:26
@kabammi: Actually, my mistake is in the transcription of the `[u]_x` matrix, but they're essentially equivalent. Good catch! – Eric Oct 25 '17 at 08:28

Imprecision with rotation matrix to align a vector to an axis

1 Answers1

Linked