Coordinate transformation based on three points

Question

Consider two coordinate system $C_1$ and $C_2$. There are three points $p^1_1 $, $p^1_2$, and $p^1_3$ in coordinate $C_1$, where these points have $p^2_1$, $p^2_2$ and $p^2_3$ values in coordinate $C_2$. (note: $p^i_j \in R^3$)

How can I find the transformation $T$ (in the form of $T=[R\hspace{0.2cm} t;0\hspace{0.2cm} 0\hspace{0.2cm} 0\hspace{0.2cm} 1]$ where $R$ is rotation matrix and $t$ is the translation vector) between these two coordinates such that for an arbitrary point in coordinate one like $p^1_4$, I can find it's value in coordinate two $p^2_4$ using $p^2_4 = T * P^1_4$.

Note1: I have used (Iterative Closest Point) ICP two find transformation between these two set of points. The computed transformation is correct for $p^2_1 = T * P^1_1$, $p^2_2 = T * P^1_2$ and $p^2_3 = T * P^1_3$. However for an arbitrary point $P^1_4$ the relation doesn't hold i.e $P^2_4 \neq T*P^1_4$.

Note 2: My points are as follows: $P^1_1=[51.2,-206.6,1894.3]; P^1_2=[51.4, -157.6, 1893.6]; P^1_3=[-48.8, -206.6, 1894.2];$ $P^2_1=[-112.087, 181.788, 989]; P^2_2=[-115.62, 230.3, 987]; P^2_3=[-215.696, 185.55, 989];$

I expect to obtain transformation that the rotation part is close to identity matrix and it's just translation.

Answer 1

You cannot find "the transformation between the coordinates" because no unique transformation exists.

Example: in 3-space, let $p^1_1 = (0,0,0), p^1_1 = (1,0,0), p^1_2 = (0,1,0)$. Let $p^2_i = p^1_i$ for $i = 1 , 2, 3$.

Then the identity map takes $p^1_i$ to $p^2_i$ for every $i$. But so does the map defined by

$$T(x,y,z) = (x, y, 17z)$$

because your coordinate frames tell us nothing about the $z$ coordinate.

This phenomenon isn't a fluke: an affine coordinate system with only three points doesn't suffice to generate coordinates on all of 3-space --- only on a 2D subspace. So when you find a mapping that takes one of your coordinate frames to the other, points outside the 2D subspace of the domain that the coordinates apply to can be sent anywhere. You need affine coordinate frames with four points (no three of which lie in a single plane); then uniqueness is easy.

Answer 2

For a 3D coordinate system: $$\left(\begin{array}{c}p^2_{1x} \\p^2_{1y} \\p^2_{1z} \end{array}\right)=T\left(\begin{array}{c}p^2_{1x} \\p^2_{1y} \\p^2_{1z} \end{array}\right)$$ and similar for $p_2$ and $p_3$. We can write this in a matrix form: $$\left(\begin{array}{ccc}p^2_{1x} &p^2_{2x} &p^2_{3x}\\p^2_{1y}&p^2_{2y} &p^2_{3y}\\p^2_{1z} &p^2_{2z}&p^2_{3z}\end{array}\right)=T\left(\begin{array}{ccc}p^1_{1x} &p^1_{2x} &p^1_{3x}\\p^1_{1y}&p^1_{2y} &p^1_{3y}\\p^1_{1z} &p^1_{2z}& p^1_{3z}\end{array}\right)$$ Then the matrix $T$ is given by $$T=\left(\begin{array}{ccc}p^2_{1x} &p^2_{2x} &p^2_{3x}\\p^2_{1y}&p^2_{2y} &p^2_{3y}\\p^2_{1z} &p^2_{2z}&p^2_{3z}\end{array}\right)\left(\begin{array}{ccc}p^1_{1x} &p^1_{2x} &p^1_{3x}\\p^1_{1y}&p^1_{2y} &p^1_{3y}\\p^1_{1z} &p^1_{2z}& p^1_{3z}\end{array}\right)^{-1}$$ The last matrix is invertible if the points are not collinear

Answer 3

I took this as a problem of inferring a 3D transform compositing a coordinate translation, coordinate rotation, and coordinate untranslation sequence:

p' = T p = T2 (R (T1 p))

If the 3 reference points form a non-degenerate triangle, the transform is solvable.

Reference points comprising a 3D triangle cannot all be collinear, which provides enough 3D information to pin down a unique rotation.

I would break the solution up into two steps.

To solve for the coordinate translations T1 and T2, compute the vector mean of the 3 vertices in both coordinates. Coordinate translate T1 puts the 3 input points into local coordinates about their centroid. After applying the (still unknown) coordinate rotator R, the T2 translate adds back the centroid of the 3 output points. Why? Coordinate rotation must be inferred in a space where each point and its transformed point are the exact same distance from the origin -- putting each triangle into (centroid-based) local coordinates does exactly that.

To solve for the coordinate rotation, coordinate translate both sets of points by their respective centroids. This leaves each set of points splayed about the origin, the only difference being a coordinate rotation.

Looking at your "before" points, choose any 2 points, and calculate direction vectors by normalizing them. Pick the corresponding "after" points, and then normalize.

These 4 direction vectors form two same-sized "directional wedges", the "before wedge" and the "after wedge" that uniquely specify a 3D coordinate rotation which maps the before onto the after.

For details of this "rotational inference", see:

Can 3D rotation be inferred from its effect on directional observations?

But, be mindful of how your "transform inference" technique fails. An example would be a reference triangle that is extremely squat, or extremely tall. The 3 vertices become nearly collinear in these cases, and the rotational inference will become ill-conditioned.

Sample data points No Good

The sample data points given in the problem statement don't form a rigid figure that has the same shape in both input and output spaces. You can check this by computing distances between points. They must match before and after the transform, as distances are preserved under the T = T1*R*T2 transform being considered.

Here are some well-conditioned data points.

     p1 = (51.2, -206.6, 94.3);

     p2 = (100, -157.6, -13.555);

     p3 = (-48.8, 26.99, 4.2);

     p1_p = (-311.0492198800183 53.73313089867574 -202.3654666666667)

     p2_p = (-192.66730901522448 20.297951787113902 -238.18379999999996)

     p3_p = (-125.99670619809075 15.845195087919318 -10.00546666666667)

See if you can infer the T1 * R * T2 transform components.