Find rigid transform from coordinates of the same points in different reference frames Given two* 3-dimensional points $p_1$ and $p_2$ expressed in different reference frames $A$ and $B$, find the rigid transform (rotation and translation) between frames $A$ and $B$.
The answer to this question is helpful but only covers the case where $A$ and $B$ have the same origin:
Finding a Rotation Transformation from two Coordinate Frames in 3-Space
My idea to use the SVD approach from the above answer with homogeneous coordinates. The hope is to extract the translation from the 4th column of the result, but I'm unsure if this will work as expected. Can anyone convince me that it will or won't work, or suggest a different approach?
*This problem is a part of a computer vision program in a robotics application. I'm trying to find the transform between a 3D camera and the origin of the robot. I can extract up to 4 distinct points in each frame. For Wahba's problem I only need two, but I suspect that to find the rigid transform I'll need 3.
 A: I'd say this won't work. The way I read it, this SVD-based approach makes use of the fact that the true rotation matrix is orthogonal with unit singular values. The SVD of the approximate result will yield two orthogonal matrices and a diagonal matrix whose elements should be almost one. Setting them to exactly one will yield the result. But you use homogenous coordinates, the true transform won't have singular values which are all one.
Looking for alternatives, I guess I'd try separating the translation and the rotation. You could treat the barycenter of your points as the center of the object, and consider its translation. Translate it to the origin, perform the rotation using SVD there, then translate to the target position. Not sure whether you get better results from an integrated technique, but I guess that very much depends on your error cost function which you haven't specified.
As for the number of points: translation has three real degrees of freedom, and so has rotation. Two arbitrary points in space gives six real degrees of freedom, but if their distance is fixed, you only have five left. So two points cannot be enough to fully determine the transformation, and you should use a third point, and if you already have a fourth, I'd plug that in as well just to make things more robust and get a better idea of how large the errors are.
