Compute the rotation matrix given two vectors using Rodrigues' formula

In the previous post, we have shown how angular velocities and rotation matrices are linked through the exponential map, or to be specific, the Rodrigues’ rotation formula. In this post I would like to give as an useful application of Rodrigues’ formula, to compute the rotation matrix between two vectors.

Let $a,b\in\mathbb{R}^3$ ($a\neq b$) be two unit vectors expressed in an arbitary coordinate frame. Our goal is to compute the rotation matrix from $a$ to $b$.

We denote by $u$ and $\theta$ be the axis and the angle of rotation, repectively. Since $a$ and $b$ are unitary, by definition the cross product gives the product between $u$ and $\theta$:
$$
a\times b = u\sin\theta,
$$
and the dot product gives the cosinus:
$$
a\cdot b = \cos\theta.
$$

According to the Rodrigues’ formula, the rotation matrix from $a$ to $b$ has a closed form:
$$
R = I + \sin(\theta){u\times} + \left(1-\cos(\theta)\right)(u\times)^2,
$$
which is equal to:

\begin{align} R &= I + \sin(\theta){u\times} + \frac{1-\cos(\theta)}{\sin^2(\theta)}({\sin(\theta)} u\times)^2 \\ &= I + \sin(\theta){u\times} + \frac{1}{1+\cos(\theta)}({\sin(\theta)} u\times)^2 \\ &= I + (a\times b)\times + \frac{1}{1+a\cdot b}\left((a\times b)\times\right)^2. \end{align}

Finally, we can write the following Python code to do the job:

def cross_3d(v):
	'''given 3d vector v this function outputs v_cross matrix'''
	v = np.matrix(v)
	if v.shape != (3,1):
		v = v.T
	v_cross = np.matrix([[0., -v[2,0], v[1,0]],
		[v[2,0], 0., -v[0,0]],
		[-v[1,0], v[0,0], 0.]])
	return v_cross
  
def rotation_matrix(a, b):
	'''compute a rotation matrix from vector a to vector b'''
	a = np.matrix(a)
	b = np.matrix(b)
	if a.shape != (3,1):
		a = a.T
	if b.shape != (3,1):
		b = b.T
	v = cross(a, b) # cross product
	c = a.T*b # dot product, note that the result is a 1x1 matrix
	v_cross = cross_3d(v)
	R = eye(3) + v_cross + v_cross*v_cross*(1/(1+c[0,0]))
	return R