Computing the 3D Transformation between Two Sets of Points

Question

Using a Microsoft Kinect, I am collecting depth data about an object. From these data, I create a "cloud" of points (point cloud), which, when plotted, allow me to view the object that I scanned using the Kinect.

However, I would like to be able to collect multiple point clouds from different "views" and align them. More specifically, I would like to use an algorithm such as Iterative Closest Point (ICP) to do so, transforming each point in my point cloud by calculating the rotation and translation between each cloud that I collect and the previously-collected cloud.

However, while I understand the process behind ICP, I do not understand how I would implement it in 3D. Perhaps it is my lack of mathematical experience or my lack of experience with frameworks such as OpenCV, but I cannot find a solution. I would like to avoid libraries such as the Point Cloud Library which does this sort of thing for me, since I would like to do it myself.

Any and all suggestions are appreciated (if there is a solution that involves OpenCV/python that I can work on, that would be even better!)

Answer 1

I am currently struggling with ICP myself. Here is what I have gathered so far:

ICP consists of three steps:

• Given two point clouds A and B, find pairs of points between A and B that probably represent the same point in space. Often this is done simply by matching each point with its closest neighbor in the other cloud, but you can use additional features such as color, texture or surface normal to improve the matching. Optionally you can then discard the worst matches.
• Given this list of correspondence pairs, find the optimal transformation from A to B
• Apply this transformation to all points in A
• repeat these three steps until you converge on an acceptable solution.

Step one is easy, although there are lots of ways to optimize its speed, since this is the major performance bottleneck of ICP; and to improve the accuracy, since this is the main source of errors. OpenCV can help you there with the FLANN library.

I assume your troubles are with step two, finding the best transformation given a list of correspondences.

One common approach works with Singular Value Decomposition (SVD). Here is a rough sketch of the algorithm. Searching for ICP & SVD will give a lot of further references.

• Take the list of corresponding points A₁..A_n and B₁..B_n from step 1
• calculate the centroid C_a of all points in A and the centroid C_b of all points in B
• Calculate the 3x3 covariance matrix M
  M = (A₁ - C_a)* (B₁ - C_b)^T + ... + (A_n - C_a)* (B_n - C_b)^T
• Use SVD to calculate the 3x3 Matrices U and V for M
  (OpenCV has a function to perform SVD)
• Calculate R = U * V^T.
  This is your desired optimal rotation matrix.
• Calculate the optimal translation as C_b - R*C_a
• The optimal transformation is the combination of R and this translation

Please note that I have not yet implemented this algorithm myself, so I am only paraphrasing what I read.