How To Multiply Two Quaternions Together Using Python

Here is how we multiply two quaternions together using Python. Multiplying two quaternions together has the effect of performing one rotation around an axis and then performing another rotation about around an axis.

import numpy as np
import random

def quaternion_multiply(Q0,Q1):
    """
    Multiplies two quaternions.

    Input
    :param Q0: A 4 element array containing the first quaternion (q01,q11,q21,q31) 
    :param Q1: A 4 element array containing the second quaternion (q02,q12,q22,q32) 

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33) 

    """
    # Extract the values from Q0
    w0 = Q0[0]
    x0 = Q0[1]
    y0 = Q0[2]
    z0 = Q0[3]
	
    # Extract the values from Q1
    w1 = Q1[0]
    x1 = Q1[1]
    y1 = Q1[2]
    z1 = Q1[3]
	
    # Computer the product of the two quaternions, term by term
    Q0Q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    Q0Q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    Q0Q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    Q0Q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1
	
    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([Q0Q1_w, Q0Q1_x, Q0Q1_y, Q0Q1_z])
	
    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32) 
    return final_quaternion
    
Q0 = np.random.rand(4) # First quaternion
Q1 = np.random.rand(4) # Second quaternion
Q = quaternion_multiply(Q0, Q1)
print("{0} x {1} = {2}".format(Q0, Q1, Q))

How to Convert a Quaternion to a Rotation Matrix

In this tutorial, I’ll show you how to convert a quaternion to a three-dimensional rotation matrix. At the end of this post, I have provided the Python code to perform the conversion.

What is a Quaternion?

A quaternion is one of several mathematical ways to represent the orientation and rotation of an object in three dimensions. Another way is to use Euler angle-based rotation matrices like I did on this post and this post (i.e. roll, pitch, and yaw), as well as the cover image of this tutorial.

Quaternions are often used instead of Euler angle rotation matrices because “compared to rotation matrices they are more compact, more numerically stable, and more efficient” (Source: Wikipedia).

Note that a quaternion describes just the rotation of a coordinate frame (i.e. some object in 3D space) about an arbitrary axis, but it doesn’t tell you anything about that object’s position.

The Use of Quaternions in Robotics

Quaternions are the default method of representing orientations and rotations in ROS, the most popular platform for robotics software development.

In robotics, we are always trying to rotate stuff. For example, we might observe an object in a camera. In order to get a robotic arm to grab the object, we need to rotate the camera reference frame to the robot reference frame so that the robot “knows” the location of the object in its own coordinate frame.

Once the rotation from camera pixel coordinates to robot base frame coordinates is complete, the robotic arm can then move its motors to the appropriate angles to pick up the object.

How to Represent Quaternions

Quaternions are an extension of complex numbers. However instead of two values (e.g. a + bi or x + yi…same thing) that represent a point (or vector), we have four values (a, b, c, d):

q = a + bi + cj + dk
complex_numbers
Visualizing a point (a, b) as a complex number on a two-dimensional Argand diagram. Source: Wikipedia

The four values in a quaternion consist of one scalar and a 3-element unit vector.

Instead of a, b, c, and d, you will commonly see:

q = w + xi + yj + zk or q = q0 + q1i + q2j + q3k
  • q0 is a scalar value that represents an angle of rotation
  • q1, q2, and q3 correspond to an axis of rotation about which the angle of rotation is performed.

Other ways you can write a quaternion are as follows:

  • q = (q0, q1, q2, q3)
  • q = (q0, q) = q0 + q

The cool thing about quaternions is they work just like complex numbers. In two dimensions, you can rotate a vector using complex number multiplication. You can do the same with quaternions. The math is more complicated with four terms instead of two, but the principle is the same.

Let’s take a look at a two-dimensional example of complex number multiplication so that you can understand the concept of multiplying imaginary (complex) numbers to rotate a vector. Quaternions add a couple more variables to extend this concept to represent rotation in the 3D space.

2D Example

Suppose we have a vector on a 2D plane with the following specifications:

(x = 3, y = 1)

This vector can be represented in complex numbers as:

3 + i  (e.g. using the x +yi form of complex numbers)

Let’s rotate this vector 45 degrees (which is π/4 in radians).

To rotate 45 degrees, we multiply the number by:

cos(π/4) + sin(π/4)i (De Moivre’s formula)

So, we have sqrt means (“take the square root of”):

(1/sqrt(2)+ i/sqrt(2)) * (3 + i) = sqrt(2) + 2sqrt(2)i 

And since:

sqrt(2) = 1.414

our new vector is:

(x = 1.414, y = 4.242)

As I mentioned earlier, the math for multiplying real quaternions together is more complex than this, but the principle is the same. Multiply an orientation (represented as a quaternion) by a rotation (represented as a quaternion) to get the new orientation.

Convert a Quaternion to a Rotation Matrix

Given a quaternion, you can find the corresponding three dimensional rotation matrix using the following formula.

quaternion-to-rotation-matrix
Source: Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality by J. B. Kuipers (Chapter 5,  Section 5.14 “Quaternions to Matrices”, pg. 125)

Python Code

In Python code, we have:

import numpy as np

def quaternion_rotation_matrix(Q):
    """
    Covert a quaternion into a full three-dimensional rotation matrix.

    Input
    :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 

    Output
    :return: A 3x3 element matrix representing the full 3D rotation matrix. 
             This rotation matrix converts a point in the local reference 
             frame to a point in the global reference frame.
    """
    # Extract the values from Q
    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]
	
    # First row of the rotation matrix
    r00 = 2 * (q0 * q0 + q1 * q1) - 1
    r01 = 2 * (q1 * q2 - q0 * q3)
    r02 = 2 * (q1 * q3 + q0 * q2)
	
    # Second row of the rotation matrix
    r10 = 2 * (q1 * q2 + q0 * q3)
    r11 = 2 * (q0 * q0 + q2 * q2) - 1
    r12 = 2 * (q2 * q3 - q0 * q1)
	
    # Third row of the rotation matrix
    r20 = 2 * (q1 * q3 - q0 * q2)
    r21 = 2 * (q2 * q3 + q0 * q1)
    r22 = 2 * (q0 * q0 + q3 * q3) - 1
	
    # 3x3 rotation matrix
    rot_matrix = np.array([[r00, r01, r02],
                           [r10, r11, r12],
                           [r20, r21, r22]])
						   
    return rot_matrix

How to Add an External C++ Library to Your Project

Libraries in C++ are collections of code that someone else wrote. They prevent you from having to reinvent the wheel when you need to implement some desired functionality. You can think of libraries as a plugin or add-on that gives you more functionality.

For example, I wanted to write a program that is able to multiply two matrices together. Instead of writing the code from scratch, I searched the web for a linear algebra library in C++ that contained functionality for multiplying matrices. I found the library named “Eigen.” I added the library to my project in the CodeLite IDE, and I was ready to go.

Without further ado, here is the step-by-step process for adding an external C++ library to your project using the CodeLite IDE and Visual Studio IDE.

Note that this process will be different if you are using another IDE for C++, but the two basic steps are the same for all IDEs:

  1. Add the path for the header files
  2. Add the path for the actual code (i.e. the library)

How to Add an External C++ Library to Your Project Using the CodeLite IDE

Step 1: Go to the website of the library.
For example, for the linear algebra library, Eigen, you go to this page: Eigen Main Page

Step 2: Download the zip file that contains all the code.

Step 3: Unzip the zip file to your computer.

Step 4: Open CodeLite (i.e. your IDE)

Step 5: Open a new Project

Step 6: Right click on project name and click on Settings

Step 7: Click the Compiler tab and add the Include Paths:
e.g. the folder that contains the folder named ‘Eigen’…C:\XYZ\eigen-eigen-21301928\
This is where the compiler can find the .h (i.e. header) files

Step 8: Click Linker and add the Libraries Search Path
e.g. C:\XYZ\eigen-eigen-21301928\
The path above needs to be the location where the linker can find the libraries (usually suffixed with .a, .dll, .lib, .so)

  • Static Libraries are – XYZ.lib for Windows, UNIX/Linux/Max – libXYZ.a
  • Dynamic Libraries are – XYZ.dll for Windows, Unix/Linux/Mac – libXYZ.so

Step 9: Go to main.cc (i.e. your source code file…could also be main.cpp) and add the preprocessor directives at the top of the source file. You can use this code to test that everything is setup properly.

#include <iostream>
#include <Eigen/Dense>

using Eigen::MatrixXd;

int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << m << std::endl;
}

Step 10: That’s it. You are ready to rock and roll!

How to Add an External C++ Library to Your Project Using the Visual Studio IDE

Step 1: Go to the website of the library.
For example, for the linear algebra library, Eigen, you go to this page: Eigen Main Page

Step 2: Download the zip file that contains all the code.

1-zip_fileJPG

Step 3: Unzip the zip file to your computer.

2-unzipJPG

Step 4: Move the folder to some directory on your computer.

I’ll move it to the C:\libraries directory. At this stage, Eigen is along the following path for me:

C:\libraries\eigen-3.3.7\eigen-3.3.7\Eigen

3-directoryJPG

Step 5: Open Visual Studio.

Step 6: Create a new C++ project.

Step 7: Write the following code into a new Source file.

I’ll name this file main.cpp.

#include <iostream>
#include <Eigen/Dense>
 
using Eigen::MatrixXd;
 
int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << m << std::endl;
}

Step 8: Click on the Project tab at the top and go to Additional Include Directories.

Project -> Properties -> C/C++ -> General (you can find it by expanding the little triangle next to C/C++) -> Additional Include Directories

4-additional-includeJPG

Step 9: Click the blank next to “Additional Include Directories”.
A small triangle dropdown should appear.

Step 10: Click <…Edit>.

Step 11: Click the Add New Line icon.

5-add-new-lineJPG

Step 12: Add the path to the folder containing Eigen

If you click the blank, an icon with three periods “…” should appear. Click that icon and add the path to the folder that contains Eigen.

In my case, the path is:

C:\libraries\eigen-3.3.7\eigen-3.3.7\

because the actual Eigen library is this folder:

C:\libraries\eigen-3.3.7\eigen-3.3.7\Eigen

Step 13: Click OK.

6-after-includeJPG

Step 14: Click Apply and then OK.

7-apply-then-okJPG

Step 15: Go to main.cpp (or whatever the name of the source code you wrote earlier is).

8-write-the-codeJPG

Step 16: Compile and Run the Code.

To compile the code, go to Build -> Build Solution.

Then to run the code, go to Debug -> Start Without Debugging.

Here is the output you should see:

9-outputJPG

That’s it! You’re all good to go.