Hello everyone. In this post I present my implementation for computing the RIGID rotation, translation and scale between two 3D point sets (code at the bottom!).
The function uses Horn’s absolute orientation method exploiting a quaternion, as explained in his paper. There are also some unit quaternion notes that are interesting for further understanding the math behind it here. Finally, I got inspiration for this from ‘Matt J’ Matlab implementation, that can be found here.
After trying some porting and cleaning of the code, I decided to give it a shot and wrote fresh new code. The function also computes the eigen values and eigen vector of the constructed quaternion, thus in the file are also included two little function for this: one computes the 4 real eigen values of any 4×4 symmetric matrix, the second computes the eigen vector for a given matrix and eigen value.
The input of the function are TWO set of CORRESPONDING 3D points, there is a version using OpenCV
cv::Point3f structure and one that takes a vector of float (arranged in the x1,y1,z1 x2,y2,z2,… fashion).
src: a vector of 3D points, which must be in float precision
dst: correspoinding 3D points to align the first set to
out: reference to the output data structure
src_to_dst: the src points mapped to fit the dst set
rotation: 3×3 rotation matrix between the two points sets
translation: 3D translation vector between the two points sets
scale: scalar representing the scale between src and dst
error_lsq: least squares error
error_max: maximum error (euclidean distance between corrispondence points)
On a single thread running on a i7-5700HQ @ 2.70GHz, medium execution time is around 0.01 ms. To provide reference, here is the result of the function against the OpenCV function
Here the code file.