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).

Answer 2

The mapping you are looking for seems to be affine transformation. Four 3D points not lying in one plain is the exact number of points needed to recover the affine transformation. The latter is, loosely speaking, multiplication by matrix and adding a vector

secondary_system = A * primary_system + t

The problem is now reduced to finding appropriate matrix A and vector t. I think, this code may help you (sorry for bad codestyle -- I'm mathematician, not programmer)

import numpy as np
# input data
ins = np.array([[3531820.440, 1174966.736, 5162268.086],
                [3531746.800, 1175275.159, 5162241.325],
                [3532510.182, 1174373.785, 5161954.920],
                [3532495.968, 1175507.195, 5161685.049]]) # <- primary system
out = np.array([[6089665.610, 3591595.470, 148.810],
                [6089633.900, 3591912.090, 143.120],
                [6089088.170, 3590826.470, 166.350],
                [6088672.490, 3591914.630, 147.440]]) # <- secondary system
p = np.array([3532412.323, 1175511.432, 5161677.111]) #<- transform this point
# finding transformation
l = len(ins)
entry = lambda r,d: np.linalg.det(np.delete(np.vstack([r, ins.T, np.ones(l)]), d, axis=0))
M = np.array([[(-1)**i * entry(R, i) for R in out.T] for i in range(l+1)])
A, t = np.hsplit(M[1:].T/(-M[0])[:,None], [l-1])
t = np.transpose(t)[0]
# output transformation
print("Affine transformation matrix:\n", A)
print("Affine transformation translation vector:\n", t)
# unittests
print("TESTING:")
for p, P in zip(np.array(ins), np.array(out)):
  image_p = np.dot(A, p) + t
  result = "[OK]" if np.allclose(image_p, P) else "[ERROR]"
  print(p, " mapped to: ", image_p, " ; expected: ", P, result)
# calculate points
print("CALCULATION:")
P = np.dot(A, p) + t
print(p, " mapped to: ", P)

This code demonstrates how to recover affine transformation as matrix + vector and tests that initial points are mapped to where they should. You can test this code with Google colab, so you don't have to install anything.

Regarding theory behind this code: it is based on equation presented in "Beginner's guide to mapping simplexes affinely", matrix recovery is described in section "Recovery of canonical notation" and number of points needed to pinpoint the exact affine transformation is discussed in "How many points do we need?" section. The same authors published "Workbook on mapping simplexes affinely" that contains many practical examples of this kind.