how to perform coordinates affine transformation using python?

Question

I would like to perform transformation for this example data set.
There are four known points with coordinates x, y, z in one coordinate[primary_system] system and next four known points with coordinates x, y, h that belong to another coordinate system[secondary_system]. Those points correspond; for example primary_system1 point and secondary_system1 point is exactly the same point but we have it's coordinates in two different coordinate systems. So I have here four pairs of adjustment points and want to transform another point coordinates from primary system to secondary system according to adjustment.

primary_system1 = (3531820.440, 1174966.736, 5162268.086)
primary_system2 = (3531746.800, 1175275.159, 5162241.325)
primary_system3 = (3532510.182, 1174373.785, 5161954.920)
primary_system4 = (3532495.968, 1175507.195, 5161685.049)

secondary_system1 = (6089665.610, 3591595.470, 148.810)
secondary_system2 = (6089633.900, 3591912.090, 143.120)
secondary_system3 = (6089088.170, 3590826.470, 166.350)
secondary_system4 = (6088672.490, 3591914.630, 147.440)

#transform this point
x = 3532412.323 
y = 1175511.432
z = 5161677.111<br>

at the moment I try to average translation for x, y and z axis using each of the four pairs of points like:

#x axis
xt1 =  secondary_system1[0] - primary_system1[0]           
xt2 =  secondary_system2[0] - primary_system2[0]
xt3 =  secondary_system3[0] - primary_system3[0]
xt4 =  secondary_system4[0] - primary_system4[0]

xt = (xt1+xt2+xt3+xt4)/4    #averaging

...and so on for y and z axis

#y axis
yt1 =  secondary_system1[1] - primary_system1[1]           
yt2 =  secondary_system2[1] - primary_system2[1]
yt3 =  secondary_system3[1] - primary_system3[1]
yt4 =  secondary_system4[1] - primary_system4[1]

yt = (yt1+yt2+yt3+yt4)/4    #averaging

#z axis
zt1 =  secondary_system1[2] - primary_system1[2]           
zt2 =  secondary_system2[2] - primary_system2[2]
zt3 =  secondary_system3[2] - primary_system3[2]
zt4 =  secondary_system4[2] - primary_system4[2]

zt = (zt1+zt2+zt3+zt4)/4    #averaging

So above I attempted to calculate average translation vector for every axis

Answer 1

If it is just a translation and rotation, then this is a transformation known as an affine transformation.

It basically takes the form:

secondary_system = A * primary_system + b

where A is a 3x3 matrix (since you're in 3D), and b is a 3x1 translation.

This can equivalently be written

secondary_system_coords2 = A2 * primary_system2,

where

• secondary_system_coords2 is the vector [secondary_system,1],
• primary_system2 is the vector [primary_system,1], and

• A2 is the 4x4 matrix:

  [   A   b ]
  [ 0,0,0,1 ]
  

(See the wiki page for more info).

So basically, you want to solve the equation:

y = A2 x

for A2, where y consist of points from secondary_system with 1 stuck on the end, and x is points from primary_system with 1 stuck on the end, and A2 is a 4x4 matrix.

Now if x was a square matrix we could solve it like:

A2 = y*x^(-1)

But x is 4x1. However, you are lucky and have 4 sets of x with 4 corresponding sets of y, so you can construct an x that is 4x4 like so:

x = [ primary_system1 | primary_system2 | primary_system3 | primary_system4 ]

where each of primary_systemi is a 4x1 column vector. Same with y.

Once you have A2, to transform a point from system1 to system 2 you just do:

transformed = A2 * point_to_transform

You can set this up (e.g. in numpy) like this:

import numpy as np
def solve_affine( p1, p2, p3, p4, s1, s2, s3, s4 ):
    x = np.transpose(np.matrix([p1,p2,p3,p4]))
    y = np.transpose(np.matrix([s1,s2,s3,s4]))
    # add ones on the bottom of x and y
    x = np.vstack((x,[1,1,1,1]))
    y = np.vstack((y,[1,1,1,1]))
    # solve for A2
    A2 = y * x.I
    # return function that takes input x and transforms it
    # don't need to return the 4th row as it is 
    return lambda x: (A2*np.vstack((np.matrix(x).reshape(3,1),1)))[0:3,:]

Then use it like this:

transformFn = solve_affine( primary_system1, primary_system2, 
                            primary_system3, primary_system4,
                            secondary_system1, secondary_system2,
                            secondary_system3, secondary_system4 )

# test: transform primary_system1 and we should get secondary_system1
np.matrix(secondary_system1).T - transformFn( primary_system1 )
# np.linalg.norm of above is 0.02555

# transform another point (x,y,z).
transformed = transformFn((x,y,z))

Note: There is of course numerical error here, and this may not be the best way to solve for the transform (you might be able to do some sort of least squares thing).

Also, the error for converting primary_systemx to secondary_systemx is (for this example) of order 10^(-2).

You'll have to consider whether this is acceptable or not (it does seem large, but it might be acceptable when compared to your input points which are all of order 10^6).