A an n x 3 matrix composed by the n known x,y,z coordinates of the original data set.
B an n x 3 composed by the n known x,y,z coordinates of the data set in its final position.
First compute the center of gravities (c.g.) for both data sets by simply computing the mean of their x,y,z coordinates. Each c.g. is a single x,y,z tuple. The -x,-y,-z of them give you both translation matrices TA, and TB, which have this shape:
Now, bring both data sets to the origin (0,0,0) by subtracting the x,y,z’s of A and B by their respective c.g.‘s. The resulting data sets are A’ and B’. Now, let’s find the rotation matrix:
In 3D space your problem can be defined as this equation in matrix form:
A'X = B', where
X is the unknown 3 x 3 transform matrix.
To find the unknown 3x3 transform, you need of course at least three points in your data set. Then, isolate
A’-1A’X = A’-1B’
X = A’-1B’
Transpose A’-1 so it becomes a 3 x n matrix to be multipliable by the
Well, for real-world problems,
A' won’t be square, so you can compute its pseudoinverse by using SVD (single value decomposition) to obtain three matrices:
svd(A’) = UΣV*
The asterisk symbol denotes a conjugate transpose of the matrix. If your values are not complex, then it denotes only the transpose matrix.
The pseudoinverse of
A', termed A’†, can be found by doing:
A’† = VΣ†U*, where:
- V is just the transpose of V* if your x,y,z values are real-valued.
- U* is, likewise, the transpose of U.
- Σ† is the pseudoinverse of Σ. This pseudoinverse can be found by:
a) replacing the non-zero values in its main diagonal by their reciprocals (1/x).
b) transposing the matrix with the reciprocals.
Then do X = A’†B’. Note that X is a matrix that may contain combined transforms. To find the rotation matrix, do an svd on X:
svd(X) = UΣV*
The rotation matrix R is simply R = VU*.
Computaionally, the Eigen library can be used to calculate these svd’s (Eigen: SVD module). Eigen is a header-only library so it’s easy to add to your project. The other matrix operations can be easily done with Eigen itself, VTK, Boost or one of the C++'s standard libraries.
I hope this helps,