We Keep Coding

How to find where two lines intersect using numpy.linalg.solve given points coordinates?

So I'm trying to use numpy.linalg.solve() to find where two lines intersect with each other using only some endpoints coordinates. If the coordinates of one lines are: (x1, y1), (x2, y2). I tried:
import numpy as np
a = np.array([[y2-y1],[x1-x2]])
b = np.array([(x1*y2)-(y1*x2)])
np.linalg.solve(a,b)
However I don't think the equations are correct and it is returning the following error:
numpy.linalg.LinAlgError: Last 2 dimensions of the array must be square
so I'm not really sure what to do, can someone help me with this?

Following these answers which gives clear explanations about the equations behind this problem and its well-known analytical resolution (based on Cramer's rule and determinants), it is possible to construct a simple linear system A x = b in order to use np.linalg.solve as requested:
import numpy as np
# Given these endpoints coordinates
# Line 1 passing through points p1 (x1,y1) and p2 (x2,y2)
p1 = [0, 0]
p2 = [1, 1]
# Line 2 passing through points p3 (x3,y3) and p4 (x4,y4)
p3 = [0, 1]
p4 = [1, 0]
# Line 1 dy, dx and determinant
a11 = (p1[1] - p2[1])
a12 = (p2[0] - p1[0])
b1 = (p1[0]*p2[1] - p2[0]*p1[1])
# Line 2 dy, dx and determinant
a21 = (p3[1] - p4[1])
a22 = (p4[0] - p3[0])
b2 = (p3[0]*p4[1] - p4[0]*p3[1])
# Construction of the linear system
# coefficient matrix
A = np.array([[a11, a12],
[a21, a22]])
# right hand side vector
b = -np.array([b1,
b2])
# solve
try:
intersection_point = np.linalg.solve(A,b)
print('Intersection point detected at:', intersection_point)
except np.linalg.LinAlgError:
print('No single intersection point detected')
which gives the intended output for those given points:
>>> Intersection point detected at: [0.5 0.5]

How to convert a 2D point on defined plane in planar uv coordinates back to 3D xyz coordinates?

I have a defined plane (one which happens to be orthogonal to the vector defined by two xyz points in 3D space). I can project any xyz point onto the plane and represent that projection uv coordinate space. I would like to take an arbitrary point in uv coordinate space and find out what its coordinates are in xyz space.
a = x2 - x1
b = y2 - y1
c = z2 - z1
d = -1*(a*x1 + b*y1 + c*z1)
magnitude = (a**2 + b**2 + c**2)**.5
u_magnitude = (b**2 + a**2)**.5
normal = [a/magnitude, b/magnitude, c/magnitude]
u = [b/u_magnitude, -a/u_magnitude, 0]
v = np.cross(normal, u)
p_u = np.dot(u,[x1,y1,z1])
p_v = np.dot(v,[x1,y1,z1])
This code I believe accurately produces the plane I want and will assign the x1,y1,z1 point in uv coordinates to p_u,p_v. My sense is that I have everything I need to do the reverse operation, but I don't know how. If I have a point u0,v0 how can I find x0,y0,z0 that describes its location in 3D space?

From the definition in the text (not reading the code), the problem is not well defined - as there is an infinite number of planes orthogonal to a given vector (think of all the options as planes at different "offsets" along the line from the first point to the second). What you need is first to pick some point through which the plane has to go.
Secondly, when we convert a (U, V) pair to 3D point, I assume you mean a 3D point on the plane.
Trying to be more concrete though, here is your code, with documentation on how I understand it, and how to do the reverse:
# ### The original computation of the plane equation ###
# Given points p1 and p2, the vector through them is W = (p2 - p1)
# We want the plane equation Ax + By + Cz + d = 0, and to make
# the plane prepandicular to the vector, we set (A, B, C) = W
p1 = np.array([x1, y1, z1])
p2 = np.array([x2, y2, z2])
A, B, C = W = p2 - p1
# Now we can solve D in the plane equation. This solution assumes that
# the plane goes through p1.
D = -1 * np.dot(W, p1)
# ### Normalizing W ###
magnitude = np.linalg.norm(W)
normal = W / magnitude
# Now that we have the plane, we want to define
# three things:
# 1. The reference point in the plane (the "origin"). Given the
# above computation of D, that is p1.
# 2. The vectors U and V that are prepandicular to W
# (and therefore spanning the plane)
# We take a vector U that we know that is perpendicular to
# W, but we also need to make sure it's not zero.
if A != 0:
u_not_normalized = np.array([B, -A, 0])
else:
# If A is 0, then either B or C have to be nonzero
u_not_normalized = np.array([0, B, -C])
u_magnitude = np.linalg.norm(u_not_normalized)
# ### Normalizing W ###
U = u_not_normalized / u_magnitude
V = np.cross(normal, U)
# Now, for a point p3 = (x3, y3, z3) it's (u, v) coordinates would be
# computed relative to our reference point (p1)
p3 = np.array([x3, y3, z3])
p3_u = np.dot(U, p3 - p1)
p3_v = np.dot(V, p3 - p1)
# And to convert the point back to 3D, we just use the same reference point
# and multiply U and V by the coordinates
p3_again = p1 + p3_u * U + p3_v * V

Understanding opencv's decomposeHomographyMat outputs

I'm trying to find the angle required to move my camera so it's directly in front of an object. If my camera is looking at the object at a 30 degree angle from the left, then my script should return 30 degrees. I'm using cv2.decomposeHomographyMat to find a rotation matrix which works fine. There are 4 solutions returned from this function, so in my script I am outputting 4 angles. Of these angles, there are only two unique angles. My problem is I don't know which of these two angles is correct.
I know the decomposeHomographyMat returns four possible solutions, but shouldn't the angles be the same? I also found the coordinates of my points projected on a 2D plane, but I wasn't sure what to do with this information in regards to finding which angle is correct (here pts3D are the 2D points of the object taken from the camera image with a 0 added for the z column making it 3D pts):
for i in range(len(Ts)):
projectedPts = cv2.projectPoints(pts3D, Rs[i], Ts[i], CAM_MATRIX, DIST_COEFFS)[0][:,0,:]
Here is a snippet from my code. Maybe I am incorrectly determining the angles from the rotation matrix? In my example below, y1 and y2 will be the same angle, and y3 and y4 will be the same angle. Can someone help explain how I determine which angle is the correct angle, and why there are two different angles returned?
def rotationMatrixToEulerAngles(R):
sy = math.sqrt(Rs[0][0] * R[0][0] + R[1][0] * R[1][0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2][1] , R[2][2])
y = math.atan2(-R[2][0], sy)
z = math.atan2(R[1][0], R[0][0])
else :
x = math.atan2(-R[1][2], R[1][1])
y = math.atan2(-R[2][0], sy)
z = 0
return np.rad2deg(y)
H, status = cv2.findHomography(corners, REFPOINTS)
output = cv2.warpPerspective(frame, H, (800,800))
# Rs returns 4 matricies, we use the first one
_, Rs, Ts, Ns = cv2.decomposeHomographyMat(H, CAM_MATRIX)
y1 = rotationMatrixToEulerAngles(Rs[0])
y2 = rotationMatrixToEulerAngles(Rs[1])
y3 = rotationMatrixToEulerAngles(Rs[2])
y4 = rotationMatrixToEulerAngles(Rs[3])
Thanks!

How to compute which way data points continue beyond an intersection?

Let's say you have two arrays of data values from a calculation, that you can model with a continuos, differentiable function each. Both "lines" of data points intersect at (at least) one point and now the question is whether the functions behind these datasets are actually crossing or anticrossing.
The image below shows the situation, where I know (from the physics behind it) that at the upper two "contact points" the yellow and green lines actually should "switch color", whereas at the lower one both functions go out of each others way:
To give an easier "toy set" of data, take this code for example:
import matplotlib.pyplot as plt
import numpy as np
x=np.arange(-10,10,.5)
y1=[np.absolute(i**3)+100*np.absolute(i) for i in x]
y2=[-np.absolute(i**3)-100*np.absolute(i) for i in x][::-1]
plt.scatter(x,y1)
plt.scatter(x,y2,color='r')
plt.show()
Which should produce the following image:
Now how could I extrapolate whether the trend behind the data is crossing (so the data from the lower left continues to the upper right) or anti-crossing (as indicated with the colors above, the data from the lower left continues to the lower right)?
So far I was able to find the "contact point" between these to datasets by looking at the derivative of the Difference between them, roughly like this:
closePoints=np.where(np.diff(np.diff(array_A - array_B) > 0))[0] + 1
(which probably would be faster to evaluate with something like scipy's cKDTree).
Should I go on and (probably very inefficiently) check the derivative on both sides of the intersection? Or can I somehow check if the extrapolation of the data on the left side fits better to crossing or anticrossing?

I understood your problem as:
You have two sequences of points in a 2D plane.
The true curves can be approximated by straight lines between consecutive points of the sequences.
You want to know how often and where the two curves intersect (not only come into contact but really cross each other) (polygon intersection).
A potential solution is:
You look at each combination of a line segment of one curve with a line segment of another curve.
Combinations where the bounding boxes of the line segments have an overlap can potentially contain intersection points.
You solve a linear equation system to compute if and where an intersection between two lines occurs
In case of no solution to the equation system the lines are parallel but not overlapping, dismiss this case
In case of one solution check that it is truly within the segments, if so record this crossing point
In case of infinitely many intersections the lines are identical. This is also no real crossing and can be dismissed.
Do this for all combinations of line segments and eliminate twin cases, i.e. where the two curves intersect at a segment start or end
Let me give some details:
How to check if two bounding-boxes (rectangles) of the segments overlap so that the segments potentially can intersect?
The minimal x/y value of one rectangle must be smaller than the maximal x/y value of the other. This must hold for both.
If you have two segments how do you solve for intersection points?
Let's say segment A has two points (x1, y1) and (x2, y2) and segment B has two points (x2, y3) and (x4, y4).
Then you simply have two parametrized line equations which have to be set equal:
(x1, y1) + t * (x2 - x1, y2 - y1) = (x3, y3) + q * (x4 - x3, y4 - y3)
And you need to find all solutions where t or q in [0, 1). The corresponding linear equation system may be rank deficient or not solvable at all, best is to use a general solver (I chose numpy.linalg.lstsq) that does everything in one go.
Curves sharing a common point
Surprisingly difficult are cases where one point is common in the segmentation of both curves. The difficulty lies then in the correct decision of real intersection vs. contact points. The solution is to compute the angle of both adjacent segments of both curves (gives 4 angles) around the common point and look at the order of the angles. If both curves come alternating when going around the equal point then it's an intersection, otherwise it isn't.
And a code example based on your data:
import math
import matplotlib.pyplot as plt
import numpy as np
def intersect_curves(x1, y1, x2, y2):
"""
x1, y1 data vector for curve 1
x2, y2 data vector for curve 2
"""
# number of points in each curve, number of segments is one less, need at least one segment in each curve
N1 = x1.shape[0]
N2 = x2.shape[0]
# get segment presentation (xi, xi+1; xi+1, xi+2; ..)
xs1 = np.vstack((x1[:-1], x1[1:]))
ys1 = np.vstack((y1[:-1], y1[1:]))
xs2 = np.vstack((x2[:-1], x2[1:]))
ys2 = np.vstack((y2[:-1], y2[1:]))
# test if bounding-boxes of segments overlap
mix1 = np.tile(np.amin(xs1, axis=0), (N2-1,1))
max1 = np.tile(np.amax(xs1, axis=0), (N2-1,1))
miy1 = np.tile(np.amin(ys1, axis=0), (N2-1,1))
may1 = np.tile(np.amax(ys1, axis=0), (N2-1,1))
mix2 = np.transpose(np.tile(np.amin(xs2, axis=0), (N1-1,1)))
max2 = np.transpose(np.tile(np.amax(xs2, axis=0), (N1-1,1)))
miy2 = np.transpose(np.tile(np.amin(ys2, axis=0), (N1-1,1)))
may2 = np.transpose(np.tile(np.amax(ys2, axis=0), (N1-1,1)))
idx = np.where((mix2 <= max1) & (max2 >= mix1) & (miy2 <= may1) & (may2 >= miy1)) # overlapping segment combinations
# going through all the possible segments
x0 = []
y0 = []
for (i, j) in zip(idx[0], idx[1]):
# get segment coordinates
xa = xs1[:, j]
ya = ys1[:, j]
xb = xs2[:, i]
yb = ys2[:, i]
# ax=b, prepare matrices a and b
a = np.array([[xa[1] - xa[0], xb[0] - xb[1]], [ya[1] - ya[0], yb[0]- yb[1]]])
b = np.array([xb[0] - xa[0], yb[0] - ya[0]])
r, residuals, rank, s = np.linalg.lstsq(a, b)
# if this is not a
if rank == 2 and not residuals and r[0] >= 0 and r[0] < 1 and r[1] >= 0 and r[1] < 1:
if r[0] == 0 and r[1] == 0 and i > 0 and j > 0:
# super special case of one segment point (not the first) in common, need to differentiate between crossing or contact
angle_a1 = math.atan2(ya[1] - ya[0], xa[1] - xa[0])
angle_b1 = math.atan2(yb[1] - yb[0], xb[1] - xb[0])
# get previous segment
xa2 = xs1[:, j-1]
ya2 = ys1[:, j-1]
xb2 = xs2[:, i-1]
yb2 = ys2[:, i-1]
angle_a2 = math.atan2(ya2[0] - ya2[1], xa2[0] - xa2[1])
angle_b2 = math.atan2(yb2[0] - yb2[1], xb2[0] - xb2[1])
# determine in which order the 4 angle are
if angle_a2 < angle_a1:
h = angle_a1
angle_a1 = angle_a2
angle_a2 = h
if (angle_b1 > angle_a1 and angle_b1 < angle_a2 and (angle_b2 < angle_a1 or angle_b2 > angle_a2)) or\
((angle_b1 < angle_a1 or angle_b1 > angle_a2) and angle_b2 > angle_a1 and angle_b2 < angle_a2):
# both in or both out, just a contact point
x0.append(xa[0])
y0.append(ya[0])
else:
x0.append(xa[0] + r[0] * (xa[1] - xa[0]))
y0.append(ya[0] + r[0] * (ya[1] - ya[0]))
return (x0, y0)
# create data
def data_A():
# data from question (does not intersect)
x1 = np.arange(-10, 10, .5)
x2 = x1
y1 = [np.absolute(x**3)+100*np.absolute(x) for x in x1]
y2 = [-np.absolute(x**3)-100*np.absolute(x) for x in x2][::-1]
return (x1, y1, x2, y2)
def data_B():
# sine, cosine, should have some intersection points
x1 = np.arange(-10, 10, .5)
x2 = x1
y1 = np.sin(x1)
y2 = np.cos(x2)
return (x1, y1, x2, y2)
def data_C():
# a spiral and a diagonal line, showing the more general case
t = np.arange(0, 10, .2)
x1 = np.sin(t * 2) * t
y1 = np.cos(t * 2) * t
x2 = np.arange(-10, 10, .5)
y2 = x2
return (x1, y1, x2, y2)
def data_D():
# parallel and overlapping, should give no intersection point
x1 = np.array([0, 1])
y1 = np.array([0, 0])
x2 = np.array([-1, 3])
y2 = np.array([0, 0])
return (x1, y1, x2, y2)
def data_E():
# crossing at a segment point, should give exactly one intersection point
x1 = np.array([-1,0,1])
y1 = np.array([0,0,0])
x2 = np.array([0,0,0])
y2 = np.array([-1,0,1])
return (x1, y1, x2, y2)
def data_F():
# contacting at one segment point, should give no intersection point
x1 = np.array([-1,0,-1])
y1 = np.array([-1,0,1])
x2 = np.array([1,0,1])
y2 = np.array([-1,0,1])
return (x1, y1, x2, y2)
x1, y1, x2, y2 = data_F() # select the data you like here
# show example data
plt.plot(x1, y1, 'b-o')
plt.plot(x2, y2, 'r-o')
# call to intersection computation
x0, y0 = intersect_curves(x1, y1, x2, y2)
print('{} intersection points'.format(len(x0)))
# display intersection points in green
plt.plot(x0, y0, 'go')
plt.show() # zoom in to see that the algorithm is correct
I tested it extensively and should get most (all) border cases right (see data_A-F in code). Some examples:
Some Comments:
The assumption about the line approximation is crucial. Most true curves might only be to some extent be approximable to lines locally. Because of this places where the two curves come close but to not intersect with a distance in the order of the distance of consecutive sampling points of your curve - you may obtain false positives or false negatives. The solution is then to either use more points or to use additonal knowledge about the true curves. Splines might give a lower error rate but also require more computations, better sampling of the curves would be preferable then.
Self-intersection is trivially included when taking two times the same curve and let them intersect
This solution has the additional advantage that it isn't restricted to curves of the form y=f(x) but it's applicable to arbitrary curves in 2D.

You could use a spline interpolation for the difference function g(x) = y1(x) - y(2). Finding the minimum of the square g(x)**2 would be a contact or crossing point. Looking at the first and second derivative you could decide if it is a contact point( g(x) has minimum, g'(x)==0, g''(x) != 0) or a crossing point (g(x) is a stationary point, g'(x)==0, g''(x)==0).
The following code searches for a minimum of g(x)**2 in constrained interval and then plot the derivatives. The use of a constrained interval is to find multiple points successively by excluding intervals in which previous points were.
import matplotlib.pyplot as plt
import numpy as np
import scipy.optimize as sopt
import scipy.interpolate as sip
# test functions:
nocrossingTest = True
if nocrossingTest:
f1 = lambda x: +np.absolute(x**3)+100*np.absolute(x)
f2 = lambda x: -np.absolute(x**3)-100*np.absolute(x)
else:
f1 = lambda x: +np.absolute(x**3)+100*x
f2 = lambda x: -np.absolute(x**3)-100*x
xp = np.arange(-10,10,.5)
y1p, y2p = f1(xp), f2(xp) # test array
# Do Interpolation of y1-y2 to find crossing point:
g12 = sip.InterpolatedUnivariateSpline(xp, y1p - y2p) # Spline Interpolator of Difference
dg12 = g12.derivative() # spline derivative
ddg12 = dg12.derivative() # spline derivative
# Bounded least square fit to find minimal distance
gg = lambda x: g12(x)*g12(x)
rr = sopt.minimize_scalar(gg, bounds=[-1,1]) # search minium in Interval [-1,1]
x_c = rr['x'] # x value with minimum distance
print("Crossing point is at x = {} (Distance: {})".format(x_c, g12(x_c)))
fg = plt.figure(1)
fg.clf()
fg,ax = plt.subplots(1, 1,num=1)
ax.set_title("Function Values $y$")
ax.plot(xp, np.vstack([y1p,y2p]).T, 'x',)
xx = np.linspace(xp[0], xp[-1], 1000)
ax.plot(xx, np.vstack([f1(xx), f2(xx)]).T, '-', alpha=0.5)
ax.grid(True)
ax.legend(loc="best")
fg.canvas.draw()
fg = plt.figure(2)
fg.clf()
fg,axx = plt.subplots(3, 1,num=2)
axx[0].set_title("$g(x) = y_1(x) - y_2(x)$")
axx[1].set_title("$dg(x)/dx$")
axx[2].set_title("$d^2g(x)/dx^2$")
for ax,g in zip(axx, [g12, dg12, ddg12]):
ax.plot(xx, g(xx))
ax.plot(x_c, g(x_c), 'ro', alpha=.5)
ax.grid(True)
fg.tight_layout()
plt.show()
The difference function show that the difference is not smooth:

how to perform coordinates affine transformation using python?

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

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

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.