I have two 3D point clouds which are identical but misaligned. I'm trying to align them (or 'register' them). The very first well-known algorithm that achieves this is the iterative closest point (ICP) algorithm from the 90s. Most of the new methods refer to this one or extend it in their approaches. So, I felt I should explore it before learning any of the new methods.
The ICP algorithm includes the 'least squares' problem. In the paper I'm just presented with its solution. I'd like to know how we get to the solution and the way is always ignored in the articles I find.
A note since it was asked:
Before getting to the 'least squares' problem, a correspondence between the two sets is estimated. That's why in the problem below we essentially try to minimize the difference between specific pairs of points.
So, here is the problem:
Given $2$ sets of 3D points $M_i,S_i, \space\space i=1,2,\ldots,N$
I want to find a rotation matrix $R$ and translation matrix $T$ such that the following is minimized :
$$D=\sum^{i=N}_{i=1}||S_i-M'_i||=\sum^{i=N}_{i=1}||S_i-RM_i-T||$$
How do I proceed with this? Or where can I study this problem?
With the little math that I know as an engineer (which I'm always trying to improve), here are my ideas :
I know the form of the rotation matrix and that it contains $3$ angles of rotation, one for all axes. The translation vector is a simple $(x,y,z)$ vector with unknown coordinates. In total we have $6$ degrees of freedom.
Since we are looking for a minimum, if we take the derivative with respect to each of the $6$ unknowns, they must all be zero. So, this will give us $6$ equations (containing trigonometric functions) with $6$ unknowns. Of course, there are things that need to be checked to ensure that this is a minimum. However, the $6 \times 6$ system already scares me.
Best Answer
Hint:
If it is like you said in the comment, then translation and rotations can be determined separately.
First you adjust for translation by moving a point onto the corresponding one, or better, to allow for some random discrepancy, by moving barycenter onto barycenter.
This is what in a linear least squares regression you will always get for the additive constant term: average to average.
Then you have to determine a "rigid body" rotation, i.e. three angles, and in case whether or not there are reflections.
Apart from a point to point correspondence, again to allow for some discrepancies, you might consider to align the inertia axes of the two clouds. Should two or three of them be substantially equal, you may use the points correspondence to decide (also for reflections).