Finding the optimal/best rotation and translation between two sets of corresponding 3D point data, so that they are aligned/registered, is a common problem I come across. An illustration of the problem is shown below for the simplest case of 3 corresponding points (the minimum required points to solve).
The corresponding points have the same colour, R is the rotation and t is the translation. We want to find the best rotation and translation that will align the points in dataset A to dataset B. Here, ‘optimal’ or ‘best’ is in terms of least square errors. This transformation is sometimes called the Euclidean or Rigid transform, because it preserves the shape and size. This is in contrast to an affine transform, which includes scaling and shearing.
This problem arises especially in tasks like 3D point cloud data registration, where the data is obtained from hardware like a 3D laser scanner or the popular Kinect device. The solution I’ll be presenting is from the paper‘A Method for Registration of 3-D Shapes’, by Besl and McKay, 1992.
Solution overview
Finding the optimal rigid transformation matrix can be broken down into the following steps:
- Find the centroids of both dataset
- Bring both dataset to the origin then find the optimal rotation, (matrix R)
- Combine R and centroids into a single 4×4 matrix transformation
Finding the centroids
This bit is easy, the centroids are just the average point and can be calculated as follows:
Here, and
are points in dataset A and B respectively. We will use these values in the next step.
Finding the optimal rotation
There are a few ways of finding optimal rotations between points. The easiest way I found is using Singular Value Decomposition (SVD), because it’s a function that is widely available in many programming languages (Matlab, Octave, C using LAPACK, C++ using OpenCV …). SVD is like this powerful magical wand (Harry Potter ain’t got nothing on the SVD) in linear algebra for solving all sorts of numerical problems, but tragically wasn’t taught when I was at uni. I won’t go into details on how it works but rather how to use it. You only need to know that the SVD will decompose/factorise a matrix (call it E), into 3 other matrices, such that:
If E is a square matrix then U, S and V are the same size as well. We’re only interested in square matrices for this problem so I won’t go into detail about rectangular ones.
To find the optimal rotation we first re-centre both dataset so that both centroids are at the origin, like shown below.
This removes the translation component, leaving on the rotation to deal with. The next step involves accumulating a matrix, called H, and using SVD to find the rotation as follows:
At this point you may be thinking, “what the, that easy?!”, and indeed you would be right. One thing to be careful is that you calculate H correctly. It should end up being a 3×3 matrix, not a 1×1 matrix. Pay close attention to the transpose symbol. It’s doing a multiplication between 2 matrices where the dimensions effectively are, 3×1 and 1×3, respectively. The ordering of the multiplication is also important, doing it the other way will find a rotation from B to A instead.
Combing R and centroids
So we’ve now got matrix R and t. The last and final step is to combine them together to get a single convenient matrix transformation, call it T. This will require TR to be a 4×4 matrix, which means R (3×3) and t (3×1) have to be converted to a 4×4 matrix by padding zeros and ones appropriately. The complete step is outlined below:
That completes the solution.
Note on usage
The solution presented can be used on any size dataset as long as there are at least 3 points. When there are more than 3 points a least square solution is obtained, such that the following error is minimised:
Here, the operator || || is the Euclidean distance between two vectors, a scalar value. r is just the square distance error between the points in the two dataset. I’ve abused the notion a bit (naughty me). P in this case is a 4×1 column vector (instead of the 3×1 matrix defined earlier), with the last value being 1, a homogeneous point.
Code
This script has been tested in Octave. It should work in Matlab but it has not been tested.
Download rigid_transform_3D.m
Read the comments in the file for usage.