Rotation matrix from exponential twist

Last time I described how to derive a geometric derivation of the the famous Rodrigues' rotation formula. While being quite easy to understand there is another way how to get the rotation matrix from the angle-axis representation.  The approach I'm gonna go through today derives the rotation matrix from a constant angular velocity of a rigid body. The goal is to get rotation matrix R from axis of rotation n and angle of rotation \phi I hope I won't spoil your excitement from the discovery if I show you how it's done right now.

    \[R = e^{[n]_\times\phi}\]

The R is 3x3 rotation matrix,  [n]_\times is a skew symmetric matrix of the unit vector representing rotation axis and the \phi is the rotation angle in radians. By the right hand rule the rotation is counter clockwise. The inverse rotation is described by flipping the rotation axis.

Here is how this equation can be derived. Lets define some basic variables first.
\omega Angular velocity in rad/s. It is a pseudovector whose direction represents the axis of rotation and the magnitude specifies the angular speed. The angular speed is a signed quantity which is reflected by the direction of this vector.

p(t) Vector from the axis of rotation to the point P at the time t
p(0) Position of the vector p(t) at the time t=0
R(t) Rotation matrix that moves the vector p(0) to the p(t)
v(t) Instantaneous velocity of the the point P at the time t

Lets start with the idea of rotating the vector v around the \omega axis from it's starting position p(0) to new position p(t) at the time t.

We know that we can rotate the v(0) \rightarrow v(t) by pre-multiplying v(0) by an orthogonal rotation matrix R. This matrix moves the vector v(0) to the new position for time t so we express it as R(t)

(1)   \begin{equation*}  p(t)=R(t)p(0) \end{equation*}

This means R(t) rotates the vector p(0) to the new position p(t).  In the figure above I labelled the angular displacement p(0) \rightarrow p(t) as angle \phi. This means the rotation matrix R(t) corresponds to the rotation by angle \phi around axis \omega. We can imagine that our \omega asix is aligned with the Y axis and the point P is rotating in the X,Z plane.

Now we need to calculate instantaneous velocity of the vector p(t). Why we are doing this will become apparent soon. Instantaneous velocity of the point P' is a perpendicular vector v(t) to the vector p(t) which we get by differentiating the above equation with respect to the time.

    \[\lim_{t\to0}v(t)=R(t)p(0) \Rightarrow v(t) = \frac{dR(t)}{dt}p(0) \]

Note p(0) is a constant.  This is all nice and well but still quite far from our goal which is to calculate the rotation matrix from the axis and angle of rotation. Ok lets carry on and see whether we can do something about it.

We know the instantaneous velocity of the vector v(t) can be also calculated as a cross product of the angular velocity and the rotating point.

    \[v(t)=\omega \times p(t)\]

And now we have something interesting, two equations that calculate the instantaneous velocity of the same vector. Lets substitute v(t)=\omega \times p(t) to the v(t) = \frac{dR(t)}{dt}p(0)

    \[\omega \times p(t) =\frac{dR(t)}{dt}p(0)\]

From the equation (1)  p(t)=R(t)p(0)  we can substitute the p(t) with R(t)p(0)

    \[\omega \times R(t)p(0) =\frac{dR(t)}{dt}p(0) \Rightarrow \omega \times R(t) =\frac{dR(t)}{dt}\]

To simplify the calculation we convert the cross product to multiplication with skew symmetric matrix

    \[[\omega]_\times R(t) =\frac{dR(t)}{dt}\]

Where the [\omega]_\times is the skew-symmetric matrix of the \omega

    \[[\omega]_\times=\begin{pmatrix}0 & -\omega_3 & \omega_2\\\omega_3 & 0 & -\omega_1\\-\omega_2 & \omega_1 & 0\end{pmatrix}\]

Now we have to solve this differential equation. Let's rearange terms into the standard form and see how we can solve it.

    \[\frac{dR(t)}{dt}-[\omega]_\times R(t)=0\]

We can see it is a first order, linear, time invariant equation which has a known solution e^x I'm gonna be more explicit here and show how to solve this equation by the homogeneous and particular solution.

This technique is based on finding two solutions, the particular and homogeneous. The result is then the sum of those two.

    \[x(t) = x_p(t) + x_h(t)\]

The particular solution x_p(t) is found by guessing a constant variable that would satisfy the equation. Since derivation of constant is zero the differential term drops out. In our case the particular solution R_p is zero.

    \[-[\omega]_\times R_p=0\]

    \[R_p=0\]

Homogeneous solutions R_h is found by replacing the right side of the differential equation with zero (removing the driving force) and guessing what solutions would satisfy this equation. In our case the equation already equals zero. As the solution we guess R_h(t)=e^{xt} The reason for this guess is that the derivative of the \frac{de^{xt}}{dt} = xe^{xt} which nicely removes the derivation without any mathematical complications.

    \[xe^{xt} - [\omega]_\times e^{xt}=0\]

    \[x - [\omega]_\times =0\]

    \[x=[\omega]_\times\]

Thus

    \[R_h(t) = e^{[\omega]_\times t}\]

Since the particular solution is zero the complete solution is only the homogeneous one.

    \[R(t) = R_h(t) = e^{[\omega]_\times t}\]

What we need to do now is to get rid of the time parameter somehow and relate the angular velocity \omega to the rotation angle \alpha While this may look like a complicated task in reality it is quite simple. Since we can choose any time interval we want we chose t=1 sec  Now the angular speed (the magnitude of the angular velocity |\omega|) is  \delta{angle}/\delta{time} so in one second t=1 the angle displacement will be just  \phi = \omega.

    \[R = e^{[\omega]_\times}\]

And there you go we can calculate the rotation matrix from a given rotation axis and the angle of rotation. The direction of the \omega vector is the axis of rotation and the magnitude is the angle of rotation. We can split it into the angle \phi and axis n  components.

For unit vector |n|=1 representing the axis of rotation we get

    \[[\omega]_\times=[n]_\times \phi\]

Thus

    \[R = e^{[n]_\times\phi}\]

We could stop here and be happy with this formula. In the Octave we can get the rotation matrix R from angle axis straight this way.

p = pi/3;  % Rotation angle in anti-clockwise direction
n = [1; 2; 3]; % Axis of rotation

% Normalization of the rotation axis vector
n_norm = n/norm(n);

% Create the skew-symmetric matrix
n_skew = [         0  -n_norm(3)   n_norm(2);
           n_norm(3)           0  -n_norm(1);
          -n_norm(2)   n_norm(1)           0];

% Calculate the rotation matrix
R = expm(n_skew*p)

Or in the Python with SciPy library

from numpy import *
from scipy.linalg import *

# Rotation axis
n = mat('[1. 2. 3.]');

# Normalize the rotation axis
n = n/numpy.linalg.norm(n)

# Build skew-symmetric matrix
n_skew = mat([[0, -n[:,2], n[:,1]],
              [n[:,2], 0, -n[:,0]],
              [-n[:,1], n[:,0], 0]])

#Angle of rotation
alpha = math.pi / 3.

R = expm3(n_skew*alpha)

The Octave and SciPy in Python nicely hide the calculation of the matrix exponential which is great but in many practical applications we need to do it on our own, many times even with very limited resources. Next time I will dive into how to calculate the matrix exponential and what everything in under the bonnet.

This entry was posted in Computer Vision. Bookmark the permalink.

Leave a Reply