Finding rotation, translation and scale between two noisy 3D point sets [w/ code]

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).

  • Input:
    • 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
  • Output:
    • 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 estimateAffine3D:

horn_example (2)horn_example (1)

Here the code file.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s