ICP with SVD – Hands on

Hi everyone,

since you came across a post with a cryptic title like that, chances are high that you are already deep in the topic. But for the 1% who dont, here is a short background info:

3D-Scan Matching using Iterative Closest Points (ICP)

When you have a 3D-Scanner (like a Laserscanner, Kinect or whatever) and try to scan a room/object the first problem is how to combine to scan-frames to one pointcloud. Usually  the scanner moved between the frames still providing some overlapping to be matched. But how to do that?

Doing some research about that one directly stumbles across the ICP (like here and here). The first problem always is the registration (which point of one frame corresponds to which on the other), but thats not topic of this post.

Once you have the correspondences, the theory is simple: Calculate the transformation using the Singular Value Decomposition (SVD) and you are done. Sounds simple right? Well once you figured out the details and climbed out of all the pitfalls, it actually really is. But since i dont think it to be necessary that everyone goes throu that process, and it seems noone at the universities finds it interessting enough to write about it, here is a walk throu how to actually do it:

Using SVD to match registered Point Clouds

So here we are. Starting with two point clouds A and B with the same size and sorted according to the correspondence. Meaning A[0] corresponds to B[0] etc. There are no further assumptions on the points. We do everything for 3 dimensions, but it basically works in every space you can imagine 😉

And lets assume that we consider A fixed and want to calculate the transformation needed to move B.

Step 1: normalize the points

The SVD calculates only the rotation (around the center) which minimizes the distance between the corresponding points. This means that we first align the two point clouds to have their mean in the center:

list normalizedPoints;

Vector3f mean(0,0,0);
for(auto it= somePoints.begin();it != somePoints.end();++it) {
  mean += *it;
}
mean /= somePoints.size();
for(auto it= somePoints.begin();it != somePoints.end();++it) {
  normalizedPoints.push_back(*it - mean);
}

Make sure to save the means as we will need them later. Now we have set A’ and B’ which will be aligned using the SVD.

Step 2: calc the sum of products of the normalized points
Matrix3f M = Matrix3f::Constant(3, 3, 0);
for(auto it1= normalizedPoints.begin(), it2= normalizedOtherPoints.begin();
 it1 != normalizedPoints.end() && it2 != normalizedOtherPoints.end(); ++it1,++it2) {
  M += (*it1)*(it2->transpose());
}

Now M is the matrix we want the SVD of.

Step 3: calculate the SVD

thanks to the eigen framework this is basically just one line of code. (You may have noticed that i already use eigen-datatypes in the rest of the code).

JacobiSVD svd(M, ComputeFullU | ComputeFullV);
Step 4: calculate Rotationmatrix
Matrix3f resultRotation= (svd.matrixV()*(svd.matrixU().transpose())).transpose();

Now this is a bit tricky. In most of the sources it states that the resulting rotation matrix is calculated as V*U^t. But it turns out that this is rotating B exactly in the wrong direction. So we need the inverse rotation. Lucky us: the transpose of a rotation matrix is the rotation matrix of the inverse rotation. Therefore the final transpose().

Step 5: determine the correct transformation for B

And here comes the step which needed some time to get the head around. R is the rotation matrix, rotating B’ (the normalized B) into A’. So this can’t be used directly to rotate points in B.

The correct transformation for any point is:

Vector3f transformed= R*(point - meanB)+meanA

Which basically means:

  • move the point so that its relative to the mean of B
  • rotate it around the mean of B with rotation R (result of the SVD)
  • move it so that the center is aligned with the mean of A

And thats it. Now all you need to do, is get the point-correspondences right (which honestly is the hardest part).

Feel free to leave a comment and tell me if anyone else is using this stuff. Specially on mobile devices 😉

Leave a Reply

Your email address will not be published. Required fields are marked *