Mark Mark - 8 days ago 4
Python Question

Calculate rotation matrix to align two vectors in 3D space?

I have two separate vectors of 3D data points that represent curves and I'm plotting these as scatter data in a 3D plot with matplotlib.

Both the vectors start at the origin, and both are of unit length. The curves are similar to each other, however, there is typically a rotation between the two curves (for test purposes, I've actually being using one curve and applying a rotation matrix to it to create the second curve).

I want to align the two curves so that they line up in 3D e.g. rotate curve b, so that its start and end points line up with curve a. I've been trying to do this by subtracting the final point from the first, to get a direction vector representing the straight line from the start to the end of each curve, converting these to unit vectors and then calculating the cross and dot products and using the methodology outlined in this answer (https://math.stackexchange.com/a/476311/357495) to calculate a rotation matrix.

However, when I do this, the calculated rotation matrix is wrong and I'm not sure why?

My code is below (I'm using Python 2.7):

# curve_1, curve_2 are arrays of 3D points, of the same length (both start at the origin)

curve_vec_1 = (curve_1[0] - curve_1[-1]).reshape(3,1)
curve_vec_2 = (curve_2[index][0] - curve_2[index][-1]).reshape(3,1)
a,b = (curve_vec_1/ np.linalg.norm(curve_vec_1)).reshape(3), (curve_vec_2/ np.linalg.norm(curve_vec_2)).reshape(3)
v = np.cross(a,b)
c = np.dot(a,b)
s = np.linalg.norm(v)
I = np.identity(3)
vXStr = '{} {} {}; {} {} {}; {} {} {}'.format(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0)
k = np.matrix(vXStr)
r = I + k + np.square(k) * ((1 -c)/(s**2))

for i in xrange(item.shape[0]):
item[i] = (np.dot(r, item[i]).reshape(3,1)).reshape(3)


In my test case, curve 2 is simply curve 1 with the following rotation matrix applied:

[[1 0 0 ]
[ 0 0.5 0.866]
[ 0 -0.866 0.5 ]]


(just a 60 degree rotation around the x axis).

The rotation matrix computed by my code to align the two vectors again is:

[[ 1. -0.32264329 0.27572962]
[ 0.53984249 1. -0.35320293]
[-0.20753816 0.64292975 1. ]]


The plot of the direction vectors for the two original curves (a and b in blue and green respectively) and the result of b transformed with the computed rotation matrix (red) is below. I'm trying to compute the rotation matrix to align the green vector to the blue.Curve plot

Answer

Problem is here:

r = I + k + np.square(k) * ((1 -c)/(s**2))

np.square(k) squares each element of the matrix. You want np.matmul(k,k) or k @ k which is the matrix multiplied by itself.

I'd also implement the side cases (especially s=0) mentioned in the comments of that answer or you will end up with errors for quite a few cases.